Code to let Gr20's BioRobotics2017 robot come to live.
Dependencies: FastPWM MODSERIAL QEI mbed
main.cpp
- Committer:
- megrootens
- Date:
- 2017-11-13
- Revision:
- 6:cddc73091ab5
- Parent:
- 2:df0c6af898ac
- Child:
- 7:b9a209f889f5
File content as of revision 6:cddc73091ab5:
#include "main.h" // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = /** * UI implementation */ namespace ui { /** * Display robot state using (flashing) LEDs */ void StatusDisplay() { switch (robot::state) { case robot::OFF: // continuous red; rgb_led[0] = LED_ON; rgb_led[1] = !LED_ON; rgb_led[2] = !LED_ON; break; case robot::CALIBRATION: // alternating red-green; busy rgb_led[0] = !rgb_led[0]; rgb_led[1] = !rgb_led[0]; rgb_led[2] = !LED_ON; break; case robot::HOMING: // alternating green-blue; busy rgb_led[0] = !LED_ON; rgb_led[1] = !rgb_led[1]; rgb_led[2] = !rgb_led[1]; break; case robot::READY: // continuous green rgb_led[0] = !LED_ON; rgb_led[1] = LED_ON; rgb_led[2] = !LED_ON; break; case robot::DEMO: // alternating red-blue; busy rgb_led[0] = !rgb_led[0]; rgb_led[1] = !LED_ON; rgb_led[2] = !rgb_led[0]; break; case robot::MANUAL: // alternating green-purple rgb_led[0] = !rgb_led[0]; rgb_led[1] = !rgb_led[0]; rgb_led[2] = rgb_led[0]; break; } } /** * Switching UI state to a new state */ void SwitchState(State new_state) { serial.printf("* Switching INPUT State to: %s\r\n",StateNames[new_state]); state = new_state; } void InterruptSwitch2() { if (robot::has_power()) { robot::Stop(); } else { robot::Start(); } } void InterruptSwitch3() { switch (state) { case STATE_SWITCHING: robot::GoToNextState(); break; } } } // end namespace ui // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = /** * Robot implementation */ namespace robot { /** * Switching ROBOT state to a new state */ void SwitchState(State new_state) { ui::serial.printf("* Switching ROBOT State to: %s\r\n",StateNames[new_state]); state = new_state; } /** * State flow // MANUAL state not implemented */ void GoToNextState() { switch (state) { case OFF: SwitchState(CALIBRATION); ui::SwitchState(ui::IGNORE); Start(); break; case CALIBRATION: SwitchState(HOMING); Start(); break; case HOMING: SwitchState(DEMO); //ui::SwitchState(ui::STATE_SWITCHING); break; case READY: SwitchState(DEMO); break; default: SwitchState(OFF); is_calibrated = false; Stop(); } } /** * Check whether any of the motors is running */ bool has_power() { return m1.has_power() or m2.has_power(); } /** * Get motor angle in [deg] for motor i=1,2 */ double get_angle(int i) { switch(i) { case 1: return m1.get_angle(); case 2: return m2.get_angle(); default: return NULL; } } /** * Compute end-effector x-coordinate [m] for given motor angles [deg]. */ double ForwardKinematicsX(double theta_1, double theta_2) { return kL1*cos(deg2rad(theta_1)) + kL2*cos(deg2rad(theta_2)); } /** * Compute end-effector y-coordinate [m] for given motor angles [deg]. */ double ForwardKinematicsY(double theta_1, double theta_2) { return kL1*sin(deg2rad(theta_1)) + kL2*sin(deg2rad(theta_2)); } /** * Compute end-effector x-coordinate [m] for current motor angles. */ double get_x() { return ForwardKinematicsX(get_angle(1),get_angle(2)); } /** * Compute end-effector y-coordinate [m] for current motor angles. */ double get_y() { return ForwardKinematicsY(get_angle(1),get_angle(2)); } /** * Compute motor angle [deg] for motor i=1,2 for given end-effector x and y * coordinate [m] */ double InverseKinematicsTheta(double x, double y, int i) { // vector to end point double r = sqrt(pow(x,2) + pow(y,2)); double dir_angle = rad2deg( atan2(y,x) ); // law of cosines for internal angles double int_angle_1 = rad2deg( acos((pow(kL1,2) + pow(r,2) - pow(kL2,2))/(2*kL1*r)) ); double int_angle_2 = rad2deg( acos((pow(kL2,2) + pow(r,2) - pow(kL1,2))/(2*kL2*r)) ); // absolute motor angles w.r.t x,y frame double theta_1 = dir_angle + int_angle_1; double theta_2 = dir_angle - int_angle_2; switch (i) { case 1: return theta_1; case 2: return theta_2; default: return NULL; } } /** * Start robot: * - set reference position to current position s.t. no 'explosion' * - start motors * - reset controller */ void Start() { ui::serial.printf("! Starting robot.\r\n"); ui::serial.printf(" - Setting current position as reference.\r\n"); ref::SetPositionAsReference(); ui::serial.printf(" - Resetting controllers.\r\n"); c1.Reset(); c2.Reset(); ui::serial.printf(" - Starting motors.\r\n"); m1.Start(); m2.Start(); } void Stop() { ui::serial.printf("! Stopping robot.\r\n"); ui::serial.printf(" - Stopping motors.\r\n"); m1.Stop(); m2.Stop(); } /** * Control loop; negative feedback control on error with reference postion */ void ControlLoop() { m1.set_pwm(c1.Update( ref::theta_1 - get_angle(1) )); m2.set_pwm(c2.Update( ref::theta_2 - get_angle(2) )); } /** * Set the motor angles to the values corresponding to the end-stops. * Only call this method when the robot is at its end-stops! */ void SetCalibrationAngles() { m1.set_angle(kCalibAngleMotor1); m2.set_angle(kCalibAngleMotor2); is_calibrated = true; } } // end namespace robot // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = /** * Reference implementation */ namespace ref { /** * Get reference x-coordinate using reference angles and forward kinematics */ double get_ref_x() { return robot::ForwardKinematicsX(theta_1,theta_2); } /** * Get reference x-coordinate using reference angles and forward kinematics */ double get_ref_y() { return robot::ForwardKinematicsY(theta_1,theta_2); } /** * Update reference signal depending on robot state */ void UpdateReference() { switch(robot::state) { case robot::CALIBRATION: CalibrationReference(); break; case robot::HOMING: HomingReference(); break; case robot::DEMO: DemoReference(); break; } } /** * Generate reference profile to bring robotto calibration position; * at start-up the motor angles are '0' (relative encoders) */ void CalibrationReference() { // Check when motors can no longer move farther bool is_ready_1 = (theta_1 - robot::get_angle(1) > kCalibrationError); bool is_ready_2 = (theta_2 - robot::get_angle(2) > kCalibrationError); // Turn off motor if end-stop reached // Motor 2 _should_ arrive first if (is_ready_1) { robot::m1.Stop(); } else { theta_1 += kCalibrationOmegaStep; } // Turn off motor if end-stop reached if (is_ready_2) { robot::m2.Stop(); } else { theta_2 += kCalibrationOmegaStep; } if (is_ready_1 & is_ready_2) { ui::serial.printf(" - Both motors at end-stop.\r\n"); robot::Stop(); // Set the calibration angles as current position robot::SetCalibrationAngles(); robot::GoToNextState(); } } /** * Bring robot to home position: STARTING FROM CALIBRATION POSITION!! */ void HomingReference() { // First move motor 1 double dist_1 = kOriginTheta1 - theta_1; if (abs(dist_1)<=kCalibrationOmegaStep) { // Motor 1 ready theta_1 = kOriginTheta1; // Only move motor 2 after motor 1 at home double dist_2 = kOriginTheta2 - theta_2; if (abs(dist_2)<=kCalibrationOmegaStep) { // motor 2 also ready theta_2 = kOriginTheta2; ui::serial.printf(" - Robot at home position.\r\n"); robot::GoToNextState(); } else { // Motor 2 still to move theta_2 += dist_2 / abs(dist_2) * kCalibrationOmegaStep; } } else { // Motor 1 still to move theta_1 += dist_1 / abs(dist_1) * kCalibrationOmegaStep; } } /** * Cycle through demo coordinates to display straight lines */ void DemoReference() { // Compare current setpoint and goal coord and slowly move linearly in the // right direction (straight line 2D) double x = robot::ForwardKinematicsX(theta_1,theta_2); double y = robot::ForwardKinematicsY(theta_1,theta_2); double x_goal = kDemoCoords[0][i_demo_coord]; double y_goal = kDemoCoords[1][i_demo_coord]; double delta_x = x_goal - x; double delta_y = y_goal - y; double dist = sqrt(pow(delta_x,2) + pow(delta_y,2)); // Compute next setpoint (x,y) double x_new = x; double y_new = y; if (dist<kMaxStep) { x_new = x_goal; y_new = y_goal; ui::serial.printf(" - Reached Demo Coord %d\r\n",i_demo_coord); i_demo_coord = ++i_demo_coord % kNumDemoCoords; if (i_demo_coord == 0) { robot::is_calibrated = false; robot::SwitchState(robot::CALIBRATION); } } else { x_new += delta_x / dist * kMaxStep; y_new += delta_y / dist * kMaxStep; } // Compute motor angles theta_1 = robot::InverseKinematicsTheta(x_new, y_new, 1); theta_2 = robot::InverseKinematicsTheta(x_new, y_new, 2); } /** * Set current robot position as reference position. */ void SetPositionAsReference() { theta_1 = robot::get_angle(1); theta_2 = robot::get_angle(2); } } // end namespace ref