Martijn Grootens / Mbed 2 deprecated mbr_2017_robot_gr20

Code to let Gr20's BioRobotics2017 robot come to live.

Dependencies:   FastPWM MODSERIAL QEI mbed

main.cpp@6:cddc73091ab5, 2017-11-13 (annotated)

Committer:
megrootens
Date:
Mon Nov 13 09:34:39 2017 +0000
Revision:
6:cddc73091ab5
Parent:
2:df0c6af898ac
Child:
7:b9a209f889f5
for readability put implementation in namespace.; not tested but compiles and should still work

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 0:caa8ee3bd882 1 #include "main.h"
megrootens 0:caa8ee3bd882 2
megrootens 0:caa8ee3bd882 3 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 6:cddc73091ab5 4
megrootens 6:cddc73091ab5 5 /**
megrootens 6:cddc73091ab5 6 * UI implementation
megrootens 6:cddc73091ab5 7 */
megrootens 6:cddc73091ab5 8 namespace ui
megrootens 6:cddc73091ab5 9 {
megrootens 0:caa8ee3bd882 10
megrootens 2:df0c6af898ac 11 /**
megrootens 2:df0c6af898ac 12 * Display robot state using (flashing) LEDs
megrootens 2:df0c6af898ac 13 */
megrootens 6:cddc73091ab5 14 void StatusDisplay()
megrootens 0:caa8ee3bd882 15 {
megrootens 0:caa8ee3bd882 16 switch (robot::state) {
megrootens 0:caa8ee3bd882 17 case robot::OFF: // continuous red;
megrootens 0:caa8ee3bd882 18 rgb_led[0] = LED_ON;
megrootens 0:caa8ee3bd882 19 rgb_led[1] = !LED_ON;
megrootens 0:caa8ee3bd882 20 rgb_led[2] = !LED_ON;
megrootens 0:caa8ee3bd882 21 break;
megrootens 0:caa8ee3bd882 22 case robot::CALIBRATION: // alternating red-green; busy
megrootens 0:caa8ee3bd882 23 rgb_led[0] = !rgb_led[0];
megrootens 0:caa8ee3bd882 24 rgb_led[1] = !rgb_led[0];
megrootens 0:caa8ee3bd882 25 rgb_led[2] = !LED_ON;
megrootens 0:caa8ee3bd882 26 break;
megrootens 0:caa8ee3bd882 27 case robot::HOMING: // alternating green-blue; busy
megrootens 0:caa8ee3bd882 28 rgb_led[0] = !LED_ON;
megrootens 0:caa8ee3bd882 29 rgb_led[1] = !rgb_led[1];
megrootens 0:caa8ee3bd882 30 rgb_led[2] = !rgb_led[1];
megrootens 0:caa8ee3bd882 31 break;
megrootens 0:caa8ee3bd882 32 case robot::READY: // continuous green
megrootens 0:caa8ee3bd882 33 rgb_led[0] = !LED_ON;
megrootens 0:caa8ee3bd882 34 rgb_led[1] = LED_ON;
megrootens 0:caa8ee3bd882 35 rgb_led[2] = !LED_ON;
megrootens 0:caa8ee3bd882 36 break;
megrootens 0:caa8ee3bd882 37 case robot::DEMO: // alternating red-blue; busy
megrootens 0:caa8ee3bd882 38 rgb_led[0] = !rgb_led[0];
megrootens 0:caa8ee3bd882 39 rgb_led[1] = !LED_ON;
megrootens 0:caa8ee3bd882 40 rgb_led[2] = !rgb_led[0];
megrootens 0:caa8ee3bd882 41 break;
megrootens 0:caa8ee3bd882 42 case robot::MANUAL: // alternating green-purple
megrootens 0:caa8ee3bd882 43 rgb_led[0] = !rgb_led[0];
megrootens 0:caa8ee3bd882 44 rgb_led[1] = !rgb_led[0];
megrootens 0:caa8ee3bd882 45 rgb_led[2] = rgb_led[0];
megrootens 0:caa8ee3bd882 46 break;
megrootens 0:caa8ee3bd882 47 }
megrootens 0:caa8ee3bd882 48 }
megrootens 0:caa8ee3bd882 49
megrootens 0:caa8ee3bd882 50
megrootens 2:df0c6af898ac 51 /**
megrootens 2:df0c6af898ac 52 * Switching UI state to a new state
megrootens 2:df0c6af898ac 53 */
megrootens 6:cddc73091ab5 54 void SwitchState(State new_state)
megrootens 0:caa8ee3bd882 55 {
megrootens 6:cddc73091ab5 56 serial.printf("* Switching INPUT State to: %s\r\n",StateNames[new_state]);
megrootens 6:cddc73091ab5 57 state = new_state;
megrootens 0:caa8ee3bd882 58 }
megrootens 0:caa8ee3bd882 59
megrootens 0:caa8ee3bd882 60
megrootens 6:cddc73091ab5 61 void InterruptSwitch2()
megrootens 0:caa8ee3bd882 62 {
megrootens 0:caa8ee3bd882 63 if (robot::has_power()) {
megrootens 0:caa8ee3bd882 64 robot::Stop();
megrootens 0:caa8ee3bd882 65 } else {
megrootens 0:caa8ee3bd882 66 robot::Start();
megrootens 0:caa8ee3bd882 67 }
megrootens 0:caa8ee3bd882 68 }
megrootens 0:caa8ee3bd882 69
megrootens 6:cddc73091ab5 70 void InterruptSwitch3()
megrootens 0:caa8ee3bd882 71 {
megrootens 0:caa8ee3bd882 72 switch (state) {
megrootens 0:caa8ee3bd882 73 case STATE_SWITCHING:
megrootens 0:caa8ee3bd882 74 robot::GoToNextState();
megrootens 0:caa8ee3bd882 75 break;
megrootens 0:caa8ee3bd882 76 }
megrootens 0:caa8ee3bd882 77 }
megrootens 0:caa8ee3bd882 78
megrootens 6:cddc73091ab5 79 } // end namespace ui
megrootens 6:cddc73091ab5 80
megrootens 0:caa8ee3bd882 81
megrootens 0:caa8ee3bd882 82
megrootens 0:caa8ee3bd882 83
megrootens 0:caa8ee3bd882 84 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 6:cddc73091ab5 85
megrootens 6:cddc73091ab5 86 /**
megrootens 6:cddc73091ab5 87 * Robot implementation
megrootens 6:cddc73091ab5 88 */
megrootens 6:cddc73091ab5 89
megrootens 6:cddc73091ab5 90 namespace robot
megrootens 6:cddc73091ab5 91 {
megrootens 0:caa8ee3bd882 92
megrootens 2:df0c6af898ac 93 /**
megrootens 2:df0c6af898ac 94 * Switching ROBOT state to a new state
megrootens 2:df0c6af898ac 95 */
megrootens 6:cddc73091ab5 96 void SwitchState(State new_state)
megrootens 2:df0c6af898ac 97 {
megrootens 2:df0c6af898ac 98 ui::serial.printf("* Switching ROBOT State to: %s\r\n",StateNames[new_state]);
megrootens 2:df0c6af898ac 99 state = new_state;
megrootens 2:df0c6af898ac 100 }
megrootens 0:caa8ee3bd882 101
megrootens 2:df0c6af898ac 102 /**
megrootens 2:df0c6af898ac 103 * State flow // MANUAL state not implemented
megrootens 2:df0c6af898ac 104 */
megrootens 6:cddc73091ab5 105 void GoToNextState()
megrootens 0:caa8ee3bd882 106 {
megrootens 0:caa8ee3bd882 107 switch (state) {
megrootens 0:caa8ee3bd882 108 case OFF:
megrootens 0:caa8ee3bd882 109 SwitchState(CALIBRATION);
megrootens 0:caa8ee3bd882 110 ui::SwitchState(ui::IGNORE);
megrootens 0:caa8ee3bd882 111 Start();
megrootens 0:caa8ee3bd882 112 break;
megrootens 0:caa8ee3bd882 113 case CALIBRATION:
megrootens 0:caa8ee3bd882 114 SwitchState(HOMING);
megrootens 0:caa8ee3bd882 115 Start();
megrootens 0:caa8ee3bd882 116 break;
megrootens 0:caa8ee3bd882 117 case HOMING:
megrootens 0:caa8ee3bd882 118 SwitchState(DEMO);
megrootens 0:caa8ee3bd882 119 //ui::SwitchState(ui::STATE_SWITCHING);
megrootens 0:caa8ee3bd882 120 break;
megrootens 0:caa8ee3bd882 121 case READY:
megrootens 0:caa8ee3bd882 122 SwitchState(DEMO);
megrootens 0:caa8ee3bd882 123 break;
megrootens 0:caa8ee3bd882 124 default:
megrootens 0:caa8ee3bd882 125 SwitchState(OFF);
megrootens 6:cddc73091ab5 126 is_calibrated = false;
megrootens 0:caa8ee3bd882 127 Stop();
megrootens 0:caa8ee3bd882 128 }
megrootens 0:caa8ee3bd882 129 }
megrootens 0:caa8ee3bd882 130
megrootens 2:df0c6af898ac 131 /**
megrootens 2:df0c6af898ac 132 * Check whether any of the motors is running
megrootens 2:df0c6af898ac 133 */
megrootens 6:cddc73091ab5 134 bool has_power()
megrootens 0:caa8ee3bd882 135 {
megrootens 0:caa8ee3bd882 136 return m1.has_power() or m2.has_power();
megrootens 0:caa8ee3bd882 137 }
megrootens 0:caa8ee3bd882 138
megrootens 2:df0c6af898ac 139 /**
megrootens 2:df0c6af898ac 140 * Get motor angle in [deg] for motor i=1,2
megrootens 2:df0c6af898ac 141 */
megrootens 6:cddc73091ab5 142 double get_angle(int i)
megrootens 0:caa8ee3bd882 143 {
megrootens 0:caa8ee3bd882 144 switch(i) {
megrootens 0:caa8ee3bd882 145 case 1:
megrootens 0:caa8ee3bd882 146 return m1.get_angle();
megrootens 0:caa8ee3bd882 147 case 2:
megrootens 0:caa8ee3bd882 148 return m2.get_angle();
megrootens 0:caa8ee3bd882 149 default:
megrootens 0:caa8ee3bd882 150 return NULL;
megrootens 0:caa8ee3bd882 151 }
megrootens 0:caa8ee3bd882 152 }
megrootens 0:caa8ee3bd882 153
megrootens 2:df0c6af898ac 154 /**
megrootens 2:df0c6af898ac 155 * Compute end-effector x-coordinate [m] for given motor angles [deg].
megrootens 2:df0c6af898ac 156 */
megrootens 6:cddc73091ab5 157 double ForwardKinematicsX(double theta_1, double theta_2)
megrootens 0:caa8ee3bd882 158 {
megrootens 0:caa8ee3bd882 159 return kL1*cos(deg2rad(theta_1)) + kL2*cos(deg2rad(theta_2));
megrootens 0:caa8ee3bd882 160 }
megrootens 0:caa8ee3bd882 161
megrootens 2:df0c6af898ac 162 /**
megrootens 2:df0c6af898ac 163 * Compute end-effector y-coordinate [m] for given motor angles [deg].
megrootens 2:df0c6af898ac 164 */
megrootens 6:cddc73091ab5 165 double ForwardKinematicsY(double theta_1, double theta_2)
megrootens 0:caa8ee3bd882 166 {
megrootens 0:caa8ee3bd882 167 return kL1*sin(deg2rad(theta_1)) + kL2*sin(deg2rad(theta_2));
megrootens 0:caa8ee3bd882 168 }
megrootens 0:caa8ee3bd882 169
megrootens 2:df0c6af898ac 170 /**
megrootens 2:df0c6af898ac 171 * Compute end-effector x-coordinate [m] for current motor angles.
megrootens 2:df0c6af898ac 172 */
megrootens 6:cddc73091ab5 173 double get_x()
megrootens 0:caa8ee3bd882 174 {
megrootens 0:caa8ee3bd882 175 return ForwardKinematicsX(get_angle(1),get_angle(2));
megrootens 0:caa8ee3bd882 176 }
megrootens 0:caa8ee3bd882 177
megrootens 2:df0c6af898ac 178 /**
megrootens 2:df0c6af898ac 179 * Compute end-effector y-coordinate [m] for current motor angles.
megrootens 2:df0c6af898ac 180 */
megrootens 6:cddc73091ab5 181 double get_y()
megrootens 0:caa8ee3bd882 182 {
megrootens 0:caa8ee3bd882 183 return ForwardKinematicsY(get_angle(1),get_angle(2));
megrootens 0:caa8ee3bd882 184 }
megrootens 0:caa8ee3bd882 185
megrootens 2:df0c6af898ac 186 /**
megrootens 2:df0c6af898ac 187 * Compute motor angle [deg] for motor i=1,2 for given end-effector x and y
megrootens 2:df0c6af898ac 188 * coordinate [m]
megrootens 2:df0c6af898ac 189 */
megrootens 6:cddc73091ab5 190 double InverseKinematicsTheta(double x, double y, int i)
megrootens 0:caa8ee3bd882 191 {
megrootens 2:df0c6af898ac 192 // vector to end point
megrootens 0:caa8ee3bd882 193 double r = sqrt(pow(x,2) + pow(y,2));
megrootens 0:caa8ee3bd882 194 double dir_angle = rad2deg( atan2(y,x) );
megrootens 2:df0c6af898ac 195
megrootens 2:df0c6af898ac 196 // law of cosines for internal angles
megrootens 2:df0c6af898ac 197 double int_angle_1 =
megrootens 2:df0c6af898ac 198 rad2deg( acos((pow(kL1,2) + pow(r,2) - pow(kL2,2))/(2*kL1*r)) );
megrootens 2:df0c6af898ac 199 double int_angle_2 =
megrootens 2:df0c6af898ac 200 rad2deg( acos((pow(kL2,2) + pow(r,2) - pow(kL1,2))/(2*kL2*r)) );
megrootens 2:df0c6af898ac 201
megrootens 2:df0c6af898ac 202 // absolute motor angles w.r.t x,y frame
megrootens 0:caa8ee3bd882 203 double theta_1 = dir_angle + int_angle_1;
megrootens 0:caa8ee3bd882 204 double theta_2 = dir_angle - int_angle_2;
megrootens 2:df0c6af898ac 205
megrootens 0:caa8ee3bd882 206 switch (i) {
megrootens 0:caa8ee3bd882 207 case 1: return theta_1;
megrootens 0:caa8ee3bd882 208 case 2: return theta_2;
megrootens 0:caa8ee3bd882 209 default: return NULL;
megrootens 0:caa8ee3bd882 210 }
megrootens 0:caa8ee3bd882 211 }
megrootens 0:caa8ee3bd882 212
megrootens 2:df0c6af898ac 213 /**
megrootens 2:df0c6af898ac 214 * Start robot:
megrootens 2:df0c6af898ac 215 * - set reference position to current position s.t. no 'explosion'
megrootens 2:df0c6af898ac 216 * - start motors
megrootens 2:df0c6af898ac 217 * - reset controller
megrootens 2:df0c6af898ac 218 */
megrootens 6:cddc73091ab5 219 void Start()
megrootens 0:caa8ee3bd882 220 {
megrootens 2:df0c6af898ac 221 ui::serial.printf("! Starting robot.\r\n");
megrootens 2:df0c6af898ac 222 ui::serial.printf(" - Setting current position as reference.\r\n");
megrootens 0:caa8ee3bd882 223 ref::SetPositionAsReference();
megrootens 0:caa8ee3bd882 224
megrootens 2:df0c6af898ac 225 ui::serial.printf(" - Resetting controllers.\r\n");
megrootens 0:caa8ee3bd882 226 c1.Reset();
megrootens 0:caa8ee3bd882 227 c2.Reset();
megrootens 0:caa8ee3bd882 228
megrootens 2:df0c6af898ac 229 ui::serial.printf(" - Starting motors.\r\n");
megrootens 0:caa8ee3bd882 230 m1.Start();
megrootens 0:caa8ee3bd882 231 m2.Start();
megrootens 0:caa8ee3bd882 232 }
megrootens 0:caa8ee3bd882 233
megrootens 6:cddc73091ab5 234 void Stop()
megrootens 0:caa8ee3bd882 235 {
megrootens 2:df0c6af898ac 236 ui::serial.printf("! Stopping robot.\r\n");
megrootens 2:df0c6af898ac 237 ui::serial.printf(" - Stopping motors.\r\n");
megrootens 0:caa8ee3bd882 238 m1.Stop();
megrootens 0:caa8ee3bd882 239 m2.Stop();
megrootens 0:caa8ee3bd882 240 }
megrootens 0:caa8ee3bd882 241
megrootens 2:df0c6af898ac 242 /**
megrootens 2:df0c6af898ac 243 * Control loop; negative feedback control on error with reference postion
megrootens 2:df0c6af898ac 244 */
megrootens 6:cddc73091ab5 245 void ControlLoop()
megrootens 0:caa8ee3bd882 246 {
megrootens 0:caa8ee3bd882 247 m1.set_pwm(c1.Update( ref::theta_1 - get_angle(1) ));
megrootens 0:caa8ee3bd882 248 m2.set_pwm(c2.Update( ref::theta_2 - get_angle(2) ));
megrootens 0:caa8ee3bd882 249 }
megrootens 0:caa8ee3bd882 250
megrootens 2:df0c6af898ac 251 /**
megrootens 2:df0c6af898ac 252 * Set the motor angles to the values corresponding to the end-stops.
megrootens 2:df0c6af898ac 253 * Only call this method when the robot is at its end-stops!
megrootens 2:df0c6af898ac 254 */
megrootens 6:cddc73091ab5 255 void SetCalibrationAngles()
megrootens 0:caa8ee3bd882 256 {
megrootens 0:caa8ee3bd882 257 m1.set_angle(kCalibAngleMotor1);
megrootens 0:caa8ee3bd882 258 m2.set_angle(kCalibAngleMotor2);
megrootens 0:caa8ee3bd882 259 is_calibrated = true;
megrootens 0:caa8ee3bd882 260 }
megrootens 0:caa8ee3bd882 261
megrootens 6:cddc73091ab5 262 } // end namespace robot
megrootens 6:cddc73091ab5 263
megrootens 6:cddc73091ab5 264
megrootens 6:cddc73091ab5 265
megrootens 0:caa8ee3bd882 266 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 6:cddc73091ab5 267
megrootens 6:cddc73091ab5 268 /**
megrootens 6:cddc73091ab5 269 * Reference implementation
megrootens 6:cddc73091ab5 270 */
megrootens 6:cddc73091ab5 271 namespace ref
megrootens 6:cddc73091ab5 272 {
megrootens 0:caa8ee3bd882 273
megrootens 2:df0c6af898ac 274 /**
megrootens 2:df0c6af898ac 275 * Get reference x-coordinate using reference angles and forward kinematics
megrootens 2:df0c6af898ac 276 */
megrootens 6:cddc73091ab5 277 double get_ref_x()
megrootens 0:caa8ee3bd882 278 {
megrootens 0:caa8ee3bd882 279 return robot::ForwardKinematicsX(theta_1,theta_2);
megrootens 0:caa8ee3bd882 280 }
megrootens 0:caa8ee3bd882 281
megrootens 2:df0c6af898ac 282 /**
megrootens 2:df0c6af898ac 283 * Get reference x-coordinate using reference angles and forward kinematics
megrootens 2:df0c6af898ac 284 */
megrootens 6:cddc73091ab5 285 double get_ref_y()
megrootens 0:caa8ee3bd882 286 {
megrootens 0:caa8ee3bd882 287 return robot::ForwardKinematicsY(theta_1,theta_2);
megrootens 0:caa8ee3bd882 288 }
megrootens 0:caa8ee3bd882 289
megrootens 2:df0c6af898ac 290 /**
megrootens 2:df0c6af898ac 291 * Update reference signal depending on robot state
megrootens 2:df0c6af898ac 292 */
megrootens 6:cddc73091ab5 293 void UpdateReference()
megrootens 0:caa8ee3bd882 294 {
megrootens 0:caa8ee3bd882 295 switch(robot::state) {
megrootens 0:caa8ee3bd882 296 case robot::CALIBRATION:
megrootens 0:caa8ee3bd882 297 CalibrationReference();
megrootens 0:caa8ee3bd882 298 break;
megrootens 0:caa8ee3bd882 299 case robot::HOMING:
megrootens 0:caa8ee3bd882 300 HomingReference();
megrootens 0:caa8ee3bd882 301 break;
megrootens 0:caa8ee3bd882 302 case robot::DEMO:
megrootens 0:caa8ee3bd882 303 DemoReference();
megrootens 0:caa8ee3bd882 304 break;
megrootens 0:caa8ee3bd882 305 }
megrootens 0:caa8ee3bd882 306 }
megrootens 0:caa8ee3bd882 307
megrootens 2:df0c6af898ac 308 /**
megrootens 2:df0c6af898ac 309 * Generate reference profile to bring robotto calibration position;
megrootens 2:df0c6af898ac 310 * at start-up the motor angles are '0' (relative encoders)
megrootens 2:df0c6af898ac 311 */
megrootens 6:cddc73091ab5 312 void CalibrationReference()
megrootens 0:caa8ee3bd882 313 {
megrootens 2:df0c6af898ac 314 // Check when motors can no longer move farther
megrootens 0:caa8ee3bd882 315 bool is_ready_1 = (theta_1 - robot::get_angle(1) > kCalibrationError);
megrootens 0:caa8ee3bd882 316 bool is_ready_2 = (theta_2 - robot::get_angle(2) > kCalibrationError);
megrootens 0:caa8ee3bd882 317
megrootens 2:df0c6af898ac 318 // Turn off motor if end-stop reached
megrootens 2:df0c6af898ac 319 // Motor 2 _should_ arrive first
megrootens 0:caa8ee3bd882 320 if (is_ready_1) {
megrootens 0:caa8ee3bd882 321 robot::m1.Stop();
megrootens 0:caa8ee3bd882 322 } else {
megrootens 0:caa8ee3bd882 323 theta_1 += kCalibrationOmegaStep;
megrootens 0:caa8ee3bd882 324 }
megrootens 0:caa8ee3bd882 325
megrootens 2:df0c6af898ac 326 // Turn off motor if end-stop reached
megrootens 0:caa8ee3bd882 327 if (is_ready_2) {
megrootens 0:caa8ee3bd882 328 robot::m2.Stop();
megrootens 0:caa8ee3bd882 329 } else {
megrootens 0:caa8ee3bd882 330 theta_2 += kCalibrationOmegaStep;
megrootens 0:caa8ee3bd882 331 }
megrootens 0:caa8ee3bd882 332
megrootens 0:caa8ee3bd882 333 if (is_ready_1 & is_ready_2) {
megrootens 2:df0c6af898ac 334 ui::serial.printf(" - Both motors at end-stop.\r\n");
megrootens 0:caa8ee3bd882 335 robot::Stop();
megrootens 2:df0c6af898ac 336 // Set the calibration angles as current position
megrootens 0:caa8ee3bd882 337 robot::SetCalibrationAngles();
megrootens 0:caa8ee3bd882 338 robot::GoToNextState();
megrootens 0:caa8ee3bd882 339 }
megrootens 0:caa8ee3bd882 340 }
megrootens 0:caa8ee3bd882 341
megrootens 2:df0c6af898ac 342 /**
megrootens 2:df0c6af898ac 343 * Bring robot to home position: STARTING FROM CALIBRATION POSITION!!
megrootens 2:df0c6af898ac 344 */
megrootens 6:cddc73091ab5 345 void HomingReference()
megrootens 0:caa8ee3bd882 346 {
megrootens 2:df0c6af898ac 347 // First move motor 1
megrootens 0:caa8ee3bd882 348 double dist_1 = kOriginTheta1 - theta_1;
megrootens 0:caa8ee3bd882 349
megrootens 0:caa8ee3bd882 350 if (abs(dist_1)<=kCalibrationOmegaStep) {
megrootens 2:df0c6af898ac 351 // Motor 1 ready
megrootens 0:caa8ee3bd882 352 theta_1 = kOriginTheta1;
megrootens 2:df0c6af898ac 353
megrootens 2:df0c6af898ac 354 // Only move motor 2 after motor 1 at home
megrootens 0:caa8ee3bd882 355 double dist_2 = kOriginTheta2 - theta_2;
megrootens 0:caa8ee3bd882 356
megrootens 0:caa8ee3bd882 357 if (abs(dist_2)<=kCalibrationOmegaStep) {
megrootens 0:caa8ee3bd882 358 // motor 2 also ready
megrootens 0:caa8ee3bd882 359 theta_2 = kOriginTheta2;
megrootens 2:df0c6af898ac 360
megrootens 2:df0c6af898ac 361 ui::serial.printf(" - Robot at home position.\r\n");
megrootens 0:caa8ee3bd882 362 robot::GoToNextState();
megrootens 0:caa8ee3bd882 363 } else {
megrootens 2:df0c6af898ac 364 // Motor 2 still to move
megrootens 0:caa8ee3bd882 365 theta_2 += dist_2 / abs(dist_2) * kCalibrationOmegaStep;
megrootens 0:caa8ee3bd882 366 }
megrootens 0:caa8ee3bd882 367
megrootens 0:caa8ee3bd882 368
megrootens 0:caa8ee3bd882 369 } else {
megrootens 2:df0c6af898ac 370 // Motor 1 still to move
megrootens 0:caa8ee3bd882 371 theta_1 += dist_1 / abs(dist_1) * kCalibrationOmegaStep;
megrootens 0:caa8ee3bd882 372 }
megrootens 0:caa8ee3bd882 373 }
megrootens 0:caa8ee3bd882 374
megrootens 2:df0c6af898ac 375 /**
megrootens 2:df0c6af898ac 376 * Cycle through demo coordinates to display straight lines
megrootens 2:df0c6af898ac 377 */
megrootens 6:cddc73091ab5 378 void DemoReference()
megrootens 0:caa8ee3bd882 379 {
megrootens 2:df0c6af898ac 380 // Compare current setpoint and goal coord and slowly move linearly in the
megrootens 2:df0c6af898ac 381 // right direction (straight line 2D)
megrootens 2:df0c6af898ac 382
megrootens 0:caa8ee3bd882 383 double x = robot::ForwardKinematicsX(theta_1,theta_2);
megrootens 0:caa8ee3bd882 384 double y = robot::ForwardKinematicsY(theta_1,theta_2);
megrootens 2:df0c6af898ac 385
megrootens 0:caa8ee3bd882 386 double x_goal = kDemoCoords[0][i_demo_coord];
megrootens 0:caa8ee3bd882 387 double y_goal = kDemoCoords[1][i_demo_coord];
megrootens 2:df0c6af898ac 388
megrootens 0:caa8ee3bd882 389 double delta_x = x_goal - x;
megrootens 0:caa8ee3bd882 390 double delta_y = y_goal - y;
megrootens 2:df0c6af898ac 391
megrootens 0:caa8ee3bd882 392 double dist = sqrt(pow(delta_x,2) + pow(delta_y,2));
megrootens 2:df0c6af898ac 393
megrootens 2:df0c6af898ac 394 // Compute next setpoint (x,y)
megrootens 0:caa8ee3bd882 395 double x_new = x;
megrootens 0:caa8ee3bd882 396 double y_new = y;
megrootens 2:df0c6af898ac 397
megrootens 0:caa8ee3bd882 398 if (dist<kMaxStep) {
megrootens 0:caa8ee3bd882 399 x_new = x_goal;
megrootens 0:caa8ee3bd882 400 y_new = y_goal;
megrootens 2:df0c6af898ac 401
megrootens 2:df0c6af898ac 402 ui::serial.printf(" - Reached Demo Coord %d\r\n",i_demo_coord);
megrootens 0:caa8ee3bd882 403 i_demo_coord = ++i_demo_coord % kNumDemoCoords;
megrootens 2:df0c6af898ac 404
megrootens 0:caa8ee3bd882 405 if (i_demo_coord == 0) {
megrootens 0:caa8ee3bd882 406 robot::is_calibrated = false;
megrootens 0:caa8ee3bd882 407 robot::SwitchState(robot::CALIBRATION);
megrootens 0:caa8ee3bd882 408 }
megrootens 0:caa8ee3bd882 409 } else {
megrootens 0:caa8ee3bd882 410 x_new += delta_x / dist * kMaxStep;
megrootens 0:caa8ee3bd882 411 y_new += delta_y / dist * kMaxStep;
megrootens 0:caa8ee3bd882 412 }
megrootens 2:df0c6af898ac 413
megrootens 2:df0c6af898ac 414 // Compute motor angles
megrootens 0:caa8ee3bd882 415 theta_1 = robot::InverseKinematicsTheta(x_new, y_new, 1);
megrootens 0:caa8ee3bd882 416 theta_2 = robot::InverseKinematicsTheta(x_new, y_new, 2);
megrootens 0:caa8ee3bd882 417 }
megrootens 0:caa8ee3bd882 418
megrootens 2:df0c6af898ac 419 /**
megrootens 2:df0c6af898ac 420 * Set current robot position as reference position.
megrootens 2:df0c6af898ac 421 */
megrootens 6:cddc73091ab5 422 void SetPositionAsReference()
megrootens 0:caa8ee3bd882 423 {
megrootens 6:cddc73091ab5 424 theta_1 = robot::get_angle(1);
megrootens 6:cddc73091ab5 425 theta_2 = robot::get_angle(2);
megrootens 2:df0c6af898ac 426 }
megrootens 6:cddc73091ab5 427
megrootens 6:cddc73091ab5 428 } // end namespace ref