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@2:df0c6af898ac, 2017-11-12 (annotated)

Committer:
megrootens
Date:
Sun Nov 12 14:06:23 2017 +0000
Revision:
2:df0c6af898ac
Parent:
0:caa8ee3bd882
Child:
6:cddc73091ab5
this works, only controller needs to be updated since I broke the moving average filtered one....;

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