Code to let Gr20's BioRobotics2017 robot come to live.
Dependencies: FastPWM MODSERIAL QEI mbed
Diff: main.cpp
- Revision:
- 2:df0c6af898ac
- Parent:
- 0:caa8ee3bd882
- Child:
- 6:cddc73091ab5
diff -r ff11ee1c6baa -r df0c6af898ac main.cpp --- a/main.cpp Sun Nov 12 12:05:22 2017 +0000 +++ b/main.cpp Sun Nov 12 14:06:23 2017 +0000 @@ -4,6 +4,9 @@ // UI implementation // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = +/** + * Display robot state using (flashing) LEDs + */ void ui::StatusDisplay() { switch (robot::state) { @@ -41,18 +44,15 @@ } - +/** + * Switching UI state to a new state + */ void ui::SwitchState(State new_state) { - ui::serial.printf("* Switching Input State to: %s\r\n",StateNames[new_state]); + ui::serial.printf("* Switching INPUT State to: %s\r\n",StateNames[new_state]); ui::state = new_state; } -void robot::SwitchState(State new_state) -{ - ui::serial.printf("* Switching Robot State to: %s\r\n",StateNames[new_state]); - state = new_state; -} void ui::InterruptSwitch2() { @@ -79,7 +79,18 @@ // Robot implementation // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = +/** + * Switching ROBOT state to a new state + */ +void robot::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 robot::GoToNextState() { switch (state) { @@ -99,9 +110,6 @@ case READY: SwitchState(DEMO); break; - case DEMO: - SwitchState(MANUAL); - break; default: SwitchState(OFF); robot::is_calibrated = false; @@ -109,11 +117,17 @@ } } +/** + * Check whether any of the motors is running + */ bool robot::has_power() { return m1.has_power() or m2.has_power(); } +/** + * Get motor angle in [deg] for motor i=1,2 + */ double robot::get_angle(int i) { switch(i) { @@ -126,37 +140,58 @@ } } +/** + * Compute end-effector x-coordinate [m] for given motor angles [deg]. + */ double robot::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 robot::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 robot::get_x() { return ForwardKinematicsX(get_angle(1),get_angle(2)); } +/** + * Compute end-effector y-coordinate [m] for current motor angles. + */ double robot::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 robot::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) ); - - 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)) ); - + + // 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; @@ -164,33 +199,48 @@ } } +/** + * Start robot: + * - set reference position to current position s.t. no 'explosion' + * - start motors + * - reset controller + */ void robot::Start() { - ui::serial.printf(" Setting current position as reference.\r\n"); + 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"); + ui::serial.printf(" - Resetting controllers.\r\n"); c1.Reset(); c2.Reset(); - ui::serial.printf(" Starting motors.\r\n"); + ui::serial.printf(" - Starting motors.\r\n"); m1.Start(); m2.Start(); } void robot::Stop() { - ui::serial.printf(" Stopping motors.\r\n"); + 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 robot::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 robot::SetCalibrationAngles() { m1.set_angle(kCalibAngleMotor1); @@ -202,16 +252,25 @@ // Reference implementation // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = +/** + * Get reference x-coordinate using reference angles and forward kinematics + */ double ref::get_ref_x() { return robot::ForwardKinematicsX(theta_1,theta_2); } +/** + * Get reference x-coordinate using reference angles and forward kinematics + */ double ref::get_ref_y() { return robot::ForwardKinematicsY(theta_1,theta_2); } +/** + * Update reference signal depending on robot state + */ void ref::UpdateReference() { switch(robot::state) { @@ -227,18 +286,25 @@ } } +/** + * Generate reference profile to bring robotto calibration position; + * at start-up the motor angles are '0' (relative encoders) + */ void ref::CalibrationReference() { - // Calibration process + // 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 { @@ -246,66 +312,77 @@ } if (is_ready_1 & is_ready_2) { + ui::serial.printf(" - Both motors at end-stop.\r\n"); robot::Stop(); - ui::serial.printf("+ Both motors at end-stop.\r\n"); + // Set the calibration angles as current position robot::SetCalibrationAngles(); robot::GoToNextState(); } } +/** + * Bring robot to home position: STARTING FROM CALIBRATION POSITION!! + */ void ref::HomingReference() { + // First move motor 1 double dist_1 = kOriginTheta1 - theta_1; if (abs(dist_1)<=kCalibrationOmegaStep) { - // motor 1 ready + // 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"); + + ui::serial.printf(" - Robot at home position.\r\n"); robot::GoToNextState(); } else { - // motor 2 still to move + // Motor 2 still to move theta_2 += dist_2 / abs(dist_2) * kCalibrationOmegaStep; } } else { - // motor 1 still to move + // Motor 1 still to move theta_1 += dist_1 / abs(dist_1) * kCalibrationOmegaStep; } - - } +/** + * Cycle through demo coordinates to display straight lines + */ void ref::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); + + 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); @@ -314,15 +391,17 @@ 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 ref::SetPositionAsReference() { ref::theta_1 = robot::get_angle(1); ref::theta_2 = robot::get_angle(2); -} \ No newline at end of file +}