62 lines
2.3 KiB
Python
62 lines
2.3 KiB
Python
import numpy as np
|
|
|
|
# Kinematics matrix - modify based on your robot type
|
|
J = ...
|
|
|
|
def controller(t, X_I, dX_I, target_pos):
|
|
"""
|
|
Real robot controller function template.
|
|
|
|
This function is called by runner.py at each control timestep.
|
|
|
|
Args:
|
|
t (float): Current time in seconds since control started
|
|
X_I (numpy.ndarray): Current robot state [x, y, theta] as 3x1 column vector
|
|
- x: position in x-direction (meters)
|
|
- y: position in y-direction (meters)
|
|
- theta: orientation angle (radians)
|
|
dX_I (numpy.ndarray): Current robot velocities [dx, dy, dtheta] as 3x1 column vector
|
|
- dx: velocity in x-direction (m/s)
|
|
- dy: velocity in y-direction (m/s)
|
|
- dtheta: angular velocity (rad/s)
|
|
target_pos (numpy.ndarray): Target position [x, y, theta] as 3x1 column vector
|
|
- x: target x-position (meters)
|
|
- y: target y-position (meters)
|
|
- theta: target orientation (radians)
|
|
|
|
Returns:
|
|
U (numpy.ndarray): Control output - wheel speeds in rad/s
|
|
Shape depends on robot type:
|
|
- 2x1 for differential/tank drive
|
|
- 3x1 for 3-wheel omnidirectional
|
|
- 4x1 for mecanum/4-wheel omnidirectional
|
|
"""
|
|
|
|
dX_R_des = ...
|
|
|
|
# Convert desired robot velocities to wheel speeds using kinematics
|
|
U = J @ dX_R_des
|
|
|
|
# Velocity limits
|
|
max_wheel_speed = 5.236 # rad/s = 50 rpm.
|
|
U = np.clip(U, -max_wheel_speed, max_wheel_speed)
|
|
|
|
return U
|
|
|
|
# Optional: Add any helper functions you might need
|
|
def normalize_angle(angle):
|
|
"""Normalize angle to [-pi, pi] range"""
|
|
while angle > np.pi:
|
|
angle -= 2*np.pi
|
|
while angle < -np.pi:
|
|
angle += 2*np.pi
|
|
return angle
|
|
|
|
def distance_to_point(x, y, target_x, target_y):
|
|
"""Calculate Euclidean distance to a target point"""
|
|
return np.sqrt((target_x - x)**2 + (target_y - y)**2)
|
|
|
|
def angle_to_point(x, y, target_x, target_y):
|
|
"""Calculate angle to a target point"""
|
|
return np.arctan2(target_y - y, target_x - x)
|