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

Committer:
megrootens
Date:
Sun Nov 12 00:14:05 2017 +0000
Revision:
0:caa8ee3bd882
Child:
2:df0c6af898ac
setup code. too much play in gears and axles slip. But performance already better than bio robotics.

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 0:caa8ee3bd882 7 void ui::StatusDisplay()
megrootens 0:caa8ee3bd882 8 {
megrootens 0:caa8ee3bd882 9 switch (robot::state) {
megrootens 0:caa8ee3bd882 10 case robot::OFF: // continuous red;
megrootens 0:caa8ee3bd882 11 rgb_led[0] = LED_ON;
megrootens 0:caa8ee3bd882 12 rgb_led[1] = !LED_ON;
megrootens 0:caa8ee3bd882 13 rgb_led[2] = !LED_ON;
megrootens 0:caa8ee3bd882 14 break;
megrootens 0:caa8ee3bd882 15 case robot::CALIBRATION: // alternating red-green; busy
megrootens 0:caa8ee3bd882 16 rgb_led[0] = !rgb_led[0];
megrootens 0:caa8ee3bd882 17 rgb_led[1] = !rgb_led[0];
megrootens 0:caa8ee3bd882 18 rgb_led[2] = !LED_ON;
megrootens 0:caa8ee3bd882 19 break;
megrootens 0:caa8ee3bd882 20 case robot::HOMING: // alternating green-blue; busy
megrootens 0:caa8ee3bd882 21 rgb_led[0] = !LED_ON;
megrootens 0:caa8ee3bd882 22 rgb_led[1] = !rgb_led[1];
megrootens 0:caa8ee3bd882 23 rgb_led[2] = !rgb_led[1];
megrootens 0:caa8ee3bd882 24 break;
megrootens 0:caa8ee3bd882 25 case robot::READY: // continuous green
megrootens 0:caa8ee3bd882 26 rgb_led[0] = !LED_ON;
megrootens 0:caa8ee3bd882 27 rgb_led[1] = LED_ON;
megrootens 0:caa8ee3bd882 28 rgb_led[2] = !LED_ON;
megrootens 0:caa8ee3bd882 29 break;
megrootens 0:caa8ee3bd882 30 case robot::DEMO: // alternating red-blue; busy
megrootens 0:caa8ee3bd882 31 rgb_led[0] = !rgb_led[0];
megrootens 0:caa8ee3bd882 32 rgb_led[1] = !LED_ON;
megrootens 0:caa8ee3bd882 33 rgb_led[2] = !rgb_led[0];
megrootens 0:caa8ee3bd882 34 break;
megrootens 0:caa8ee3bd882 35 case robot::MANUAL: // alternating green-purple
megrootens 0:caa8ee3bd882 36 rgb_led[0] = !rgb_led[0];
megrootens 0:caa8ee3bd882 37 rgb_led[1] = !rgb_led[0];
megrootens 0:caa8ee3bd882 38 rgb_led[2] = rgb_led[0];
megrootens 0:caa8ee3bd882 39 break;
megrootens 0:caa8ee3bd882 40 }
megrootens 0:caa8ee3bd882 41 }
megrootens 0:caa8ee3bd882 42
megrootens 0:caa8ee3bd882 43
megrootens 0:caa8ee3bd882 44
megrootens 0:caa8ee3bd882 45 void ui::SwitchState(State new_state)
megrootens 0:caa8ee3bd882 46 {
megrootens 0:caa8ee3bd882 47 ui::serial.printf("* Switching Input State to: %s\r\n",StateNames[new_state]);
megrootens 0:caa8ee3bd882 48 ui::state = new_state;
megrootens 0:caa8ee3bd882 49 }
megrootens 0:caa8ee3bd882 50
megrootens 0:caa8ee3bd882 51 void robot::SwitchState(State new_state)
megrootens 0:caa8ee3bd882 52 {
megrootens 0:caa8ee3bd882 53 ui::serial.printf("* Switching Robot State to: %s\r\n",StateNames[new_state]);
megrootens 0:caa8ee3bd882 54 state = new_state;
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 0:caa8ee3bd882 82
megrootens 0:caa8ee3bd882 83 void robot::GoToNextState()
megrootens 0:caa8ee3bd882 84 {
megrootens 0:caa8ee3bd882 85 switch (state) {
megrootens 0:caa8ee3bd882 86 case OFF:
megrootens 0:caa8ee3bd882 87 SwitchState(CALIBRATION);
megrootens 0:caa8ee3bd882 88 ui::SwitchState(ui::IGNORE);
megrootens 0:caa8ee3bd882 89 Start();
megrootens 0:caa8ee3bd882 90 break;
megrootens 0:caa8ee3bd882 91 case CALIBRATION:
megrootens 0:caa8ee3bd882 92 SwitchState(HOMING);
megrootens 0:caa8ee3bd882 93 Start();
megrootens 0:caa8ee3bd882 94 break;
megrootens 0:caa8ee3bd882 95 case HOMING:
megrootens 0:caa8ee3bd882 96 SwitchState(DEMO);
megrootens 0:caa8ee3bd882 97 //ui::SwitchState(ui::STATE_SWITCHING);
megrootens 0:caa8ee3bd882 98 break;
megrootens 0:caa8ee3bd882 99 case READY:
megrootens 0:caa8ee3bd882 100 SwitchState(DEMO);
megrootens 0:caa8ee3bd882 101 break;
megrootens 0:caa8ee3bd882 102 case DEMO:
megrootens 0:caa8ee3bd882 103 SwitchState(MANUAL);
megrootens 0:caa8ee3bd882 104 break;
megrootens 0:caa8ee3bd882 105 default:
megrootens 0:caa8ee3bd882 106 SwitchState(OFF);
megrootens 0:caa8ee3bd882 107 robot::is_calibrated = false;
megrootens 0:caa8ee3bd882 108 Stop();
megrootens 0:caa8ee3bd882 109 }
megrootens 0:caa8ee3bd882 110 }
megrootens 0:caa8ee3bd882 111
megrootens 0:caa8ee3bd882 112 bool robot::has_power()
megrootens 0:caa8ee3bd882 113 {
megrootens 0:caa8ee3bd882 114 return m1.has_power() or m2.has_power();
megrootens 0:caa8ee3bd882 115 }
megrootens 0:caa8ee3bd882 116
megrootens 0:caa8ee3bd882 117 double robot::get_angle(int i)
megrootens 0:caa8ee3bd882 118 {
megrootens 0:caa8ee3bd882 119 switch(i) {
megrootens 0:caa8ee3bd882 120 case 1:
megrootens 0:caa8ee3bd882 121 return m1.get_angle();
megrootens 0:caa8ee3bd882 122 case 2:
megrootens 0:caa8ee3bd882 123 return m2.get_angle();
megrootens 0:caa8ee3bd882 124 default:
megrootens 0:caa8ee3bd882 125 return NULL;
megrootens 0:caa8ee3bd882 126 }
megrootens 0:caa8ee3bd882 127 }
megrootens 0:caa8ee3bd882 128
megrootens 0:caa8ee3bd882 129 double robot::ForwardKinematicsX(double theta_1, double theta_2)
megrootens 0:caa8ee3bd882 130 {
megrootens 0:caa8ee3bd882 131 return kL1*cos(deg2rad(theta_1)) + kL2*cos(deg2rad(theta_2));
megrootens 0:caa8ee3bd882 132 }
megrootens 0:caa8ee3bd882 133
megrootens 0:caa8ee3bd882 134 double robot::ForwardKinematicsY(double theta_1, double theta_2)
megrootens 0:caa8ee3bd882 135 {
megrootens 0:caa8ee3bd882 136 return kL1*sin(deg2rad(theta_1)) + kL2*sin(deg2rad(theta_2));
megrootens 0:caa8ee3bd882 137 }
megrootens 0:caa8ee3bd882 138
megrootens 0:caa8ee3bd882 139 double robot::get_x()
megrootens 0:caa8ee3bd882 140 {
megrootens 0:caa8ee3bd882 141 return ForwardKinematicsX(get_angle(1),get_angle(2));
megrootens 0:caa8ee3bd882 142 }
megrootens 0:caa8ee3bd882 143
megrootens 0:caa8ee3bd882 144 double robot::get_y()
megrootens 0:caa8ee3bd882 145 {
megrootens 0:caa8ee3bd882 146 return ForwardKinematicsY(get_angle(1),get_angle(2));
megrootens 0:caa8ee3bd882 147 }
megrootens 0:caa8ee3bd882 148
megrootens 0:caa8ee3bd882 149 double robot::InverseKinematicsTheta(double x, double y, int i)
megrootens 0:caa8ee3bd882 150 {
megrootens 0:caa8ee3bd882 151 double r = sqrt(pow(x,2) + pow(y,2));
megrootens 0:caa8ee3bd882 152 double dir_angle = rad2deg( atan2(y,x) );
megrootens 0:caa8ee3bd882 153
megrootens 0:caa8ee3bd882 154 double int_angle_1 = rad2deg( acos((pow(kL1,2) + pow(r,2) - pow(kL2,2))/(2*kL1*r)) );
megrootens 0:caa8ee3bd882 155 double int_angle_2 = rad2deg( acos((pow(kL2,2) + pow(r,2) - pow(kL1,2))/(2*kL2*r)) );
megrootens 0:caa8ee3bd882 156
megrootens 0:caa8ee3bd882 157 double theta_1 = dir_angle + int_angle_1;
megrootens 0:caa8ee3bd882 158 double theta_2 = dir_angle - int_angle_2;
megrootens 0:caa8ee3bd882 159
megrootens 0:caa8ee3bd882 160 switch (i) {
megrootens 0:caa8ee3bd882 161 case 1: return theta_1;
megrootens 0:caa8ee3bd882 162 case 2: return theta_2;
megrootens 0:caa8ee3bd882 163 default: return NULL;
megrootens 0:caa8ee3bd882 164 }
megrootens 0:caa8ee3bd882 165 }
megrootens 0:caa8ee3bd882 166
megrootens 0:caa8ee3bd882 167 void robot::Start()
megrootens 0:caa8ee3bd882 168 {
megrootens 0:caa8ee3bd882 169 ui::serial.printf(" Setting current position as reference.\r\n");
megrootens 0:caa8ee3bd882 170 ref::SetPositionAsReference();
megrootens 0:caa8ee3bd882 171
megrootens 0:caa8ee3bd882 172 ui::serial.printf(" Resetting controllers.\r\n");
megrootens 0:caa8ee3bd882 173 c1.Reset();
megrootens 0:caa8ee3bd882 174 c2.Reset();
megrootens 0:caa8ee3bd882 175
megrootens 0:caa8ee3bd882 176 ui::serial.printf(" Starting motors.\r\n");
megrootens 0:caa8ee3bd882 177 m1.Start();
megrootens 0:caa8ee3bd882 178 m2.Start();
megrootens 0:caa8ee3bd882 179 }
megrootens 0:caa8ee3bd882 180
megrootens 0:caa8ee3bd882 181 void robot::Stop()
megrootens 0:caa8ee3bd882 182 {
megrootens 0:caa8ee3bd882 183 ui::serial.printf(" Stopping motors.\r\n");
megrootens 0:caa8ee3bd882 184 m1.Stop();
megrootens 0:caa8ee3bd882 185 m2.Stop();
megrootens 0:caa8ee3bd882 186 }
megrootens 0:caa8ee3bd882 187
megrootens 0:caa8ee3bd882 188 void robot::ControlLoop()
megrootens 0:caa8ee3bd882 189 {
megrootens 0:caa8ee3bd882 190 m1.set_pwm(c1.Update( ref::theta_1 - get_angle(1) ));
megrootens 0:caa8ee3bd882 191 m2.set_pwm(c2.Update( ref::theta_2 - get_angle(2) ));
megrootens 0:caa8ee3bd882 192 }
megrootens 0:caa8ee3bd882 193
megrootens 0:caa8ee3bd882 194 void robot::SetCalibrationAngles()
megrootens 0:caa8ee3bd882 195 {
megrootens 0:caa8ee3bd882 196 m1.set_angle(kCalibAngleMotor1);
megrootens 0:caa8ee3bd882 197 m2.set_angle(kCalibAngleMotor2);
megrootens 0:caa8ee3bd882 198 is_calibrated = true;
megrootens 0:caa8ee3bd882 199 }
megrootens 0:caa8ee3bd882 200
megrootens 0:caa8ee3bd882 201 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:caa8ee3bd882 202 // Reference implementation
megrootens 0:caa8ee3bd882 203 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
megrootens 0:caa8ee3bd882 204
megrootens 0:caa8ee3bd882 205 double ref::get_ref_x()
megrootens 0:caa8ee3bd882 206 {
megrootens 0:caa8ee3bd882 207 return robot::ForwardKinematicsX(theta_1,theta_2);
megrootens 0:caa8ee3bd882 208 }
megrootens 0:caa8ee3bd882 209
megrootens 0:caa8ee3bd882 210 double ref::get_ref_y()
megrootens 0:caa8ee3bd882 211 {
megrootens 0:caa8ee3bd882 212 return robot::ForwardKinematicsY(theta_1,theta_2);
megrootens 0:caa8ee3bd882 213 }
megrootens 0:caa8ee3bd882 214
megrootens 0:caa8ee3bd882 215 void ref::UpdateReference()
megrootens 0:caa8ee3bd882 216 {
megrootens 0:caa8ee3bd882 217 switch(robot::state) {
megrootens 0:caa8ee3bd882 218 case robot::CALIBRATION:
megrootens 0:caa8ee3bd882 219 CalibrationReference();
megrootens 0:caa8ee3bd882 220 break;
megrootens 0:caa8ee3bd882 221 case robot::HOMING:
megrootens 0:caa8ee3bd882 222 HomingReference();
megrootens 0:caa8ee3bd882 223 break;
megrootens 0:caa8ee3bd882 224 case robot::DEMO:
megrootens 0:caa8ee3bd882 225 DemoReference();
megrootens 0:caa8ee3bd882 226 break;
megrootens 0:caa8ee3bd882 227 }
megrootens 0:caa8ee3bd882 228 }
megrootens 0:caa8ee3bd882 229
megrootens 0:caa8ee3bd882 230 void ref::CalibrationReference()
megrootens 0:caa8ee3bd882 231 {
megrootens 0:caa8ee3bd882 232 // Calibration process
megrootens 0:caa8ee3bd882 233 bool is_ready_1 = (theta_1 - robot::get_angle(1) > kCalibrationError);
megrootens 0:caa8ee3bd882 234 bool is_ready_2 = (theta_2 - robot::get_angle(2) > kCalibrationError);
megrootens 0:caa8ee3bd882 235
megrootens 0:caa8ee3bd882 236 if (is_ready_1) {
megrootens 0:caa8ee3bd882 237 robot::m1.Stop();
megrootens 0:caa8ee3bd882 238 } else {
megrootens 0:caa8ee3bd882 239 theta_1 += kCalibrationOmegaStep;
megrootens 0:caa8ee3bd882 240 }
megrootens 0:caa8ee3bd882 241
megrootens 0:caa8ee3bd882 242 if (is_ready_2) {
megrootens 0:caa8ee3bd882 243 robot::m2.Stop();
megrootens 0:caa8ee3bd882 244 } else {
megrootens 0:caa8ee3bd882 245 theta_2 += kCalibrationOmegaStep;
megrootens 0:caa8ee3bd882 246 }
megrootens 0:caa8ee3bd882 247
megrootens 0:caa8ee3bd882 248 if (is_ready_1 & is_ready_2) {
megrootens 0:caa8ee3bd882 249 robot::Stop();
megrootens 0:caa8ee3bd882 250 ui::serial.printf("+ Both motors at end-stop.\r\n");
megrootens 0:caa8ee3bd882 251 robot::SetCalibrationAngles();
megrootens 0:caa8ee3bd882 252 robot::GoToNextState();
megrootens 0:caa8ee3bd882 253 }
megrootens 0:caa8ee3bd882 254 }
megrootens 0:caa8ee3bd882 255
megrootens 0:caa8ee3bd882 256 void ref::HomingReference()
megrootens 0:caa8ee3bd882 257 {
megrootens 0:caa8ee3bd882 258 double dist_1 = kOriginTheta1 - theta_1;
megrootens 0:caa8ee3bd882 259
megrootens 0:caa8ee3bd882 260 if (abs(dist_1)<=kCalibrationOmegaStep) {
megrootens 0:caa8ee3bd882 261 // motor 1 ready
megrootens 0:caa8ee3bd882 262 theta_1 = kOriginTheta1;
megrootens 0:caa8ee3bd882 263
megrootens 0:caa8ee3bd882 264 double dist_2 = kOriginTheta2 - theta_2;
megrootens 0:caa8ee3bd882 265
megrootens 0:caa8ee3bd882 266 if (abs(dist_2)<=kCalibrationOmegaStep) {
megrootens 0:caa8ee3bd882 267 // motor 2 also ready
megrootens 0:caa8ee3bd882 268 theta_2 = kOriginTheta2;
megrootens 0:caa8ee3bd882 269
megrootens 0:caa8ee3bd882 270 ui::serial.printf(" Robot at home position.\r\n");
megrootens 0:caa8ee3bd882 271 robot::GoToNextState();
megrootens 0:caa8ee3bd882 272 } else {
megrootens 0:caa8ee3bd882 273 // motor 2 still to move
megrootens 0:caa8ee3bd882 274 theta_2 += dist_2 / abs(dist_2) * kCalibrationOmegaStep;
megrootens 0:caa8ee3bd882 275 }
megrootens 0:caa8ee3bd882 276
megrootens 0:caa8ee3bd882 277
megrootens 0:caa8ee3bd882 278 } else {
megrootens 0:caa8ee3bd882 279 // motor 1 still to move
megrootens 0:caa8ee3bd882 280 theta_1 += dist_1 / abs(dist_1) * kCalibrationOmegaStep;
megrootens 0:caa8ee3bd882 281 }
megrootens 0:caa8ee3bd882 282
megrootens 0:caa8ee3bd882 283
megrootens 0:caa8ee3bd882 284 }
megrootens 0:caa8ee3bd882 285
megrootens 0:caa8ee3bd882 286 void ref::DemoReference()
megrootens 0:caa8ee3bd882 287 {
megrootens 0:caa8ee3bd882 288 double x = robot::ForwardKinematicsX(theta_1,theta_2);
megrootens 0:caa8ee3bd882 289 double y = robot::ForwardKinematicsY(theta_1,theta_2);
megrootens 0:caa8ee3bd882 290
megrootens 0:caa8ee3bd882 291 double x_goal = kDemoCoords[0][i_demo_coord];
megrootens 0:caa8ee3bd882 292 double y_goal = kDemoCoords[1][i_demo_coord];
megrootens 0:caa8ee3bd882 293
megrootens 0:caa8ee3bd882 294 double delta_x = x_goal - x;
megrootens 0:caa8ee3bd882 295 double delta_y = y_goal - y;
megrootens 0:caa8ee3bd882 296
megrootens 0:caa8ee3bd882 297 double dist = sqrt(pow(delta_x,2) + pow(delta_y,2));
megrootens 0:caa8ee3bd882 298
megrootens 0:caa8ee3bd882 299 double x_new = x;
megrootens 0:caa8ee3bd882 300 double y_new = y;
megrootens 0:caa8ee3bd882 301
megrootens 0:caa8ee3bd882 302 if (dist<kMaxStep) {
megrootens 0:caa8ee3bd882 303 x_new = x_goal;
megrootens 0:caa8ee3bd882 304 y_new = y_goal;
megrootens 0:caa8ee3bd882 305
megrootens 0:caa8ee3bd882 306 ui::serial.printf(" Reached Demo Coord %d\r\n",i_demo_coord);
megrootens 0:caa8ee3bd882 307 i_demo_coord = ++i_demo_coord % kNumDemoCoords;
megrootens 0:caa8ee3bd882 308
megrootens 0:caa8ee3bd882 309 if (i_demo_coord == 0) {
megrootens 0:caa8ee3bd882 310 robot::is_calibrated = false;
megrootens 0:caa8ee3bd882 311 robot::SwitchState(robot::CALIBRATION);
megrootens 0:caa8ee3bd882 312 }
megrootens 0:caa8ee3bd882 313 } else {
megrootens 0:caa8ee3bd882 314 x_new += delta_x / dist * kMaxStep;
megrootens 0:caa8ee3bd882 315 y_new += delta_y / dist * kMaxStep;
megrootens 0:caa8ee3bd882 316 }
megrootens 0:caa8ee3bd882 317
megrootens 0:caa8ee3bd882 318 theta_1 = robot::InverseKinematicsTheta(x_new, y_new, 1);
megrootens 0:caa8ee3bd882 319 theta_2 = robot::InverseKinematicsTheta(x_new, y_new, 2);
megrootens 0:caa8ee3bd882 320
megrootens 0:caa8ee3bd882 321
megrootens 0:caa8ee3bd882 322 }
megrootens 0:caa8ee3bd882 323
megrootens 0:caa8ee3bd882 324 void ref::SetPositionAsReference()
megrootens 0:caa8ee3bd882 325 {
megrootens 0:caa8ee3bd882 326 ref::theta_1 = robot::get_angle(1);
megrootens 0:caa8ee3bd882 327 ref::theta_2 = robot::get_angle(2);
megrootens 0:caa8ee3bd882 328 }