diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c18dd8d --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/real_controller.py b/real_controller.py new file mode 100644 index 0000000..3041c45 --- /dev/null +++ b/real_controller.py @@ -0,0 +1,61 @@ +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)