Margreet Morsink / Mbed 2 deprecated ControlClaire_motor12

EMG control + motor control of motor 1 + 2

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of ControlClaire by Margreet Morsink

main.cpp@13:b4ed3eba926b, 2016-10-28 (annotated)

Committer:
s1558382
Date:
Fri Oct 28 09:16:33 2016 +0000
Revision:
13:b4ed3eba926b
Parent:
12:fe8002389810
Child:
14:08c982d2bb2a
EMG + MOTOR about to test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1558382 0:62fe4a2a8101 1 #include "mbed.h"
s1558382 12:fe8002389810 2 #include "BiQuad.h"
s1558382 1:43fad4d1dee0 3 #include "MODSERIAL.h"
s1558382 0:62fe4a2a8101 4 #include "QEI.h"
s1558382 0:62fe4a2a8101 5 #include "math.h"
s1558382 0:62fe4a2a8101 6
s1558382 12:fe8002389810 7 //=========== PORTS ==========================================================
s1558382 12:fe8002389810 8 AnalogIn emg1(A0); //right biceps
s1558382 12:fe8002389810 9 AnalogIn emg2(A1); //left biceps
s1558382 0:62fe4a2a8101 10 Serial pc(USBTX,USBRX);
s1558382 12:fe8002389810 11
s1558382 12:fe8002389810 12 PwmOut motor1MagnitudePin(D5);
s1558382 12:fe8002389810 13 PwmOut motor2MagnitudePin(D6);
s1558382 12:fe8002389810 14 DigitalOut motor1DirectionPin (D4);
s1558382 12:fe8002389810 15 DigitalOut motor2DirectionPin (D7);
s1558382 12:fe8002389810 16
s1558382 12:fe8002389810 17 QEI encoder_m1(D12,D13,NC,32);
s1558382 12:fe8002389810 18 QEI encoder_m2(D10,D11,NC,32);
s1558382 12:fe8002389810 19
s1558382 12:fe8002389810 20 DigitalIn button(D2);
s1558382 12:fe8002389810 21
s1558382 12:fe8002389810 22 //=========== OBJECTS USED ===================================================
s1558382 12:fe8002389810 23 // BiQuads
s1558382 12:fe8002389810 24 BiQuadChain bqc1;
s1558382 12:fe8002389810 25 BiQuadChain bqc2;
s1558382 12:fe8002389810 26 BiQuad bq1(0.98818943122,-1.59905034133, 0.98818943122, -1.59325743247, 0.98217188389); // notch
s1558382 12:fe8002389810 27 BiQuad bq2(0.80010174727,-1.60020349454, 0.80010174727, -1.55983681590, 0.64057017317); // high pass
s1558382 12:fe8002389810 28 //Take absolute value between high pass and low pass
s1558382 12:fe8002389810 29 BiQuad bq3(0.00035313141, 0.00070626282, 0.00035313141, -1.94614721828, 0.94755974392); // low pass
s1558382 12:fe8002389810 30
s1558382 12:fe8002389810 31 //States
s1558382 13:b4ed3eba926b 32 enum state_t {REST0, REST, CALIBRATION, COUNTING, CONFIRMATION, GO2TABLESPOT, CLEANING, RETURN2REST};
s1558382 12:fe8002389810 33 state_t state = REST0;
s1558382 12:fe8002389810 34
s1558382 12:fe8002389810 35 //Tickers
s1558382 12:fe8002389810 36 Ticker stateTick;
s1558382 12:fe8002389810 37 Ticker screenTick;
s1558382 12:fe8002389810 38 Ticker angPos1;
s1558382 12:fe8002389810 39 Ticker t1;
s1558382 12:fe8002389810 40 Ticker t2;
s1558382 12:fe8002389810 41 Ticker t3;
s1558382 12:fe8002389810 42 Ticker t4;
s1558382 12:fe8002389810 43 Ticker t5;
s1558382 12:fe8002389810 44 Ticker t6;
s1558382 12:fe8002389810 45
s1558382 12:fe8002389810 46 Timer stateTimer;
s1558382 12:fe8002389810 47
s1558382 12:fe8002389810 48 //Go Flags
s1558382 12:fe8002389810 49 volatile bool fn1_go = false;
s1558382 12:fe8002389810 50 void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
s1558382 12:fe8002389810 51 volatile bool fn2_go = false;
s1558382 12:fe8002389810 52 void fn2_activate(){ fn2_go = true; }; //Activates the go-flag
s1558382 12:fe8002389810 53 volatile bool fn3_go = false;
s1558382 12:fe8002389810 54 void fn3_activate(){ fn3_go = true; }; //Activates the go-flag
s1558382 12:fe8002389810 55 volatile bool fn4_go = false;
s1558382 12:fe8002389810 56 void fn4_activate(){ fn4_go = true; }; //Activates the go−flag
s1558382 12:fe8002389810 57 volatile bool fn5_go = false;
s1558382 12:fe8002389810 58 void fn5_activate(){ fn5_go = true; }; //Activates the go-flag
s1558382 12:fe8002389810 59 volatile bool fn6_go = false;
s1558382 12:fe8002389810 60 void fn6_activate(){ fn6_go = true; }; //Activates the go-flag
s1558382 12:fe8002389810 61
s1558382 12:fe8002389810 62 //=========== CONSTANTS =====================================================
s1558382 12:fe8002389810 63 const float ts = 0.005; //Sample time 200 Hz
s1558382 12:fe8002389810 64 const double pi = 3.14159265359;
s1558382 12:fe8002389810 65 //const double transmissionShoulder =94.4/40.2;
s1558382 12:fe8002389810 66 //const double transmissionElbow = 1.0;
s1558382 12:fe8002389810 67
s1558382 12:fe8002389810 68 //Controller constants
s1558382 12:fe8002389810 69 const double m1_Kp = 120.0, m1_Ki = 1.44876354368902, m1_Kd = -1.55261758822823, m1_N = 1.70578345077793;
s1558382 12:fe8002389810 70 const double m2_Kp = 120.0, m2_Ki = 1.44876354368902, m2_Kd = -1.55261758822823, m2_N = 1.70578345077793;
s1558382 12:fe8002389810 71 const double m1_Ts = 0.001; // Controller sample time motor 1
s1558382 12:fe8002389810 72 const double m2_Ts = 0.001; // Controller sample time motor 2
s1558382 12:fe8002389810 73
s1558382 12:fe8002389810 74 //Angles
s1558382 12:fe8002389810 75 double O1 = 1.7633;
s1558382 12:fe8002389810 76 double O2 = 2.0915;
s1558382 12:fe8002389810 77 double O3 = 1.8685;
s1558382 12:fe8002389810 78 double O4 = 1.1363;
s1558382 12:fe8002389810 79 double O5 = 2.3960;
s1558382 12:fe8002389810 80 double O6 = 2.0827;
s1558382 12:fe8002389810 81 double B1 = 1.3551;
s1558382 12:fe8002389810 82 double B2 = 0.5964;
s1558382 12:fe8002389810 83 double B3 = 0.06652;
s1558382 12:fe8002389810 84 double B4 = 0.0669;
s1558382 12:fe8002389810 85 double B5 = 1.7462;
s1558382 12:fe8002389810 86 double B6 = -0.8994;
s1558382 12:fe8002389810 87
s1558382 12:fe8002389810 88
s1558382 12:fe8002389810 89 //Max/Min angles
s1558382 12:fe8002389810 90 const double max_rad_m1 = pi;
s1558382 12:fe8002389810 91 const double min_rad_m1 = -0.5*pi;
s1558382 12:fe8002389810 92 const double max_rad_m2 = pi;
s1558382 12:fe8002389810 93 const double min_rad_m2 = -0.5*pi;
s1558382 0:62fe4a2a8101 94
s1558382 12:fe8002389810 95 //=========== BOOLEANS ======================================================
s1558382 12:fe8002389810 96 bool contracted = false;
s1558382 12:fe8002389810 97 bool confirm = false;
s1558382 12:fe8002389810 98 bool calibration = false;
s1558382 12:fe8002389810 99 bool EMG = true;
s1558382 12:fe8002389810 100
s1558382 12:fe8002389810 101 //=========== VARIABLES =====================================================
s1558382 12:fe8002389810 102 //Filtered EMG Signal
s1558382 12:fe8002389810 103 double emgFilteredRight;
s1558382 12:fe8002389810 104 double emgFilteredLeft;
s1558382 12:fe8002389810 105
s1558382 12:fe8002389810 106 //Max values
s1558382 12:fe8002389810 107 double emgMaxRight;
s1558382 12:fe8002389810 108 double emgMaxLeft;
s1558382 12:fe8002389810 109
s1558382 12:fe8002389810 110 //Setting the thresholds
s1558382 12:fe8002389810 111 double emgThresholdRightLow;
s1558382 12:fe8002389810 112 double emgThresholdRightHigh;
s1558382 12:fe8002389810 113 double emgThresholdLeftLow;
s1558382 12:fe8002389810 114 double emgThresholdLeftHigh;
s1558382 12:fe8002389810 115
s1558382 12:fe8002389810 116 int emgCounts;
s1558382 12:fe8002389810 117 int tableSpot;
s1558382 12:fe8002389810 118
s1558382 12:fe8002389810 119 //Timing aspect
s1558382 12:fe8002389810 120 int currentRepetitions = 0;
s1558382 12:fe8002389810 121 int previousRepetitions = 0;
s1558382 12:fe8002389810 122
s1558382 12:fe8002389810 123 //Motor variables
s1558382 12:fe8002389810 124 double m1_v1 = 0;
s1558382 12:fe8002389810 125 double m1_v2 = 0;
s1558382 12:fe8002389810 126 double m2_v1 = 0;
s1558382 12:fe8002389810 127 double m2_v2 = 0;
s1558382 12:fe8002389810 128
s1558382 12:fe8002389810 129 //Position variable
s1558382 12:fe8002389810 130 volatile double radians_m1;
s1558382 12:fe8002389810 131 volatile double radians_m2;
s1558382 12:fe8002389810 132
s1558382 12:fe8002389810 133 //Plant variable
s1558382 12:fe8002389810 134 volatile double motor1;
s1558382 12:fe8002389810 135 volatile double motor2;
s1558382 12:fe8002389810 136
s1558382 12:fe8002389810 137 //Reference positions
s1558382 12:fe8002389810 138 double reference_m1 = 0;
s1558382 12:fe8002389810 139 double reference_m2 = 0;
s1558382 12:fe8002389810 140
s1558382 12:fe8002389810 141 //=========== FUNCTIONS ======================================================
s1558382 12:fe8002389810 142
s1558382 12:fe8002389810 143 //=========== EMG ============================================================
s1558382 12:fe8002389810 144 void rest()
s1558382 12:fe8002389810 145 /*--------------------------------------------------
s1558382 12:fe8002389810 146 This function is for the resting situation.
s1558382 12:fe8002389810 147 --------------------------------------------------*/
s1558382 12:fe8002389810 148 {
s1558382 12:fe8002389810 149 currentRepetitions = previousRepetitions+1;
s1558382 12:fe8002389810 150 previousRepetitions = currentRepetitions;
s1558382 12:fe8002389810 151 }
s1558382 12:fe8002389810 152
s1558382 12:fe8002389810 153 void emgCount()
s1558382 12:fe8002389810 154 /*--------------------------------------------------
s1558382 12:fe8002389810 155 This function counts the contractions of the right
s1558382 12:fe8002389810 156 biceps.
s1558382 12:fe8002389810 157 --------------------------------------------------*/
s1558382 12:fe8002389810 158 {
s1558382 12:fe8002389810 159 if(emgFilteredRight >= emgThresholdRightHigh)
s1558382 12:fe8002389810 160 {
s1558382 12:fe8002389810 161 contracted = true;
s1558382 12:fe8002389810 162 }
s1558382 12:fe8002389810 163
s1558382 12:fe8002389810 164 if(contracted == true && emgFilteredRight <= emgThresholdRightLow)
s1558382 12:fe8002389810 165 {
s1558382 12:fe8002389810 166 contracted = false;
s1558382 12:fe8002389810 167 emgCounts++;
s1558382 12:fe8002389810 168 }
s1558382 12:fe8002389810 169 }
s1558382 12:fe8002389810 170
s1558382 12:fe8002389810 171 void emgConfirm()
s1558382 12:fe8002389810 172 /*--------------------------------------------------
s1558382 12:fe8002389810 173 This function confirms the EMG counts via contracting
s1558382 12:fe8002389810 174 the left biceps.
s1558382 12:fe8002389810 175 --------------------------------------------------*/
s1558382 12:fe8002389810 176 {
s1558382 12:fe8002389810 177 if(emgFilteredLeft >= emgThresholdLeftHigh)
s1558382 12:fe8002389810 178 {
s1558382 12:fe8002389810 179 contracted = true;
s1558382 12:fe8002389810 180 }
s1558382 12:fe8002389810 181
s1558382 12:fe8002389810 182 if(contracted == true && emgFilteredLeft <= emgThresholdLeftLow)
s1558382 12:fe8002389810 183 {
s1558382 12:fe8002389810 184 contracted = false;
s1558382 12:fe8002389810 185 confirm = true;
s1558382 12:fe8002389810 186 }
s1558382 12:fe8002389810 187 }
s1558382 2:29d7391d7bc5 188
s1558382 12:fe8002389810 189 void emgFilterRight()
s1558382 12:fe8002389810 190 /*--------------------------------------------------
s1558382 12:fe8002389810 191 This function filters and samples the EMG signal
s1558382 12:fe8002389810 192 from the right biceps.
s1558382 12:fe8002389810 193 --------------------------------------------------*/
s1558382 12:fe8002389810 194 {
s1558382 12:fe8002389810 195 double emgValueRight = emg1.read();
s1558382 12:fe8002389810 196 double emgFilter1Right = bqc1.step(emgValueRight);
s1558382 12:fe8002389810 197 double emgAbsRight = fabs(emgFilter1Right);
s1558382 12:fe8002389810 198
s1558382 12:fe8002389810 199 emgFilteredRight = bqc2.step(emgAbsRight);
s1558382 12:fe8002389810 200
s1558382 12:fe8002389810 201 if(state != CALIBRATION)
s1558382 12:fe8002389810 202 {
s1558382 12:fe8002389810 203 emgCount();
s1558382 12:fe8002389810 204 }
s1558382 12:fe8002389810 205 }
s1558382 12:fe8002389810 206
s1558382 12:fe8002389810 207 void emgFilterLeft()
s1558382 12:fe8002389810 208 /*--------------------------------------------------
s1558382 12:fe8002389810 209 This function filters and samples the EMG signal
s1558382 12:fe8002389810 210 from the left biceps.
s1558382 12:fe8002389810 211 --------------------------------------------------*/
s1558382 12:fe8002389810 212 {
s1558382 12:fe8002389810 213 double emgValueLeft = emg2.read();
s1558382 12:fe8002389810 214 double emgFilter1Left = bqc1.step(emgValueLeft);
s1558382 12:fe8002389810 215 double emgAbsLeft = fabs(emgFilter1Left);
s1558382 12:fe8002389810 216
s1558382 12:fe8002389810 217 emgFilteredLeft = bqc2.step(emgAbsLeft);
s1558382 12:fe8002389810 218
s1558382 12:fe8002389810 219 if(state == CONFIRMATION)
s1558382 12:fe8002389810 220 {
s1558382 12:fe8002389810 221 emgConfirm();
s1558382 12:fe8002389810 222 }
s1558382 12:fe8002389810 223 }
s1558382 7:5b14dbb9e6d1 224
s1558382 12:fe8002389810 225 void emgMaxValueRight()
s1558382 12:fe8002389810 226 /*--------------------------------------------------
s1558382 12:fe8002389810 227 This function measures the maximal EMG value to set
s1558382 12:fe8002389810 228 thresholds for the right biceps.
s1558382 12:fe8002389810 229 It is used for calibration.
s1558382 12:fe8002389810 230 --------------------------------------------------*/
s1558382 12:fe8002389810 231 {
s1558382 12:fe8002389810 232 emgFilterRight();
s1558382 12:fe8002389810 233 if(emgFilteredRight >= emgMaxRight)
s1558382 12:fe8002389810 234 {
s1558382 12:fe8002389810 235 emgMaxRight = emgFilteredRight;
s1558382 12:fe8002389810 236 }
s1558382 12:fe8002389810 237
s1558382 12:fe8002389810 238 //Setting the thresholds
s1558382 12:fe8002389810 239 emgThresholdRightLow = 0.4*emgMaxRight;
s1558382 12:fe8002389810 240 emgThresholdRightHigh = 0.7*emgMaxRight;
s1558382 12:fe8002389810 241 }
s1558382 12:fe8002389810 242
s1558382 12:fe8002389810 243 void emgMaxValueLeft()
s1558382 12:fe8002389810 244 /*--------------------------------------------------
s1558382 12:fe8002389810 245 This function measures the maximal EMG value to set
s1558382 12:fe8002389810 246 thresholds for the left biceps.
s1558382 12:fe8002389810 247 It is used for calibration.
s1558382 12:fe8002389810 248 --------------------------------------------------*/
s1558382 12:fe8002389810 249 {
s1558382 12:fe8002389810 250 emgFilterLeft();
s1558382 12:fe8002389810 251 if(emgFilteredLeft >= emgMaxLeft)
s1558382 12:fe8002389810 252 {
s1558382 12:fe8002389810 253 emgMaxLeft = emgFilteredLeft;
s1558382 12:fe8002389810 254 }
s1558382 12:fe8002389810 255
s1558382 12:fe8002389810 256 //Setting the thresholds
s1558382 12:fe8002389810 257 emgThresholdLeftLow = 0.4*emgMaxLeft;
s1558382 12:fe8002389810 258 emgThresholdLeftHigh = 0.7*emgMaxLeft;
s1558382 12:fe8002389810 259 }
s1558382 12:fe8002389810 260
s1558382 12:fe8002389810 261 void emgCalibration()
s1558382 12:fe8002389810 262 /*--------------------------------------------------
s1558382 12:fe8002389810 263 This function calibrates the EMG signal from the
s1558382 12:fe8002389810 264 right biceps.
s1558382 12:fe8002389810 265 --------------------------------------------------*/
s1558382 0:62fe4a2a8101 266 {
s1558382 12:fe8002389810 267 currentRepetitions = previousRepetitions+1;
s1558382 12:fe8002389810 268 previousRepetitions = currentRepetitions;
s1558382 12:fe8002389810 269
s1558382 12:fe8002389810 270 if(currentRepetitions <= 1000)
s1558382 12:fe8002389810 271 {
s1558382 12:fe8002389810 272 emgMaxValueRight();
s1558382 12:fe8002389810 273 }
s1558382 12:fe8002389810 274 else if(1000 <= currentRepetitions && currentRepetitions <= 2000)
s1558382 12:fe8002389810 275 {
s1558382 12:fe8002389810 276 emgMaxValueLeft();
s1558382 12:fe8002389810 277 }
s1558382 12:fe8002389810 278 else if(currentRepetitions >= 2000)
s1558382 12:fe8002389810 279 {
s1558382 12:fe8002389810 280 calibration = true;
s1558382 12:fe8002389810 281 }
s1558382 12:fe8002389810 282 }
s1558382 12:fe8002389810 283
s1558382 12:fe8002389810 284 void getTableSpot()
s1558382 12:fe8002389810 285 /*--------------------------------------------------
s1558382 12:fe8002389810 286 This function determines to which spot on the table
s1558382 12:fe8002389810 287 the robot arm should move.
s1558382 12:fe8002389810 288 --------------------------------------------------*/
s1558382 12:fe8002389810 289 {
s1558382 12:fe8002389810 290 currentRepetitions = previousRepetitions+1;
s1558382 12:fe8002389810 291 previousRepetitions = currentRepetitions;
s1558382 12:fe8002389810 292
s1558382 12:fe8002389810 293 if(200 <= currentRepetitions && currentRepetitions < 2000)
s1558382 1:43fad4d1dee0 294 {
s1558382 12:fe8002389810 295 emgFilterRight();
s1558382 12:fe8002389810 296 tableSpot = emgCounts;
s1558382 12:fe8002389810 297 }
s1558382 12:fe8002389810 298 }
s1558382 12:fe8002389810 299
s1558382 12:fe8002389810 300 //=========== MOTOR ==========================================================
s1558382 12:fe8002389810 301 double PID1( double err, const double Kp, const double Ki, const double Kd,
s1558382 12:fe8002389810 302 const double Ts, const double N, double &v1, double &v2 )
s1558382 12:fe8002389810 303 /*--------------------------------------------------
s1558382 12:fe8002389810 304 This function is a PID controller for motor 1.
s1558382 12:fe8002389810 305 --------------------------------------------------*/
s1558382 12:fe8002389810 306 {
s1558382 12:fe8002389810 307 // These variables are only calculated once!
s1558382 12:fe8002389810 308 const double a1 = (-4.0/(N*Ts+2));
s1558382 12:fe8002389810 309 const double a2 = -(N*Ts-2)/(N*Ts+2);
s1558382 12:fe8002389810 310 const double b0 = (4.0*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
s1558382 12:fe8002389810 311 const double b1 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
s1558382 12:fe8002389810 312 const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
s1558382 12:fe8002389810 313
s1558382 12:fe8002389810 314 double v = err - a1*v1 - a2*v2;
s1558382 12:fe8002389810 315 double u = b0*v + b1*v1 + b2*v2;
s1558382 12:fe8002389810 316 v2 = v1; v1 = v;
s1558382 12:fe8002389810 317 return u;
s1558382 12:fe8002389810 318 }
s1558382 12:fe8002389810 319
s1558382 12:fe8002389810 320 double PID2( double err, const double Kp, const double Ki, const double Kd,
s1558382 12:fe8002389810 321 const double Ts, const double N, double &v1_2, double &v2_2 )
s1558382 12:fe8002389810 322 /*--------------------------------------------------
s1558382 12:fe8002389810 323 This function is a PID controller for motor 1.
s1558382 12:fe8002389810 324 --------------------------------------------------*/
s1558382 12:fe8002389810 325 {
s1558382 12:fe8002389810 326 // These variables are only calculated once!
s1558382 12:fe8002389810 327 const double a1 = (-4.0/(N*Ts+2));
s1558382 12:fe8002389810 328 const double a2 = -(N*Ts-2)/(N*Ts+2);
s1558382 12:fe8002389810 329 const double b0 = (4.0*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
s1558382 12:fe8002389810 330 const double b1 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
s1558382 12:fe8002389810 331 const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
s1558382 12:fe8002389810 332
s1558382 12:fe8002389810 333 double v = err - a1*v1_2 - a2*v2_2;
s1558382 12:fe8002389810 334 double u = b0*v + b1*v1_2 + b2*v2_2;
s1558382 12:fe8002389810 335 v2_2 = v1_2; v1_2 = v;
s1558382 12:fe8002389810 336 return u;
s1558382 12:fe8002389810 337 }
s1558382 12:fe8002389810 338
s1558382 12:fe8002389810 339 void getAngPosition_m1()
s1558382 12:fe8002389810 340 /*--------------------------------------------------
s1558382 12:fe8002389810 341 This function gets the angular position of motor 1.
s1558382 12:fe8002389810 342 --------------------------------------------------*/
s1558382 12:fe8002389810 343 {
s1558382 12:fe8002389810 344 volatile int pulses_m1 = encoder_m1.getPulses();
s1558382 12:fe8002389810 345 radians_m1 = (pulses_m1*0.002991*0.5); //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
s1558382 12:fe8002389810 346 }
s1558382 12:fe8002389810 347
s1558382 12:fe8002389810 348 void getAngPosition_m2()
s1558382 12:fe8002389810 349 /*--------------------------------------------------
s1558382 12:fe8002389810 350 This function gets the angular position of motor 2.
s1558382 12:fe8002389810 351 --------------------------------------------------*/
s1558382 12:fe8002389810 352 {
s1558382 12:fe8002389810 353 volatile int pulses_m2 = encoder_m2.getPulses();
s1558382 12:fe8002389810 354 radians_m2 = (pulses_m2*0.002991*0.5); //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
s1558382 12:fe8002389810 355 }
s1558382 12:fe8002389810 356
s1558382 12:fe8002389810 357 void motor1_Controller(double radians_m1)
s1558382 12:fe8002389810 358 /*--------------------------------------------------
s1558382 12:fe8002389810 359 This function measures the error and applies it to
s1558382 12:fe8002389810 360 the output to the plant.
s1558382 12:fe8002389810 361 --------------------------------------------------*/
s1558382 12:fe8002389810 362 {
s1558382 12:fe8002389810 363 volatile double error_m1 = reference_m1 - radians_m1;
s1558382 12:fe8002389810 364 motor1 = PID1( error_m1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, m1_v1, m1_v2 );
s1558382 12:fe8002389810 365 }
s1558382 12:fe8002389810 366
s1558382 12:fe8002389810 367 // Next task, measure the error and apply the output to the plant
s1558382 12:fe8002389810 368 void motor2_Controller(double radians_m2)
s1558382 12:fe8002389810 369 /*--------------------------------------------------
s1558382 12:fe8002389810 370 This function measures the error and applies it to
s1558382 12:fe8002389810 371 the output to the plant.
s1558382 12:fe8002389810 372 --------------------------------------------------*/
s1558382 12:fe8002389810 373 {
s1558382 12:fe8002389810 374 volatile double error_m2 = reference_m2 - radians_m2;
s1558382 12:fe8002389810 375 motor2 = PID1( error_m2,m2_Kp,m2_Ki,m2_Kd,m2_Ts, m2_N, m2_v1, m2_v2 );
s1558382 12:fe8002389810 376
s1558382 13:b4ed3eba926b 377 // pc.printf("motor2 = %d",reference_m2);
s1558382 12:fe8002389810 378 }
s1558382 12:fe8002389810 379
s1558382 13:b4ed3eba926b 380 void control_m1(double motor1, double radians_m1)
s1558382 12:fe8002389810 381 /*--------------------------------------------------
s1558382 12:fe8002389810 382 This function controls motor 1.
s1558382 12:fe8002389810 383 --------------------------------------------------*/
s1558382 12:fe8002389810 384 {
s1558382 12:fe8002389810 385 if(abs(motor1) > 1.0)
s1558382 12:fe8002389810 386 {
s1558382 12:fe8002389810 387 motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
s1558382 12:fe8002389810 388 }
s1558382 1:43fad4d1dee0 389 else
s1558382 1:43fad4d1dee0 390 {
s1558382 12:fe8002389810 391 motor1MagnitudePin=0.0;
s1558382 12:fe8002389810 392 }
s1558382 12:fe8002389810 393
s1558382 12:fe8002389810 394 if(motor1 <= 0)
s1558382 12:fe8002389810 395 {
s1558382 12:fe8002389810 396 motor1DirectionPin=0.0;
s1558382 12:fe8002389810 397 }
s1558382 12:fe8002389810 398 else
s1558382 12:fe8002389810 399 {
s1558382 12:fe8002389810 400 motor1DirectionPin=1.0;
s1558382 1:43fad4d1dee0 401 }
s1558382 12:fe8002389810 402
s1558382 12:fe8002389810 403 if (radians_m1>max_rad_m1)
s1558382 12:fe8002389810 404 {
s1558382 12:fe8002389810 405 motor1MagnitudePin = 0;
s1558382 12:fe8002389810 406 }
s1558382 12:fe8002389810 407 else if (radians_m1<min_rad_m1)
s1558382 12:fe8002389810 408 {
s1558382 12:fe8002389810 409 motor1MagnitudePin = 0;
s1558382 12:fe8002389810 410 }
s1558382 0:62fe4a2a8101 411 }
s1558382 12:fe8002389810 412
s1558382 13:b4ed3eba926b 413 void control_m2(double motor2, double radians_m2)
s1558382 12:fe8002389810 414 /*--------------------------------------------------
s1558382 12:fe8002389810 415 This function controls motor 2.
s1558382 12:fe8002389810 416 --------------------------------------------------*/
s1558382 2:29d7391d7bc5 417 {
s1558382 12:fe8002389810 418 if(abs(motor2) > 1)
s1558382 1:43fad4d1dee0 419 {
s1558382 12:fe8002389810 420 motor2MagnitudePin=0.2;///MOET NOG TUSSENWAAREN KRIJGEN
s1558382 1:43fad4d1dee0 421 }
s1558382 1:43fad4d1dee0 422 else
s1558382 1:43fad4d1dee0 423 {
s1558382 12:fe8002389810 424 motor2MagnitudePin=0.0;
s1558382 12:fe8002389810 425 }
s1558382 12:fe8002389810 426
s1558382 12:fe8002389810 427 if(motor2 <= 0)
s1558382 12:fe8002389810 428 {
s1558382 12:fe8002389810 429 motor2DirectionPin=1.0;
s1558382 12:fe8002389810 430 }
s1558382 12:fe8002389810 431 else
s1558382 12:fe8002389810 432 {
s1558382 12:fe8002389810 433 motor2DirectionPin=0.0;
s1558382 12:fe8002389810 434 }
s1558382 12:fe8002389810 435
s1558382 12:fe8002389810 436 if (radians_m2>max_rad_m2)
s1558382 12:fe8002389810 437 {
s1558382 12:fe8002389810 438 motor2MagnitudePin = 0;
s1558382 12:fe8002389810 439 }
s1558382 12:fe8002389810 440 else if (radians_m2<min_rad_m2)
s1558382 12:fe8002389810 441 {
s1558382 12:fe8002389810 442 motor2MagnitudePin = 0;
s1558382 12:fe8002389810 443 }
s1558382 12:fe8002389810 444 }
s1558382 12:fe8002389810 445
s1558382 12:fe8002389810 446 //=========== STATE ==========================================================
s1558382 12:fe8002389810 447 void stateMachine()
s1558382 12:fe8002389810 448 /*--------------------------------------------------
s1558382 12:fe8002389810 449 This function makes sure different functions are
s1558382 12:fe8002389810 450 done in a certain order.
s1558382 12:fe8002389810 451 --------------------------------------------------*/
s1558382 12:fe8002389810 452 {
s1558382 12:fe8002389810 453 stateTimer.reset();
s1558382 12:fe8002389810 454 switch(state)
s1558382 12:fe8002389810 455 {
s1558382 12:fe8002389810 456 case REST0:
s1558382 12:fe8002389810 457 {
s1558382 12:fe8002389810 458 pc.printf("Hi I am Claire!\r\n");
s1558382 12:fe8002389810 459 state = REST;
s1558382 12:fe8002389810 460 }
s1558382 12:fe8002389810 461 break;
s1558382 12:fe8002389810 462
s1558382 12:fe8002389810 463 case REST:
s1558382 12:fe8002389810 464 {
s1558382 12:fe8002389810 465 rest();
s1558382 12:fe8002389810 466 if(currentRepetitions >= 1000 && EMG == 1)
s1558382 12:fe8002389810 467 {
s1558382 12:fe8002389810 468 currentRepetitions = 0;
s1558382 12:fe8002389810 469 previousRepetitions = 0;
s1558382 12:fe8002389810 470 state = CALIBRATION;
s1558382 12:fe8002389810 471 }
s1558382 12:fe8002389810 472 else if(currentRepetitions >= 1000)
s1558382 12:fe8002389810 473 {
s1558382 12:fe8002389810 474 currentRepetitions = 0;
s1558382 12:fe8002389810 475 previousRepetitions = 0;
s1558382 12:fe8002389810 476 emgCounts = 0;
s1558382 12:fe8002389810 477 state = COUNTING;
s1558382 12:fe8002389810 478 }
s1558382 12:fe8002389810 479 }
s1558382 12:fe8002389810 480 break;
s1558382 12:fe8002389810 481
s1558382 12:fe8002389810 482 case CALIBRATION:
s1558382 12:fe8002389810 483 {
s1558382 12:fe8002389810 484 emgCalibration();
s1558382 12:fe8002389810 485 if(currentRepetitions >=2200)
s1558382 12:fe8002389810 486 {
s1558382 12:fe8002389810 487 currentRepetitions = 0;
s1558382 12:fe8002389810 488 previousRepetitions = 0;
s1558382 12:fe8002389810 489 state = COUNTING;
s1558382 12:fe8002389810 490 EMG = 0;
s1558382 12:fe8002389810 491 }
s1558382 12:fe8002389810 492 }
s1558382 12:fe8002389810 493 break;
s1558382 12:fe8002389810 494
s1558382 12:fe8002389810 495 case COUNTING:
s1558382 12:fe8002389810 496 {
s1558382 12:fe8002389810 497 getTableSpot();
s1558382 12:fe8002389810 498 if(currentRepetitions >= 2200)
s1558382 12:fe8002389810 499 {
s1558382 12:fe8002389810 500 currentRepetitions = 0;
s1558382 12:fe8002389810 501 previousRepetitions = 0;
s1558382 12:fe8002389810 502 state = CONFIRMATION;
s1558382 12:fe8002389810 503 }
s1558382 12:fe8002389810 504 }
s1558382 12:fe8002389810 505 break;
s1558382 12:fe8002389810 506
s1558382 12:fe8002389810 507 case CONFIRMATION:
s1558382 12:fe8002389810 508 {
s1558382 12:fe8002389810 509 rest(); //For counting purposes
s1558382 12:fe8002389810 510 emgFilterLeft();
s1558382 12:fe8002389810 511 if(confirm == true && currentRepetitions >= 1200)
s1558382 12:fe8002389810 512 {
s1558382 12:fe8002389810 513 currentRepetitions = 0;
s1558382 12:fe8002389810 514 previousRepetitions = 0;
s1558382 12:fe8002389810 515 state = GO2TABLESPOT;
s1558382 12:fe8002389810 516 }
s1558382 12:fe8002389810 517 else if(currentRepetitions >= 1200)
s1558382 12:fe8002389810 518 {
s1558382 12:fe8002389810 519 currentRepetitions = 0;
s1558382 12:fe8002389810 520 previousRepetitions = 0;
s1558382 12:fe8002389810 521 state = REST;
s1558382 12:fe8002389810 522 }
s1558382 12:fe8002389810 523 }
s1558382 12:fe8002389810 524 break;
s1558382 12:fe8002389810 525
s1558382 12:fe8002389810 526 case GO2TABLESPOT:
s1558382 12:fe8002389810 527 {
s1558382 12:fe8002389810 528 rest(); //For counting purposes
s1558382 12:fe8002389810 529 if(currentRepetitions < 2200) //Tijd = 10 secondes, misschien anders.
s1558382 12:fe8002389810 530 if(tableSpot == 1)
s1558382 12:fe8002389810 531 {
s1558382 12:fe8002389810 532 reference_m1 = O1;
s1558382 12:fe8002389810 533 reference_m2 = B1;
s1558382 12:fe8002389810 534 }
s1558382 12:fe8002389810 535
s1558382 12:fe8002389810 536 else if(tableSpot == 2)
s1558382 12:fe8002389810 537 {
s1558382 12:fe8002389810 538 reference_m1 = O2;
s1558382 12:fe8002389810 539 reference_m2 = B2;
s1558382 12:fe8002389810 540 }
s1558382 12:fe8002389810 541
s1558382 12:fe8002389810 542 else if(tableSpot == 3)
s1558382 12:fe8002389810 543 {
s1558382 12:fe8002389810 544 reference_m1 = O3;
s1558382 12:fe8002389810 545 reference_m2 = B3;
s1558382 12:fe8002389810 546 }
s1558382 12:fe8002389810 547
s1558382 12:fe8002389810 548 else if(tableSpot == 4)
s1558382 12:fe8002389810 549 {
s1558382 12:fe8002389810 550 reference_m1 = O4;
s1558382 12:fe8002389810 551 reference_m2 = B4;
s1558382 12:fe8002389810 552 }
s1558382 12:fe8002389810 553
s1558382 12:fe8002389810 554 else if(tableSpot == 5)
s1558382 12:fe8002389810 555 {
s1558382 12:fe8002389810 556 reference_m1 = O5;
s1558382 12:fe8002389810 557 reference_m2 = B5;
s1558382 12:fe8002389810 558 }
s1558382 12:fe8002389810 559
s1558382 12:fe8002389810 560 else if(tableSpot == 6)
s1558382 12:fe8002389810 561 {
s1558382 12:fe8002389810 562 reference_m1 = O6;
s1558382 12:fe8002389810 563 reference_m2 = B6;
s1558382 12:fe8002389810 564 }
s1558382 12:fe8002389810 565
s1558382 12:fe8002389810 566 if(currentRepetitions >= 2200)
s1558382 12:fe8002389810 567 {
s1558382 12:fe8002389810 568 currentRepetitions = 0;
s1558382 12:fe8002389810 569 previousRepetitions = 0;
s1558382 12:fe8002389810 570 state = CLEANING;
s1558382 12:fe8002389810 571 }
s1558382 12:fe8002389810 572 }
s1558382 12:fe8002389810 573 break;
s1558382 12:fe8002389810 574
s1558382 13:b4ed3eba926b 575 case CLEANING:
s1558382 12:fe8002389810 576 {
s1558382 12:fe8002389810 577 rest(); //For counting purposes
s1558382 12:fe8002389810 578 if(currentRepetitions >= 1200)
s1558382 12:fe8002389810 579 {
s1558382 12:fe8002389810 580 currentRepetitions = 0;
s1558382 12:fe8002389810 581 previousRepetitions = 0;
s1558382 12:fe8002389810 582 state = RETURN2REST;
s1558382 12:fe8002389810 583 }
s1558382 12:fe8002389810 584 }
s1558382 12:fe8002389810 585 break;
s1558382 12:fe8002389810 586
s1558382 13:b4ed3eba926b 587 case RETURN2REST:
s1558382 12:fe8002389810 588 {
s1558382 12:fe8002389810 589 rest(); //For counting purposes
s1558382 12:fe8002389810 590 if(currentRepetitions < 2200)
s1558382 12:fe8002389810 591 {
s1558382 12:fe8002389810 592 reference_m1 = 0;
s1558382 12:fe8002389810 593 reference_m2 = 0;
s1558382 12:fe8002389810 594 }
s1558382 12:fe8002389810 595
s1558382 12:fe8002389810 596 if(currentRepetitions >= 2200)
s1558382 12:fe8002389810 597 {
s1558382 12:fe8002389810 598 currentRepetitions = 0;
s1558382 12:fe8002389810 599 previousRepetitions = 0;
s1558382 12:fe8002389810 600 state = REST;
s1558382 12:fe8002389810 601 }
s1558382 12:fe8002389810 602 }
s1558382 12:fe8002389810 603 break;
s1558382 12:fe8002389810 604
s1558382 12:fe8002389810 605 default:
s1558382 12:fe8002389810 606 {
s1558382 12:fe8002389810 607 currentRepetitions = 0;
s1558382 12:fe8002389810 608 previousRepetitions = 0;
s1558382 12:fe8002389810 609 state = REST;
s1558382 12:fe8002389810 610 }
s1558382 1:43fad4d1dee0 611 }
s1558382 0:62fe4a2a8101 612 }
s1558382 0:62fe4a2a8101 613
s1558382 12:fe8002389810 614 void screenUpdate()
s1558382 12:fe8002389810 615 /*--------------------------------------------------
s1558382 12:fe8002389810 616 This function prints the commands to the screen.
s1558382 12:fe8002389810 617 --------------------------------------------------*/
s1558382 12:fe8002389810 618 {
s1558382 12:fe8002389810 619 if(state == CALIBRATION)
s1558382 12:fe8002389810 620 {
s1558382 12:fe8002389810 621 if(currentRepetitions == 100)
s1558382 12:fe8002389810 622 {
s1558382 12:fe8002389810 623 pc.printf("Welcome to calibration.\n\rYou have 5 seconds to contract your right biceps.\n\r");
s1558382 12:fe8002389810 624 }
s1558382 12:fe8002389810 625 else if(currentRepetitions == 200)
s1558382 12:fe8002389810 626 {
s1558382 12:fe8002389810 627 pc.printf("5\n\r");
s1558382 12:fe8002389810 628 }
s1558382 12:fe8002389810 629 else if(currentRepetitions == 400)
s1558382 12:fe8002389810 630 {
s1558382 12:fe8002389810 631 pc.printf("4\n\r");
s1558382 12:fe8002389810 632 }
s1558382 12:fe8002389810 633 else if(currentRepetitions == 600)
s1558382 12:fe8002389810 634 {
s1558382 12:fe8002389810 635 pc.printf("3\n\r");
s1558382 12:fe8002389810 636 }
s1558382 12:fe8002389810 637 else if(currentRepetitions == 800)
s1558382 12:fe8002389810 638 {
s1558382 12:fe8002389810 639 pc.printf("2\n\r");
s1558382 12:fe8002389810 640 }
s1558382 12:fe8002389810 641 else if(currentRepetitions == 1000)
s1558382 12:fe8002389810 642 {
s1558382 12:fe8002389810 643 pc.printf("1\n\r");
s1558382 12:fe8002389810 644 }
s1558382 12:fe8002389810 645 else if(currentRepetitions == 1050)
s1558382 12:fe8002389810 646 {
s1558382 12:fe8002389810 647 pc.printf("You have 5 seconds to contract your left biceps.\n\r");
s1558382 12:fe8002389810 648 }
s1558382 12:fe8002389810 649 else if(currentRepetitions == 1200)
s1558382 12:fe8002389810 650 {
s1558382 12:fe8002389810 651 pc.printf("5\n\r");
s1558382 12:fe8002389810 652 }
s1558382 12:fe8002389810 653 else if(currentRepetitions == 1400)
s1558382 12:fe8002389810 654 {
s1558382 12:fe8002389810 655 pc.printf("4\n\r");
s1558382 12:fe8002389810 656 }
s1558382 12:fe8002389810 657 else if(currentRepetitions == 1600)
s1558382 12:fe8002389810 658 {
s1558382 12:fe8002389810 659 pc.printf("3\n\r");
s1558382 12:fe8002389810 660 }
s1558382 12:fe8002389810 661 else if(currentRepetitions == 1800)
s1558382 12:fe8002389810 662 {
s1558382 12:fe8002389810 663 pc.printf("2\n\r");
s1558382 12:fe8002389810 664 }
s1558382 12:fe8002389810 665 else if(currentRepetitions == 2000)
s1558382 12:fe8002389810 666 {
s1558382 12:fe8002389810 667 pc.printf("1\n\r");
s1558382 12:fe8002389810 668 }
s1558382 12:fe8002389810 669 else if(currentRepetitions == 2050)
s1558382 12:fe8002389810 670 {
s1558382 12:fe8002389810 671 pc.printf("Calibrations is complete.\n\r");
s1558382 12:fe8002389810 672 /*
s1558382 12:fe8002389810 673 pc.printf("ThresholdRightHigh = %f\n\r.",emgThresholdRightHigh);
s1558382 12:fe8002389810 674 pc.printf("ThresholdLeftHigh = %f\n\r.",emgThresholdLeftHigh);
s1558382 12:fe8002389810 675 pc.printf("ThresholdRightLow = %f\n\r.",emgThresholdRightLow);
s1558382 12:fe8002389810 676 pc.printf("ThresholdLeftLow = %f\n\r.",emgThresholdLeftLow);
s1558382 12:fe8002389810 677 */
s1558382 12:fe8002389810 678 }
s1558382 12:fe8002389810 679 }
s1558382 12:fe8002389810 680
s1558382 12:fe8002389810 681 else if(state == COUNTING)
s1558382 12:fe8002389810 682 {
s1558382 12:fe8002389810 683 if(currentRepetitions == 100)
s1558382 12:fe8002389810 684 {
s1558382 12:fe8002389810 685 pc.printf("You have 10 seconds to contract your right biceps x times to go to that spot on the table.\n\r");
s1558382 12:fe8002389810 686 }
s1558382 12:fe8002389810 687 else if(currentRepetitions == 200)
s1558382 12:fe8002389810 688 {
s1558382 12:fe8002389810 689 pc.printf("10\n\r");
s1558382 12:fe8002389810 690 }
s1558382 12:fe8002389810 691 else if(currentRepetitions == 400)
s1558382 12:fe8002389810 692 {
s1558382 12:fe8002389810 693 pc.printf("9\n\r");
s1558382 12:fe8002389810 694 }
s1558382 12:fe8002389810 695 else if(currentRepetitions == 600)
s1558382 12:fe8002389810 696 {
s1558382 12:fe8002389810 697 pc.printf("8\n\r");
s1558382 12:fe8002389810 698 }
s1558382 12:fe8002389810 699 else if(currentRepetitions == 800)
s1558382 12:fe8002389810 700 {
s1558382 12:fe8002389810 701 pc.printf("7\n\r");
s1558382 12:fe8002389810 702 }
s1558382 12:fe8002389810 703 else if(currentRepetitions == 1000)
s1558382 12:fe8002389810 704 {
s1558382 12:fe8002389810 705 pc.printf("6\n\r");
s1558382 12:fe8002389810 706 }
s1558382 12:fe8002389810 707 else if(currentRepetitions == 1200)
s1558382 12:fe8002389810 708 {
s1558382 12:fe8002389810 709 pc.printf("5\n\r");
s1558382 12:fe8002389810 710 }
s1558382 12:fe8002389810 711 else if(currentRepetitions == 1400)
s1558382 12:fe8002389810 712 {
s1558382 12:fe8002389810 713 pc.printf("4\n\r");
s1558382 12:fe8002389810 714 }
s1558382 12:fe8002389810 715 else if(currentRepetitions == 1600)
s1558382 12:fe8002389810 716 {
s1558382 12:fe8002389810 717 pc.printf("3\n\r");
s1558382 12:fe8002389810 718 }
s1558382 12:fe8002389810 719 else if(currentRepetitions == 1800)
s1558382 12:fe8002389810 720 {
s1558382 12:fe8002389810 721 pc.printf("2\n\r");
s1558382 12:fe8002389810 722 }
s1558382 12:fe8002389810 723 else if(currentRepetitions == 2000)
s1558382 12:fe8002389810 724 {
s1558382 12:fe8002389810 725 pc.printf("1\n\r");
s1558382 12:fe8002389810 726 }
s1558382 12:fe8002389810 727 else if(currentRepetitions == 2050)
s1558382 12:fe8002389810 728 {
s1558382 12:fe8002389810 729 pc.printf("You have selected table spot nr %i, is this correct?\n\r",tableSpot);
s1558382 12:fe8002389810 730 }
s1558382 12:fe8002389810 731 }
s1558382 1:43fad4d1dee0 732
s1558382 12:fe8002389810 733 else if(state == CONFIRMATION)
s1558382 12:fe8002389810 734 {
s1558382 12:fe8002389810 735 if(currentRepetitions == 100)
s1558382 12:fe8002389810 736 {
s1558382 12:fe8002389810 737 pc.printf("You have 5 seconds to confirm with your left biceps.\n\r");
s1558382 12:fe8002389810 738 }
s1558382 12:fe8002389810 739 else if(currentRepetitions == 200)
s1558382 12:fe8002389810 740 {
s1558382 12:fe8002389810 741 pc.printf("5\n\r");
s1558382 12:fe8002389810 742 }
s1558382 12:fe8002389810 743 else if(currentRepetitions == 400)
s1558382 12:fe8002389810 744 {
s1558382 12:fe8002389810 745 pc.printf("4\n\r");
s1558382 12:fe8002389810 746 }
s1558382 12:fe8002389810 747 else if(currentRepetitions == 600)
s1558382 12:fe8002389810 748 {
s1558382 12:fe8002389810 749 pc.printf("3\n\r");
s1558382 12:fe8002389810 750 }
s1558382 12:fe8002389810 751 else if(currentRepetitions == 800)
s1558382 12:fe8002389810 752 {
s1558382 12:fe8002389810 753 pc.printf("2\n\r");
s1558382 12:fe8002389810 754 }
s1558382 12:fe8002389810 755 else if(currentRepetitions == 1000)
s1558382 12:fe8002389810 756 {
s1558382 12:fe8002389810 757 pc.printf("1\n\r");
s1558382 12:fe8002389810 758 }
s1558382 12:fe8002389810 759 else if(confirm == true && currentRepetitions == 1050)
s1558382 12:fe8002389810 760 {
s1558382 12:fe8002389810 761 pc.printf("Table spot nr. %i will be cleaned now.\n\r", tableSpot);
s1558382 12:fe8002389810 762 }
s1558382 12:fe8002389810 763 else if(confirm == false && currentRepetitions == 1050)
s1558382 12:fe8002389810 764 {
s1558382 12:fe8002389810 765 pc.printf("You have cancelled.\n\r");
s1558382 12:fe8002389810 766 }
s1558382 12:fe8002389810 767 }
s1558382 12:fe8002389810 768
s1558382 12:fe8002389810 769 else if(state == GO2TABLESPOT)
s1558382 12:fe8002389810 770 {
s1558382 12:fe8002389810 771 if(currentRepetitions == 100)
s1558382 12:fe8002389810 772 {
s1558382 12:fe8002389810 773 pc.printf("Claire is going to table spot nr. %i.\n\r",tableSpot);
s1558382 12:fe8002389810 774 }
s1558382 12:fe8002389810 775 else if(currentRepetitions == 200)
s1558382 12:fe8002389810 776 {
s1558382 12:fe8002389810 777 pc.printf("10\n\r");
s1558382 12:fe8002389810 778 }
s1558382 12:fe8002389810 779 else if(currentRepetitions == 400)
s1558382 12:fe8002389810 780 {
s1558382 12:fe8002389810 781 pc.printf("9\n\r");
s1558382 12:fe8002389810 782 }
s1558382 12:fe8002389810 783 else if(currentRepetitions == 600)
s1558382 12:fe8002389810 784 {
s1558382 12:fe8002389810 785 pc.printf("8\n\r");
s1558382 12:fe8002389810 786 }
s1558382 12:fe8002389810 787 else if(currentRepetitions == 800)
s1558382 12:fe8002389810 788 {
s1558382 12:fe8002389810 789 pc.printf("7\n\r");
s1558382 12:fe8002389810 790 }
s1558382 12:fe8002389810 791 else if(currentRepetitions == 1000)
s1558382 12:fe8002389810 792 {
s1558382 12:fe8002389810 793 pc.printf("6\n\r");
s1558382 12:fe8002389810 794 }
s1558382 12:fe8002389810 795 else if(currentRepetitions == 1200)
s1558382 12:fe8002389810 796 {
s1558382 12:fe8002389810 797 pc.printf("5\n\r");
s1558382 12:fe8002389810 798 }
s1558382 12:fe8002389810 799 else if(currentRepetitions == 1400)
s1558382 12:fe8002389810 800 {
s1558382 12:fe8002389810 801 pc.printf("4\n\r");
s1558382 12:fe8002389810 802 }
s1558382 12:fe8002389810 803 else if(currentRepetitions == 1600)
s1558382 12:fe8002389810 804 {
s1558382 12:fe8002389810 805 pc.printf("3\n\r");
s1558382 12:fe8002389810 806 }
s1558382 12:fe8002389810 807 else if(currentRepetitions == 1800)
s1558382 12:fe8002389810 808 {
s1558382 12:fe8002389810 809 pc.printf("2\n\r");
s1558382 12:fe8002389810 810 }
s1558382 12:fe8002389810 811 else if(currentRepetitions == 2000)
s1558382 12:fe8002389810 812 {
s1558382 12:fe8002389810 813 pc.printf("1\n\r");
s1558382 12:fe8002389810 814 }
s1558382 13:b4ed3eba926b 815 else if(currentRepetitions == 2050)
s1558382 12:fe8002389810 816 {
s1558382 12:fe8002389810 817 pc.printf("Claire is at table spot nr. %i. It will be cleaned now.\n\r",tableSpot);
s1558382 12:fe8002389810 818 }
s1558382 12:fe8002389810 819 }
s1558382 12:fe8002389810 820
s1558382 12:fe8002389810 821 else if(state == CLEANING)
s1558382 12:fe8002389810 822 {
s1558382 12:fe8002389810 823 if(currentRepetitions == 100)
s1558382 12:fe8002389810 824 {
s1558382 12:fe8002389810 825 pc.printf("Claire is cleaning table spot nr. %i. It will be done in\n\r.",tableSpot);
s1558382 12:fe8002389810 826 }
s1558382 12:fe8002389810 827 else if(currentRepetitions == 200)
s1558382 12:fe8002389810 828 {
s1558382 12:fe8002389810 829 pc.printf("5\n\r");
s1558382 12:fe8002389810 830 }
s1558382 12:fe8002389810 831 else if(currentRepetitions == 400)
s1558382 12:fe8002389810 832 {
s1558382 12:fe8002389810 833 pc.printf("4\n\r");
s1558382 12:fe8002389810 834 }
s1558382 12:fe8002389810 835 else if(currentRepetitions == 600)
s1558382 12:fe8002389810 836 {
s1558382 12:fe8002389810 837 pc.printf("3\n\r");
s1558382 12:fe8002389810 838 }
s1558382 12:fe8002389810 839 else if(currentRepetitions == 800)
s1558382 12:fe8002389810 840 {
s1558382 12:fe8002389810 841 pc.printf("2\n\r");
s1558382 12:fe8002389810 842 }
s1558382 12:fe8002389810 843 else if(currentRepetitions == 1000)
s1558382 12:fe8002389810 844 {
s1558382 12:fe8002389810 845 pc.printf("1\n\r");
s1558382 12:fe8002389810 846 }
s1558382 12:fe8002389810 847 }
s1558382 12:fe8002389810 848
s1558382 12:fe8002389810 849 else if(state == RETURN2REST)
s1558382 12:fe8002389810 850 {
s1558382 12:fe8002389810 851 if(currentRepetitions == 100)
s1558382 12:fe8002389810 852 {
s1558382 12:fe8002389810 853 pc.printf("Claire is going back to the start. Which table spot do you want to clean next?\r\n");
s1558382 12:fe8002389810 854 }
s1558382 12:fe8002389810 855 }
s1558382 0:62fe4a2a8101 856 }
s1558382 5:97977cb8daa3 857
s1558382 12:fe8002389810 858 //=========== MAIN ===========================================================
s1558382 1:43fad4d1dee0 859 int main()
s1558382 0:62fe4a2a8101 860 {
s1558382 1:43fad4d1dee0 861 pc.baud(115200);
s1558382 12:fe8002389810 862
s1558382 12:fe8002389810 863 bqc1.add(&bq1).add(&bq2);
s1558382 12:fe8002389810 864 bqc2.add(&bq3);
s1558382 12:fe8002389810 865
s1558382 12:fe8002389810 866 state = REST0;
s1558382 12:fe8002389810 867 stateTick.attach(&stateMachine,ts);
s1558382 12:fe8002389810 868
s1558382 12:fe8002389810 869 motor1MagnitudePin.period(1.0/1000.0);
s1558382 12:fe8002389810 870 motor2MagnitudePin.period(1.0/1000.0);
s1558382 12:fe8002389810 871 t1.attach(&fn1_activate, 0.0001f);
s1558382 12:fe8002389810 872 t2.attach(&fn2_activate, 0.0001f);
s1558382 12:fe8002389810 873 t3.attach(&fn3_activate, 0.0001f);
s1558382 12:fe8002389810 874 t4.attach(&fn4_activate, 0.0001f);
s1558382 12:fe8002389810 875 t5.attach(&fn5_activate, 0.0001f);
s1558382 12:fe8002389810 876 t6.attach(&fn6_activate, 0.0001f);
s1558382 12:fe8002389810 877
s1558382 12:fe8002389810 878 while(1)
s1558382 12:fe8002389810 879 {
s1558382 12:fe8002389810 880 screenUpdate();
s1558382 12:fe8002389810 881 wait(0.005);
s1558382 12:fe8002389810 882
s1558382 12:fe8002389810 883 if(fn1_go)
s1558382 12:fe8002389810 884 {
s1558382 12:fe8002389810 885 fn1_go = false;
s1558382 12:fe8002389810 886 getAngPosition_m1();
s1558382 12:fe8002389810 887 }
s1558382 12:fe8002389810 888
s1558382 12:fe8002389810 889 if(fn2_go)
s1558382 12:fe8002389810 890 {
s1558382 12:fe8002389810 891 fn2_go = false;
s1558382 12:fe8002389810 892 motor1_Controller(radians_m1);
s1558382 12:fe8002389810 893 }
s1558382 12:fe8002389810 894
s1558382 12:fe8002389810 895 if(fn3_go)
s1558382 12:fe8002389810 896 {
s1558382 12:fe8002389810 897 fn3_go = false;
s1558382 12:fe8002389810 898 control_m1(motor1,radians_m1);
s1558382 12:fe8002389810 899 }
s1558382 12:fe8002389810 900
s1558382 12:fe8002389810 901 if(fn4_go)
s1558382 12:fe8002389810 902 {
s1558382 12:fe8002389810 903 fn4_go = false;
s1558382 12:fe8002389810 904 getAngPosition_m2();
s1558382 12:fe8002389810 905 }
s1558382 12:fe8002389810 906
s1558382 12:fe8002389810 907 if(fn5_go)
s1558382 12:fe8002389810 908 {
s1558382 12:fe8002389810 909 fn5_go = false;
s1558382 12:fe8002389810 910 motor2_Controller(radians_m2);
s1558382 12:fe8002389810 911 }
s1558382 12:fe8002389810 912
s1558382 12:fe8002389810 913 if(fn6_go)
s1558382 12:fe8002389810 914 {
s1558382 12:fe8002389810 915 fn6_go = false;
s1558382 12:fe8002389810 916 control_m2(motor2,radians_m2);
s1558382 12:fe8002389810 917 }
s1558382 12:fe8002389810 918 }
s1558382 12:fe8002389810 919 }