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@21:1c7106689c96, 2016-11-02 (annotated)

Committer:
s1558382
Date:
Wed Nov 02 16:58:51 2016 +0000
Revision:
21:1c7106689c96
Parent:
20:3a5292c0f26a 
New motor values, and go flags of motor 3 very high.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1558382 18:5ddef43222ea 1 /*****************************************************************************
s1558382 18:5ddef43222ea 2 ** **
s1558382 18:5ddef43222ea 3 ** BIOROBOTICS 2016 GROUP 8 **
s1558382 18:5ddef43222ea 4 ** GROUP MEMBERS **
s1558382 18:5ddef43222ea 5 ** **
s1558382 18:5ddef43222ea 6 ** ANNELIES DE JONG s1609580 **
s1558382 18:5ddef43222ea 7 ** INGMAR LOOHUIS s1607359 **
s1558382 18:5ddef43222ea 8 ** ROBBERT VAN MIDDELAAR sjdjdjdj **
s1558382 18:5ddef43222ea 9 ** MARGREET MORSINK s1558382 **
s1558382 18:5ddef43222ea 10 ** KARLIJN NIJHUIS s1777777 **
s1558382 18:5ddef43222ea 11 ** **
s1558382 18:5ddef43222ea 12 ******************************************************************************/
s1558382 18:5ddef43222ea 13
s1558382 0:62fe4a2a8101 14 #include "mbed.h"
s1558382 12:fe8002389810 15 #include "BiQuad.h"
s1558382 1:43fad4d1dee0 16 #include "MODSERIAL.h"
s1558382 0:62fe4a2a8101 17 #include "QEI.h"
s1558382 0:62fe4a2a8101 18 #include "math.h"
s1558382 0:62fe4a2a8101 19
s1558382 12:fe8002389810 20 //=========== PORTS ==========================================================
s1558382 12:fe8002389810 21 AnalogIn emg1(A0); //right biceps
s1558382 12:fe8002389810 22 AnalogIn emg2(A1); //left biceps
s1558382 0:62fe4a2a8101 23 Serial pc(USBTX,USBRX);
s1558382 12:fe8002389810 24
s1558382 12:fe8002389810 25 PwmOut motor1MagnitudePin(D5);
s1558382 12:fe8002389810 26 PwmOut motor2MagnitudePin(D6);
s1558382 12:fe8002389810 27 DigitalOut motor1DirectionPin (D4);
s1558382 12:fe8002389810 28 DigitalOut motor2DirectionPin (D7);
s1558382 12:fe8002389810 29
s1558382 12:fe8002389810 30 QEI encoder_m1(D12,D13,NC,32);
s1558382 12:fe8002389810 31 QEI encoder_m2(D10,D11,NC,32);
s1558382 12:fe8002389810 32
s1558382 12:fe8002389810 33 DigitalIn button(D2);
s1558382 12:fe8002389810 34
s1558382 16:ea41e12f2484 35 //Motor 3
s1558382 16:ea41e12f2484 36 BusOut motor_out(D0, D1, D2, D3); // blue - pink - yellow - orange
s1558382 16:ea41e12f2484 37
s1558382 12:fe8002389810 38 //=========== OBJECTS USED ===================================================
s1558382 12:fe8002389810 39 // BiQuads
s1558382 12:fe8002389810 40 BiQuadChain bqc1;
s1558382 12:fe8002389810 41 BiQuadChain bqc2;
s1558382 12:fe8002389810 42 BiQuad bq1(0.98818943122,-1.59905034133, 0.98818943122, -1.59325743247, 0.98217188389); // notch
s1558382 12:fe8002389810 43 BiQuad bq2(0.80010174727,-1.60020349454, 0.80010174727, -1.55983681590, 0.64057017317); // high pass
s1558382 12:fe8002389810 44 //Take absolute value between high pass and low pass
s1558382 12:fe8002389810 45 BiQuad bq3(0.00035313141, 0.00070626282, 0.00035313141, -1.94614721828, 0.94755974392); // low pass
s1558382 12:fe8002389810 46
s1558382 12:fe8002389810 47 //States
s1558382 17:ff6ec6c50082 48 enum state_t {REST0, REST, CALIBRATION, COUNTING, CONFIRMATION, GO2TABLESPOT, READY2CLEAN, HANDDOWN, CLEANING, CLEANCONFIRM, HANDUP, RETURN2REST};
s1558382 12:fe8002389810 49 state_t state = REST0;
s1558382 12:fe8002389810 50
s1558382 12:fe8002389810 51 //Tickers
s1558382 12:fe8002389810 52 Ticker stateTick;
s1558382 12:fe8002389810 53 Ticker screenTick;
s1558382 12:fe8002389810 54 Ticker angPos1;
s1558382 16:ea41e12f2484 55 Ticker t0;
s1558382 12:fe8002389810 56 Ticker t1;
s1558382 12:fe8002389810 57 Ticker t2;
s1558382 12:fe8002389810 58 Ticker t3;
s1558382 12:fe8002389810 59 Ticker t4;
s1558382 12:fe8002389810 60 Ticker t5;
s1558382 12:fe8002389810 61 Ticker t6;
s1558382 16:ea41e12f2484 62 Ticker t7;
s1558382 12:fe8002389810 63
s1558382 12:fe8002389810 64 Timer stateTimer;
s1558382 12:fe8002389810 65
s1558382 12:fe8002389810 66 //Go Flags
s1558382 16:ea41e12f2484 67 volatile bool fn0_go = false;
s1558382 16:ea41e12f2484 68 void fn0_activate(){ fn0_go = true; }; //Activates the go−flag
s1558382 12:fe8002389810 69 volatile bool fn1_go = false;
s1558382 12:fe8002389810 70 void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
s1558382 12:fe8002389810 71 volatile bool fn2_go = false;
s1558382 12:fe8002389810 72 void fn2_activate(){ fn2_go = true; }; //Activates the go-flag
s1558382 12:fe8002389810 73 volatile bool fn3_go = false;
s1558382 12:fe8002389810 74 void fn3_activate(){ fn3_go = true; }; //Activates the go-flag
s1558382 12:fe8002389810 75 volatile bool fn4_go = false;
s1558382 12:fe8002389810 76 void fn4_activate(){ fn4_go = true; }; //Activates the go−flag
s1558382 12:fe8002389810 77 volatile bool fn5_go = false;
s1558382 12:fe8002389810 78 void fn5_activate(){ fn5_go = true; }; //Activates the go-flag
s1558382 12:fe8002389810 79 volatile bool fn6_go = false;
s1558382 12:fe8002389810 80 void fn6_activate(){ fn6_go = true; }; //Activates the go-flag
s1558382 16:ea41e12f2484 81 volatile bool fn7_go = false;
s1558382 16:ea41e12f2484 82 void fn7_activate(){ fn7_go = true; }; //Activates the go−flag
s1558382 19:e1c923e0d0a3 83 volatile bool state_go = false;
s1558382 19:e1c923e0d0a3 84 void state_activate(){ state_go = true; };
s1558382 20:3a5292c0f26a 85 volatile bool screen_go = false;
s1558382 20:3a5292c0f26a 86 void screen_activate(){ screen_go = true; };
s1558382 12:fe8002389810 87
s1558382 12:fe8002389810 88 //=========== CONSTANTS =====================================================
s1558382 12:fe8002389810 89 const float ts = 0.005; //Sample time 200 Hz
s1558382 12:fe8002389810 90 const double pi = 3.14159265359;
s1558382 16:ea41e12f2484 91 const double transmissionShoulder = 94.4/40.2;
s1558382 16:ea41e12f2484 92 const double transmissionElbow = 1.0;
s1558382 12:fe8002389810 93
s1558382 12:fe8002389810 94 //Controller constants
s1558382 12:fe8002389810 95 const double m1_Kp = 120.0, m1_Ki = 1.44876354368902, m1_Kd = -1.55261758822823, m1_N = 1.70578345077793;
s1558382 12:fe8002389810 96 const double m2_Kp = 120.0, m2_Ki = 1.44876354368902, m2_Kd = -1.55261758822823, m2_N = 1.70578345077793;
s1558382 12:fe8002389810 97 const double m1_Ts = 0.001; // Controller sample time motor 1
s1558382 12:fe8002389810 98 const double m2_Ts = 0.001; // Controller sample time motor 2
s1558382 12:fe8002389810 99
s1558382 12:fe8002389810 100 //Angles
s1558382 19:e1c923e0d0a3 101 double O1 = 1.7463;
s1558382 19:e1c923e0d0a3 102 double O2 = 2.2183;
s1558382 19:e1c923e0d0a3 103 double O3 = 2.1194;
s1558382 19:e1c923e0d0a3 104 double O4 = 1.5004;
s1558382 19:e1c923e0d0a3 105 double O5 = 2.2240;
s1558382 19:e1c923e0d0a3 106 double O6 = 0.5323;
s1558382 19:e1c923e0d0a3 107 double B1 = 1.5381;
s1558382 19:e1c923e0d0a3 108 double B2 = 0.7378;
s1558382 19:e1c923e0d0a3 109 double B3 = 0.0082;
s1558382 19:e1c923e0d0a3 110 double B4 = -0.1243;
s1558382 19:e1c923e0d0a3 111 double B5 = 2.0296;
s1558382 19:e1c923e0d0a3 112 double B6 = 1.7815;
s1558382 12:fe8002389810 113
s1558382 16:ea41e12f2484 114 //Motor 3
s1558382 16:ea41e12f2484 115 int up = 1;
s1558382 16:ea41e12f2484 116 int down = 0;
s1558382 16:ea41e12f2484 117
s1558382 12:fe8002389810 118 //=========== BOOLEANS ======================================================
s1558382 12:fe8002389810 119 bool contracted = false;
s1558382 12:fe8002389810 120 bool confirm = false;
s1558382 12:fe8002389810 121 bool calibration = false;
s1558382 12:fe8002389810 122 bool EMG = true;
s1558382 12:fe8002389810 123
s1558382 12:fe8002389810 124 //=========== VARIABLES =====================================================
s1558382 12:fe8002389810 125 //Filtered EMG Signal
s1558382 12:fe8002389810 126 double emgFilteredRight;
s1558382 12:fe8002389810 127 double emgFilteredLeft;
s1558382 12:fe8002389810 128
s1558382 12:fe8002389810 129 //Max values
s1558382 12:fe8002389810 130 double emgMaxRight;
s1558382 12:fe8002389810 131 double emgMaxLeft;
s1558382 12:fe8002389810 132
s1558382 12:fe8002389810 133 //Setting the thresholds
s1558382 12:fe8002389810 134 double emgThresholdRightLow;
s1558382 12:fe8002389810 135 double emgThresholdRightHigh;
s1558382 12:fe8002389810 136 double emgThresholdLeftLow;
s1558382 12:fe8002389810 137 double emgThresholdLeftHigh;
s1558382 12:fe8002389810 138
s1558382 12:fe8002389810 139 int emgCounts;
s1558382 12:fe8002389810 140 int tableSpot;
s1558382 12:fe8002389810 141
s1558382 12:fe8002389810 142 //Timing aspect
s1558382 12:fe8002389810 143 int currentRepetitions = 0;
s1558382 12:fe8002389810 144 int previousRepetitions = 0;
s1558382 12:fe8002389810 145
s1558382 16:ea41e12f2484 146 //Motor 3
s1558382 16:ea41e12f2484 147 int motor3_direction = up; // direction
s1558382 16:ea41e12f2484 148 int begin_step = 0;
s1558382 16:ea41e12f2484 149 int teller = 0;
s1558382 17:ff6ec6c50082 150 double m3_pos_ts = 0;
s1558382 17:ff6ec6c50082 151 double m3_clean_ts = 0;
s1558382 16:ea41e12f2484 152
s1558382 12:fe8002389810 153 //Motor variables
s1558382 12:fe8002389810 154 double m1_v1 = 0;
s1558382 12:fe8002389810 155 double m1_v2 = 0;
s1558382 12:fe8002389810 156 double m2_v1 = 0;
s1558382 12:fe8002389810 157 double m2_v2 = 0;
s1558382 12:fe8002389810 158
s1558382 12:fe8002389810 159 //Position variable
s1558382 12:fe8002389810 160 volatile double radians_m1;
s1558382 12:fe8002389810 161 volatile double radians_m2;
s1558382 12:fe8002389810 162
s1558382 12:fe8002389810 163 //Plant variable
s1558382 12:fe8002389810 164 volatile double motor1;
s1558382 12:fe8002389810 165 volatile double motor2;
s1558382 12:fe8002389810 166
s1558382 12:fe8002389810 167 //Reference positions
s1558382 12:fe8002389810 168 double reference_m1 = 0;
s1558382 12:fe8002389810 169 double reference_m2 = 0;
s1558382 12:fe8002389810 170
s1558382 12:fe8002389810 171 //=========== FUNCTIONS ======================================================
s1558382 12:fe8002389810 172
s1558382 12:fe8002389810 173 //=========== EMG ============================================================
s1558382 12:fe8002389810 174 void rest()
s1558382 12:fe8002389810 175 /*--------------------------------------------------
s1558382 12:fe8002389810 176 This function is for the resting situation.
s1558382 12:fe8002389810 177 --------------------------------------------------*/
s1558382 12:fe8002389810 178 {
s1558382 12:fe8002389810 179 currentRepetitions = previousRepetitions+1;
s1558382 12:fe8002389810 180 previousRepetitions = currentRepetitions;
s1558382 12:fe8002389810 181 }
s1558382 12:fe8002389810 182
s1558382 12:fe8002389810 183 void emgCount()
s1558382 12:fe8002389810 184 /*--------------------------------------------------
s1558382 12:fe8002389810 185 This function counts the contractions of the right
s1558382 12:fe8002389810 186 biceps.
s1558382 12:fe8002389810 187 --------------------------------------------------*/
s1558382 12:fe8002389810 188 {
s1558382 12:fe8002389810 189 if(emgFilteredRight >= emgThresholdRightHigh)
s1558382 12:fe8002389810 190 {
s1558382 12:fe8002389810 191 contracted = true;
s1558382 12:fe8002389810 192 }
s1558382 12:fe8002389810 193
s1558382 12:fe8002389810 194 if(contracted == true && emgFilteredRight <= emgThresholdRightLow)
s1558382 12:fe8002389810 195 {
s1558382 12:fe8002389810 196 contracted = false;
s1558382 12:fe8002389810 197 emgCounts++;
s1558382 12:fe8002389810 198 }
s1558382 12:fe8002389810 199 }
s1558382 12:fe8002389810 200
s1558382 12:fe8002389810 201 void emgConfirm()
s1558382 12:fe8002389810 202 /*--------------------------------------------------
s1558382 12:fe8002389810 203 This function confirms the EMG counts via contracting
s1558382 12:fe8002389810 204 the left biceps.
s1558382 12:fe8002389810 205 --------------------------------------------------*/
s1558382 12:fe8002389810 206 {
s1558382 17:ff6ec6c50082 207 confirm = false;
s1558382 17:ff6ec6c50082 208
s1558382 12:fe8002389810 209 if(emgFilteredLeft >= emgThresholdLeftHigh)
s1558382 12:fe8002389810 210 {
s1558382 12:fe8002389810 211 contracted = true;
s1558382 12:fe8002389810 212 }
s1558382 12:fe8002389810 213
s1558382 12:fe8002389810 214 if(contracted == true && emgFilteredLeft <= emgThresholdLeftLow)
s1558382 12:fe8002389810 215 {
s1558382 12:fe8002389810 216 contracted = false;
s1558382 12:fe8002389810 217 confirm = true;
s1558382 12:fe8002389810 218 }
s1558382 12:fe8002389810 219 }
s1558382 2:29d7391d7bc5 220
s1558382 12:fe8002389810 221 void emgFilterRight()
s1558382 12:fe8002389810 222 /*--------------------------------------------------
s1558382 12:fe8002389810 223 This function filters and samples the EMG signal
s1558382 12:fe8002389810 224 from the right biceps.
s1558382 12:fe8002389810 225 --------------------------------------------------*/
s1558382 12:fe8002389810 226 {
s1558382 12:fe8002389810 227 double emgValueRight = emg1.read();
s1558382 12:fe8002389810 228 double emgFilter1Right = bqc1.step(emgValueRight);
s1558382 12:fe8002389810 229 double emgAbsRight = fabs(emgFilter1Right);
s1558382 12:fe8002389810 230
s1558382 12:fe8002389810 231 emgFilteredRight = bqc2.step(emgAbsRight);
s1558382 12:fe8002389810 232
s1558382 12:fe8002389810 233 if(state != CALIBRATION)
s1558382 12:fe8002389810 234 {
s1558382 12:fe8002389810 235 emgCount();
s1558382 12:fe8002389810 236 }
s1558382 12:fe8002389810 237 }
s1558382 12:fe8002389810 238
s1558382 12:fe8002389810 239 void emgFilterLeft()
s1558382 12:fe8002389810 240 /*--------------------------------------------------
s1558382 12:fe8002389810 241 This function filters and samples the EMG signal
s1558382 12:fe8002389810 242 from the left biceps.
s1558382 12:fe8002389810 243 --------------------------------------------------*/
s1558382 12:fe8002389810 244 {
s1558382 12:fe8002389810 245 double emgValueLeft = emg2.read();
s1558382 12:fe8002389810 246 double emgFilter1Left = bqc1.step(emgValueLeft);
s1558382 12:fe8002389810 247 double emgAbsLeft = fabs(emgFilter1Left);
s1558382 12:fe8002389810 248
s1558382 12:fe8002389810 249 emgFilteredLeft = bqc2.step(emgAbsLeft);
s1558382 12:fe8002389810 250
s1558382 17:ff6ec6c50082 251 if(state == CONFIRMATION || READY2CLEAN || CLEANCONFIRM)
s1558382 12:fe8002389810 252 {
s1558382 12:fe8002389810 253 emgConfirm();
s1558382 12:fe8002389810 254 }
s1558382 12:fe8002389810 255 }
s1558382 7:5b14dbb9e6d1 256
s1558382 12:fe8002389810 257 void emgMaxValueRight()
s1558382 12:fe8002389810 258 /*--------------------------------------------------
s1558382 12:fe8002389810 259 This function measures the maximal EMG value to set
s1558382 12:fe8002389810 260 thresholds for the right biceps.
s1558382 12:fe8002389810 261 It is used for calibration.
s1558382 12:fe8002389810 262 --------------------------------------------------*/
s1558382 12:fe8002389810 263 {
s1558382 12:fe8002389810 264 emgFilterRight();
s1558382 12:fe8002389810 265 if(emgFilteredRight >= emgMaxRight)
s1558382 12:fe8002389810 266 {
s1558382 12:fe8002389810 267 emgMaxRight = emgFilteredRight;
s1558382 12:fe8002389810 268 }
s1558382 12:fe8002389810 269
s1558382 12:fe8002389810 270 //Setting the thresholds
s1558382 12:fe8002389810 271 emgThresholdRightLow = 0.4*emgMaxRight;
s1558382 12:fe8002389810 272 emgThresholdRightHigh = 0.7*emgMaxRight;
s1558382 12:fe8002389810 273 }
s1558382 12:fe8002389810 274
s1558382 12:fe8002389810 275 void emgMaxValueLeft()
s1558382 12:fe8002389810 276 /*--------------------------------------------------
s1558382 12:fe8002389810 277 This function measures the maximal EMG value to set
s1558382 12:fe8002389810 278 thresholds for the left biceps.
s1558382 12:fe8002389810 279 It is used for calibration.
s1558382 12:fe8002389810 280 --------------------------------------------------*/
s1558382 12:fe8002389810 281 {
s1558382 12:fe8002389810 282 emgFilterLeft();
s1558382 12:fe8002389810 283 if(emgFilteredLeft >= emgMaxLeft)
s1558382 12:fe8002389810 284 {
s1558382 12:fe8002389810 285 emgMaxLeft = emgFilteredLeft;
s1558382 12:fe8002389810 286 }
s1558382 12:fe8002389810 287
s1558382 12:fe8002389810 288 //Setting the thresholds
s1558382 12:fe8002389810 289 emgThresholdLeftLow = 0.4*emgMaxLeft;
s1558382 12:fe8002389810 290 emgThresholdLeftHigh = 0.7*emgMaxLeft;
s1558382 12:fe8002389810 291 }
s1558382 12:fe8002389810 292
s1558382 12:fe8002389810 293 void emgCalibration()
s1558382 12:fe8002389810 294 /*--------------------------------------------------
s1558382 12:fe8002389810 295 This function calibrates the EMG signal from the
s1558382 12:fe8002389810 296 right biceps.
s1558382 12:fe8002389810 297 --------------------------------------------------*/
s1558382 0:62fe4a2a8101 298 {
s1558382 12:fe8002389810 299 currentRepetitions = previousRepetitions+1;
s1558382 12:fe8002389810 300 previousRepetitions = currentRepetitions;
s1558382 12:fe8002389810 301
s1558382 12:fe8002389810 302 if(currentRepetitions <= 1000)
s1558382 12:fe8002389810 303 {
s1558382 12:fe8002389810 304 emgMaxValueRight();
s1558382 12:fe8002389810 305 }
s1558382 12:fe8002389810 306 else if(1000 <= currentRepetitions && currentRepetitions <= 2000)
s1558382 12:fe8002389810 307 {
s1558382 12:fe8002389810 308 emgMaxValueLeft();
s1558382 12:fe8002389810 309 }
s1558382 12:fe8002389810 310 else if(currentRepetitions >= 2000)
s1558382 12:fe8002389810 311 {
s1558382 12:fe8002389810 312 calibration = true;
s1558382 12:fe8002389810 313 }
s1558382 12:fe8002389810 314 }
s1558382 12:fe8002389810 315
s1558382 12:fe8002389810 316 void getTableSpot()
s1558382 12:fe8002389810 317 /*--------------------------------------------------
s1558382 12:fe8002389810 318 This function determines to which spot on the table
s1558382 12:fe8002389810 319 the robot arm should move.
s1558382 12:fe8002389810 320 --------------------------------------------------*/
s1558382 12:fe8002389810 321 {
s1558382 12:fe8002389810 322 currentRepetitions = previousRepetitions+1;
s1558382 12:fe8002389810 323 previousRepetitions = currentRepetitions;
s1558382 12:fe8002389810 324
s1558382 12:fe8002389810 325 if(200 <= currentRepetitions && currentRepetitions < 2000)
s1558382 1:43fad4d1dee0 326 {
s1558382 12:fe8002389810 327 emgFilterRight();
s1558382 12:fe8002389810 328 tableSpot = emgCounts;
s1558382 12:fe8002389810 329 }
s1558382 12:fe8002389810 330 }
s1558382 12:fe8002389810 331
s1558382 12:fe8002389810 332 //=========== MOTOR ==========================================================
s1558382 12:fe8002389810 333 double PID1( double err, const double Kp, const double Ki, const double Kd,
s1558382 12:fe8002389810 334 const double Ts, const double N, double &v1, double &v2 )
s1558382 12:fe8002389810 335 /*--------------------------------------------------
s1558382 12:fe8002389810 336 This function is a PID controller for motor 1.
s1558382 12:fe8002389810 337 --------------------------------------------------*/
s1558382 12:fe8002389810 338 {
s1558382 12:fe8002389810 339 // These variables are only calculated once!
s1558382 12:fe8002389810 340 const double a1 = (-4.0/(N*Ts+2));
s1558382 12:fe8002389810 341 const double a2 = -(N*Ts-2)/(N*Ts+2);
s1558382 12:fe8002389810 342 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 343 const double b1 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
s1558382 12:fe8002389810 344 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 345
s1558382 12:fe8002389810 346 double v = err - a1*v1 - a2*v2;
s1558382 12:fe8002389810 347 double u = b0*v + b1*v1 + b2*v2;
s1558382 12:fe8002389810 348 v2 = v1; v1 = v;
s1558382 12:fe8002389810 349 return u;
s1558382 12:fe8002389810 350 }
s1558382 12:fe8002389810 351
s1558382 12:fe8002389810 352 double PID2( double err, const double Kp, const double Ki, const double Kd,
s1558382 12:fe8002389810 353 const double Ts, const double N, double &v1_2, double &v2_2 )
s1558382 12:fe8002389810 354 /*--------------------------------------------------
s1558382 12:fe8002389810 355 This function is a PID controller for motor 1.
s1558382 12:fe8002389810 356 --------------------------------------------------*/
s1558382 12:fe8002389810 357 {
s1558382 12:fe8002389810 358 // These variables are only calculated once!
s1558382 12:fe8002389810 359 const double a1 = (-4.0/(N*Ts+2));
s1558382 12:fe8002389810 360 const double a2 = -(N*Ts-2)/(N*Ts+2);
s1558382 12:fe8002389810 361 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 362 const double b1 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
s1558382 12:fe8002389810 363 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 364
s1558382 12:fe8002389810 365 double v = err - a1*v1_2 - a2*v2_2;
s1558382 12:fe8002389810 366 double u = b0*v + b1*v1_2 + b2*v2_2;
s1558382 12:fe8002389810 367 v2_2 = v1_2; v1_2 = v;
s1558382 12:fe8002389810 368 return u;
s1558382 12:fe8002389810 369 }
s1558382 12:fe8002389810 370
s1558382 12:fe8002389810 371 void getAngPosition_m1()
s1558382 12:fe8002389810 372 /*--------------------------------------------------
s1558382 12:fe8002389810 373 This function gets the angular position of motor 1.
s1558382 12:fe8002389810 374 --------------------------------------------------*/
s1558382 12:fe8002389810 375 {
s1558382 12:fe8002389810 376 volatile int pulses_m1 = encoder_m1.getPulses();
s1558382 12:fe8002389810 377 radians_m1 = (pulses_m1*0.002991*0.5); //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
s1558382 12:fe8002389810 378 }
s1558382 12:fe8002389810 379
s1558382 12:fe8002389810 380 void getAngPosition_m2()
s1558382 12:fe8002389810 381 /*--------------------------------------------------
s1558382 12:fe8002389810 382 This function gets the angular position of motor 2.
s1558382 12:fe8002389810 383 --------------------------------------------------*/
s1558382 12:fe8002389810 384 {
s1558382 12:fe8002389810 385 volatile int pulses_m2 = encoder_m2.getPulses();
s1558382 12:fe8002389810 386 radians_m2 = (pulses_m2*0.002991*0.5); //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
s1558382 12:fe8002389810 387 }
s1558382 12:fe8002389810 388
s1558382 12:fe8002389810 389 void motor1_Controller(double radians_m1)
s1558382 12:fe8002389810 390 /*--------------------------------------------------
s1558382 12:fe8002389810 391 This function measures the error and applies it to
s1558382 12:fe8002389810 392 the output to the plant.
s1558382 12:fe8002389810 393 --------------------------------------------------*/
s1558382 12:fe8002389810 394 {
s1558382 12:fe8002389810 395 volatile double error_m1 = reference_m1 - radians_m1;
s1558382 12:fe8002389810 396 motor1 = PID1( error_m1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, m1_v1, m1_v2 );
s1558382 12:fe8002389810 397 }
s1558382 12:fe8002389810 398
s1558382 12:fe8002389810 399 // Next task, measure the error and apply the output to the plant
s1558382 12:fe8002389810 400 void motor2_Controller(double radians_m2)
s1558382 12:fe8002389810 401 /*--------------------------------------------------
s1558382 12:fe8002389810 402 This function measures the error and applies it to
s1558382 12:fe8002389810 403 the output to the plant.
s1558382 12:fe8002389810 404 --------------------------------------------------*/
s1558382 12:fe8002389810 405 {
s1558382 12:fe8002389810 406 volatile double error_m2 = reference_m2 - radians_m2;
s1558382 12:fe8002389810 407 motor2 = PID1( error_m2,m2_Kp,m2_Ki,m2_Kd,m2_Ts, m2_N, m2_v1, m2_v2 );
s1558382 12:fe8002389810 408
s1558382 13:b4ed3eba926b 409 // pc.printf("motor2 = %d",reference_m2);
s1558382 12:fe8002389810 410 }
s1558382 12:fe8002389810 411
s1558382 13:b4ed3eba926b 412 void control_m1(double motor1, double radians_m1)
s1558382 12:fe8002389810 413 /*--------------------------------------------------
s1558382 12:fe8002389810 414 This function controls motor 1.
s1558382 12:fe8002389810 415 --------------------------------------------------*/
s1558382 12:fe8002389810 416 {
s1558382 12:fe8002389810 417 if(abs(motor1) > 1.0)
s1558382 12:fe8002389810 418 {
s1558382 21:1c7106689c96 419 motor1MagnitudePin=0.6;//MOET NOG TUSSENWAAREN KRIJGEN
s1558382 12:fe8002389810 420 }
s1558382 1:43fad4d1dee0 421 else
s1558382 1:43fad4d1dee0 422 {
s1558382 12:fe8002389810 423 motor1MagnitudePin=0.0;
s1558382 12:fe8002389810 424 }
s1558382 12:fe8002389810 425
s1558382 12:fe8002389810 426 if(motor1 <= 0)
s1558382 12:fe8002389810 427 {
s1558382 12:fe8002389810 428 motor1DirectionPin=0.0;
s1558382 12:fe8002389810 429 }
s1558382 12:fe8002389810 430 else
s1558382 12:fe8002389810 431 {
s1558382 12:fe8002389810 432 motor1DirectionPin=1.0;
s1558382 21:1c7106689c96 433 }
s1558382 0:62fe4a2a8101 434 }
s1558382 12:fe8002389810 435
s1558382 13:b4ed3eba926b 436 void control_m2(double motor2, double radians_m2)
s1558382 12:fe8002389810 437 /*--------------------------------------------------
s1558382 12:fe8002389810 438 This function controls motor 2.
s1558382 12:fe8002389810 439 --------------------------------------------------*/
s1558382 2:29d7391d7bc5 440 {
s1558382 12:fe8002389810 441 if(abs(motor2) > 1)
s1558382 1:43fad4d1dee0 442 {
s1558382 21:1c7106689c96 443 motor2MagnitudePin=0.5;///MOET NOG TUSSENWAAREN KRIJGEN
s1558382 1:43fad4d1dee0 444 }
s1558382 1:43fad4d1dee0 445 else
s1558382 1:43fad4d1dee0 446 {
s1558382 12:fe8002389810 447 motor2MagnitudePin=0.0;
s1558382 12:fe8002389810 448 }
s1558382 12:fe8002389810 449
s1558382 12:fe8002389810 450 if(motor2 <= 0)
s1558382 12:fe8002389810 451 {
s1558382 12:fe8002389810 452 motor2DirectionPin=1.0;
s1558382 12:fe8002389810 453 }
s1558382 12:fe8002389810 454 else
s1558382 12:fe8002389810 455 {
s1558382 12:fe8002389810 456 motor2DirectionPin=0.0;
s1558382 12:fe8002389810 457 }
s1558382 12:fe8002389810 458 }
s1558382 12:fe8002389810 459
s1558382 16:ea41e12f2484 460 void motor3_position()
s1558382 16:ea41e12f2484 461 {
s1558382 16:ea41e12f2484 462 /*--------------------------------------------------
s1558382 16:ea41e12f2484 463 This function controls motor 3 to go up and down.
s1558382 16:ea41e12f2484 464 --------------------------------------------------*/
s1558382 16:ea41e12f2484 465 switch(begin_step)
s1558382 16:ea41e12f2484 466 {
s1558382 16:ea41e12f2484 467 case 0: motor_out = 0x1; break; // 0001
s1558382 16:ea41e12f2484 468 case 1: motor_out = 0x3; break; // 0011
s1558382 16:ea41e12f2484 469 case 2: motor_out = 0x2; break; // 0010
s1558382 16:ea41e12f2484 470 case 3: motor_out = 0x6; break; // 0110
s1558382 16:ea41e12f2484 471 case 4: motor_out = 0x4; break; // 0100
s1558382 16:ea41e12f2484 472 case 5: motor_out = 0xC; break; // 1100
s1558382 16:ea41e12f2484 473 case 6: motor_out = 0x8; break; // 1000
s1558382 16:ea41e12f2484 474 case 7: motor_out = 0x9; break; // 1001
s1558382 16:ea41e12f2484 475
s1558382 16:ea41e12f2484 476 default: motor_out = 0x0; break; // 0000
s1558382 16:ea41e12f2484 477 }
s1558382 16:ea41e12f2484 478
s1558382 17:ff6ec6c50082 479 if(motor3_direction == up)
s1558382 16:ea41e12f2484 480 {
s1558382 16:ea41e12f2484 481 begin_step++;
s1558382 16:ea41e12f2484 482 teller++;
s1558382 16:ea41e12f2484 483 }
s1558382 16:ea41e12f2484 484 else
s1558382 16:ea41e12f2484 485 {
s1558382 16:ea41e12f2484 486 begin_step--;
s1558382 16:ea41e12f2484 487 teller--;
s1558382 16:ea41e12f2484 488 }
s1558382 16:ea41e12f2484 489
s1558382 16:ea41e12f2484 490 if(begin_step>7)begin_step=0;
s1558382 16:ea41e12f2484 491
s1558382 16:ea41e12f2484 492 if(begin_step<0)begin_step=7;
s1558382 17:ff6ec6c50082 493
s1558382 17:ff6ec6c50082 494 /*
s1558382 17:ff6ec6c50082 495 if(teller <= -100000)
s1558382 17:ff6ec6c50082 496 {
s1558382 17:ff6ec6c50082 497 m3_pos_ts = 0;
s1558382 17:ff6ec6c50082 498 }
s1558382 16:ea41e12f2484 499
s1558382 17:ff6ec6c50082 500 if(teller => 0)
s1558382 17:ff6ec6c50082 501 {
s1558382 17:ff6ec6c50082 502 m3_pos_ts = 0;
s1558382 17:ff6ec6c50082 503 }
s1558382 17:ff6ec6c50082 504 */
s1558382 16:ea41e12f2484 505 }
s1558382 17:ff6ec6c50082 506
s1558382 16:ea41e12f2484 507 void motor3_clean()
s1558382 16:ea41e12f2484 508 /*--------------------------------------------------
s1558382 16:ea41e12f2484 509 This function controls motor 3, makes it go back \
s1558382 16:ea41e12f2484 510 and forth.
s1558382 16:ea41e12f2484 511 --------------------------------------------------*/
s1558382 16:ea41e12f2484 512 {
s1558382 16:ea41e12f2484 513 switch(begin_step)
s1558382 16:ea41e12f2484 514 {
s1558382 16:ea41e12f2484 515 case 0: motor_out = 0x1; break; // 0001
s1558382 16:ea41e12f2484 516 case 1: motor_out = 0x3; break; // 0011
s1558382 16:ea41e12f2484 517 case 2: motor_out = 0x2; break; // 0010
s1558382 16:ea41e12f2484 518 case 3: motor_out = 0x6; break; // 0110
s1558382 16:ea41e12f2484 519 case 4: motor_out = 0x4; break; // 0100
s1558382 16:ea41e12f2484 520 case 5: motor_out = 0xC; break; // 1100
s1558382 16:ea41e12f2484 521 case 6: motor_out = 0x8; break; // 1000
s1558382 16:ea41e12f2484 522 case 7: motor_out = 0x9; break; // 1001
s1558382 16:ea41e12f2484 523
s1558382 16:ea41e12f2484 524 default: motor_out = 0x0; break; // 0000
s1558382 16:ea41e12f2484 525 }
s1558382 16:ea41e12f2484 526
s1558382 16:ea41e12f2484 527 if(motor3_direction==1)
s1558382 16:ea41e12f2484 528 {
s1558382 16:ea41e12f2484 529 begin_step++;
s1558382 16:ea41e12f2484 530 teller++;
s1558382 16:ea41e12f2484 531 }
s1558382 16:ea41e12f2484 532
s1558382 16:ea41e12f2484 533 else
s1558382 16:ea41e12f2484 534 {
s1558382 16:ea41e12f2484 535 begin_step--;
s1558382 16:ea41e12f2484 536 teller--;
s1558382 16:ea41e12f2484 537 }
s1558382 16:ea41e12f2484 538
s1558382 16:ea41e12f2484 539 if(begin_step>7)begin_step=0;
s1558382 16:ea41e12f2484 540
s1558382 19:e1c923e0d0a3 541 if(begin_step<0)begin_step=7;
s1558382 16:ea41e12f2484 542
s1558382 19:e1c923e0d0a3 543 if(teller <= -11000)
s1558382 16:ea41e12f2484 544 {
s1558382 17:ff6ec6c50082 545 motor3_direction = up;
s1558382 16:ea41e12f2484 546 }
s1558382 16:ea41e12f2484 547
s1558382 19:e1c923e0d0a3 548 if(teller >= -9000)
s1558382 16:ea41e12f2484 549 {
s1558382 17:ff6ec6c50082 550 motor3_direction = down;
s1558382 16:ea41e12f2484 551 }
s1558382 16:ea41e12f2484 552 }
s1558382 16:ea41e12f2484 553
s1558382 12:fe8002389810 554 //=========== STATE ==========================================================
s1558382 12:fe8002389810 555 void stateMachine()
s1558382 12:fe8002389810 556 /*--------------------------------------------------
s1558382 12:fe8002389810 557 This function makes sure different functions are
s1558382 12:fe8002389810 558 done in a certain order.
s1558382 12:fe8002389810 559 --------------------------------------------------*/
s1558382 12:fe8002389810 560 {
s1558382 12:fe8002389810 561 stateTimer.reset();
s1558382 12:fe8002389810 562 switch(state)
s1558382 12:fe8002389810 563 {
s1558382 12:fe8002389810 564 case REST0:
s1558382 12:fe8002389810 565 {
s1558382 12:fe8002389810 566 pc.printf("Hi I am Claire!\r\n");
s1558382 12:fe8002389810 567 state = REST;
s1558382 12:fe8002389810 568 }
s1558382 12:fe8002389810 569 break;
s1558382 12:fe8002389810 570
s1558382 12:fe8002389810 571 case REST:
s1558382 12:fe8002389810 572 {
s1558382 12:fe8002389810 573 rest();
s1558382 12:fe8002389810 574 if(currentRepetitions >= 1000 && EMG == 1)
s1558382 12:fe8002389810 575 {
s1558382 12:fe8002389810 576 currentRepetitions = 0;
s1558382 12:fe8002389810 577 previousRepetitions = 0;
s1558382 12:fe8002389810 578 state = CALIBRATION;
s1558382 12:fe8002389810 579 }
s1558382 12:fe8002389810 580 else if(currentRepetitions >= 1000)
s1558382 12:fe8002389810 581 {
s1558382 12:fe8002389810 582 currentRepetitions = 0;
s1558382 12:fe8002389810 583 previousRepetitions = 0;
s1558382 12:fe8002389810 584 emgCounts = 0;
s1558382 12:fe8002389810 585 state = COUNTING;
s1558382 12:fe8002389810 586 }
s1558382 12:fe8002389810 587 }
s1558382 12:fe8002389810 588 break;
s1558382 12:fe8002389810 589
s1558382 12:fe8002389810 590 case CALIBRATION:
s1558382 12:fe8002389810 591 {
s1558382 12:fe8002389810 592 emgCalibration();
s1558382 12:fe8002389810 593 if(currentRepetitions >=2200)
s1558382 12:fe8002389810 594 {
s1558382 12:fe8002389810 595 currentRepetitions = 0;
s1558382 12:fe8002389810 596 previousRepetitions = 0;
s1558382 12:fe8002389810 597 state = COUNTING;
s1558382 12:fe8002389810 598 EMG = 0;
s1558382 12:fe8002389810 599 }
s1558382 12:fe8002389810 600 }
s1558382 12:fe8002389810 601 break;
s1558382 12:fe8002389810 602
s1558382 12:fe8002389810 603 case COUNTING:
s1558382 12:fe8002389810 604 {
s1558382 12:fe8002389810 605 getTableSpot();
s1558382 12:fe8002389810 606 if(currentRepetitions >= 2200)
s1558382 12:fe8002389810 607 {
s1558382 12:fe8002389810 608 currentRepetitions = 0;
s1558382 12:fe8002389810 609 previousRepetitions = 0;
s1558382 12:fe8002389810 610 state = CONFIRMATION;
s1558382 12:fe8002389810 611 }
s1558382 12:fe8002389810 612 }
s1558382 12:fe8002389810 613 break;
s1558382 12:fe8002389810 614
s1558382 12:fe8002389810 615 case CONFIRMATION:
s1558382 12:fe8002389810 616 {
s1558382 12:fe8002389810 617 rest(); //For counting purposes
s1558382 12:fe8002389810 618 emgFilterLeft();
s1558382 12:fe8002389810 619 if(confirm == true && currentRepetitions >= 1200)
s1558382 12:fe8002389810 620 {
s1558382 12:fe8002389810 621 currentRepetitions = 0;
s1558382 12:fe8002389810 622 previousRepetitions = 0;
s1558382 12:fe8002389810 623 state = GO2TABLESPOT;
s1558382 12:fe8002389810 624 }
s1558382 12:fe8002389810 625 else if(currentRepetitions >= 1200)
s1558382 12:fe8002389810 626 {
s1558382 12:fe8002389810 627 currentRepetitions = 0;
s1558382 12:fe8002389810 628 previousRepetitions = 0;
s1558382 12:fe8002389810 629 state = REST;
s1558382 12:fe8002389810 630 }
s1558382 12:fe8002389810 631 }
s1558382 12:fe8002389810 632 break;
s1558382 12:fe8002389810 633
s1558382 12:fe8002389810 634 case GO2TABLESPOT:
s1558382 12:fe8002389810 635 {
s1558382 12:fe8002389810 636 rest(); //For counting purposes
s1558382 12:fe8002389810 637 if(currentRepetitions < 2200) //Tijd = 10 secondes, misschien anders.
s1558382 12:fe8002389810 638 if(tableSpot == 1)
s1558382 12:fe8002389810 639 {
s1558382 16:ea41e12f2484 640 reference_m1 = O1*transmissionElbow;
s1558382 16:ea41e12f2484 641 reference_m2 = B1*transmissionShoulder;
s1558382 12:fe8002389810 642 }
s1558382 12:fe8002389810 643
s1558382 12:fe8002389810 644 else if(tableSpot == 2)
s1558382 12:fe8002389810 645 {
s1558382 16:ea41e12f2484 646 reference_m1 = O2*transmissionElbow;
s1558382 16:ea41e12f2484 647 reference_m2 = B2*transmissionShoulder;
s1558382 12:fe8002389810 648 }
s1558382 12:fe8002389810 649
s1558382 12:fe8002389810 650 else if(tableSpot == 3)
s1558382 12:fe8002389810 651 {
s1558382 16:ea41e12f2484 652 reference_m1 = O3*transmissionElbow;
s1558382 16:ea41e12f2484 653 reference_m2 = B3*transmissionShoulder;
s1558382 12:fe8002389810 654 }
s1558382 12:fe8002389810 655
s1558382 12:fe8002389810 656 else if(tableSpot == 4)
s1558382 12:fe8002389810 657 {
s1558382 16:ea41e12f2484 658 reference_m1 = O4*transmissionElbow;
s1558382 16:ea41e12f2484 659 reference_m2 = B4*transmissionShoulder;
s1558382 12:fe8002389810 660 }
s1558382 12:fe8002389810 661
s1558382 12:fe8002389810 662 else if(tableSpot == 5)
s1558382 12:fe8002389810 663 {
s1558382 16:ea41e12f2484 664 reference_m1 = O5*transmissionElbow;
s1558382 16:ea41e12f2484 665 reference_m2 = B5*transmissionShoulder;
s1558382 12:fe8002389810 666 }
s1558382 12:fe8002389810 667
s1558382 12:fe8002389810 668 else if(tableSpot == 6)
s1558382 12:fe8002389810 669 {
s1558382 16:ea41e12f2484 670 reference_m1 = O6*transmissionElbow;
s1558382 16:ea41e12f2484 671 reference_m2 = B6*transmissionShoulder;
s1558382 12:fe8002389810 672 }
s1558382 14:08c982d2bb2a 673
s1558382 14:08c982d2bb2a 674 else
s1558382 14:08c982d2bb2a 675 {
s1558382 14:08c982d2bb2a 676 if(currentRepetitions == 200)
s1558382 14:08c982d2bb2a 677 {
s1558382 14:08c982d2bb2a 678 currentRepetitions = 0;
s1558382 14:08c982d2bb2a 679 previousRepetitions = 0;
s1558382 15:48f4d8310b38 680 tableSpot = 0;
s1558382 14:08c982d2bb2a 681 state = COUNTING;
s1558382 14:08c982d2bb2a 682 }
s1558382 14:08c982d2bb2a 683 }
s1558382 12:fe8002389810 684
s1558382 15:48f4d8310b38 685 if(currentRepetitions >= 2200 && tableSpot >= 1 && tableSpot <= 6 )
s1558382 12:fe8002389810 686 {
s1558382 12:fe8002389810 687 currentRepetitions = 0;
s1558382 12:fe8002389810 688 previousRepetitions = 0;
s1558382 12:fe8002389810 689 state = CLEANING;
s1558382 12:fe8002389810 690 }
s1558382 12:fe8002389810 691 }
s1558382 12:fe8002389810 692 break;
s1558382 12:fe8002389810 693
s1558382 17:ff6ec6c50082 694 case READY2CLEAN:
s1558382 12:fe8002389810 695 {
s1558382 12:fe8002389810 696 rest(); //For counting purposes
s1558382 17:ff6ec6c50082 697 emgFilterLeft();
s1558382 17:ff6ec6c50082 698
s1558382 17:ff6ec6c50082 699 if(confirm == true && currentRepetitions >= 1200)
s1558382 17:ff6ec6c50082 700 {
s1558382 17:ff6ec6c50082 701 currentRepetitions = 0;
s1558382 17:ff6ec6c50082 702 previousRepetitions = 0;
s1558382 17:ff6ec6c50082 703 state = HANDDOWN;
s1558382 17:ff6ec6c50082 704 }
s1558382 17:ff6ec6c50082 705 else if(currentRepetitions >= 1200)
s1558382 12:fe8002389810 706 {
s1558382 12:fe8002389810 707 currentRepetitions = 0;
s1558382 12:fe8002389810 708 previousRepetitions = 0;
s1558382 12:fe8002389810 709 state = RETURN2REST;
s1558382 12:fe8002389810 710 }
s1558382 12:fe8002389810 711 }
s1558382 12:fe8002389810 712 break;
s1558382 12:fe8002389810 713
s1558382 17:ff6ec6c50082 714 case HANDDOWN:
s1558382 17:ff6ec6c50082 715 {
s1558382 17:ff6ec6c50082 716 m3_pos_ts = 0.0014;
s1558382 17:ff6ec6c50082 717 motor3_direction = down;
s1558382 17:ff6ec6c50082 718
s1558382 17:ff6ec6c50082 719 if(teller == -10000)
s1558382 17:ff6ec6c50082 720 {
s1558382 21:1c7106689c96 721 m3_pos_ts = 100000;
s1558382 17:ff6ec6c50082 722 state = CLEANING;
s1558382 17:ff6ec6c50082 723 }
s1558382 17:ff6ec6c50082 724 }
s1558382 17:ff6ec6c50082 725 break;
s1558382 17:ff6ec6c50082 726
s1558382 17:ff6ec6c50082 727 case CLEANING:
s1558382 17:ff6ec6c50082 728 {
s1558382 17:ff6ec6c50082 729 rest(); //For counting purposes
s1558382 17:ff6ec6c50082 730 m3_clean_ts = 0.0014;
s1558382 17:ff6ec6c50082 731 if(currentRepetitions >= 100)
s1558382 17:ff6ec6c50082 732 {
s1558382 17:ff6ec6c50082 733 reference_m1+=0.1;
s1558382 17:ff6ec6c50082 734 reference_m2+=0.1;
s1558382 17:ff6ec6c50082 735 }
s1558382 17:ff6ec6c50082 736 else if(currentRepetitions >= 500)
s1558382 17:ff6ec6c50082 737 {
s1558382 17:ff6ec6c50082 738 reference_m1-=0.2;
s1558382 17:ff6ec6c50082 739 reference_m2-=0.2;
s1558382 17:ff6ec6c50082 740 }
s1558382 17:ff6ec6c50082 741 else if(currentRepetitions >= 900)
s1558382 17:ff6ec6c50082 742 {
s1558382 17:ff6ec6c50082 743 reference_m1+=0.2;
s1558382 17:ff6ec6c50082 744 reference_m2+=0.2;
s1558382 17:ff6ec6c50082 745 }
s1558382 17:ff6ec6c50082 746 else if(currentRepetitions >= 1300)
s1558382 17:ff6ec6c50082 747 {
s1558382 17:ff6ec6c50082 748 reference_m1-=0.2;
s1558382 17:ff6ec6c50082 749 reference_m2-=0.2;
s1558382 17:ff6ec6c50082 750 }
s1558382 17:ff6ec6c50082 751 else if(currentRepetitions >= 1700)
s1558382 17:ff6ec6c50082 752 {
s1558382 17:ff6ec6c50082 753 reference_m1+=0.2;
s1558382 17:ff6ec6c50082 754 reference_m2+=0.2;
s1558382 17:ff6ec6c50082 755 }
s1558382 17:ff6ec6c50082 756
s1558382 17:ff6ec6c50082 757 if(currentRepetitions >= 2050)
s1558382 17:ff6ec6c50082 758 {
s1558382 17:ff6ec6c50082 759 currentRepetitions = 0;
s1558382 17:ff6ec6c50082 760 previousRepetitions = 0;
s1558382 17:ff6ec6c50082 761 m3_clean_ts = 0;
s1558382 17:ff6ec6c50082 762 state = CLEANCONFIRM;
s1558382 17:ff6ec6c50082 763 }
s1558382 17:ff6ec6c50082 764 }
s1558382 17:ff6ec6c50082 765 break;
s1558382 17:ff6ec6c50082 766
s1558382 17:ff6ec6c50082 767 case CLEANCONFIRM:
s1558382 17:ff6ec6c50082 768 {
s1558382 17:ff6ec6c50082 769 rest();
s1558382 17:ff6ec6c50082 770 emgFilterLeft();
s1558382 17:ff6ec6c50082 771
s1558382 17:ff6ec6c50082 772 if(confirm == true && currentRepetitions >= 1200)
s1558382 17:ff6ec6c50082 773 {
s1558382 17:ff6ec6c50082 774 currentRepetitions = 0;
s1558382 17:ff6ec6c50082 775 previousRepetitions = 0;
s1558382 17:ff6ec6c50082 776 state = HANDUP;
s1558382 17:ff6ec6c50082 777 }
s1558382 17:ff6ec6c50082 778 else if(currentRepetitions >= 1200)
s1558382 17:ff6ec6c50082 779 {
s1558382 17:ff6ec6c50082 780 currentRepetitions = 0;
s1558382 17:ff6ec6c50082 781 previousRepetitions = 0;
s1558382 17:ff6ec6c50082 782 state = CLEANING;
s1558382 17:ff6ec6c50082 783 }
s1558382 17:ff6ec6c50082 784 }
s1558382 17:ff6ec6c50082 785 break;
s1558382 17:ff6ec6c50082 786
s1558382 17:ff6ec6c50082 787 case HANDUP:
s1558382 17:ff6ec6c50082 788 {
s1558382 17:ff6ec6c50082 789 m3_pos_ts = 0.0014;
s1558382 17:ff6ec6c50082 790 motor3_direction = up;
s1558382 17:ff6ec6c50082 791
s1558382 17:ff6ec6c50082 792 if(teller == 0)
s1558382 17:ff6ec6c50082 793 {
s1558382 21:1c7106689c96 794 m3_pos_ts = 100000;
s1558382 17:ff6ec6c50082 795 state = RETURN2REST;
s1558382 17:ff6ec6c50082 796 }
s1558382 17:ff6ec6c50082 797 }
s1558382 17:ff6ec6c50082 798 break;
s1558382 17:ff6ec6c50082 799
s1558382 13:b4ed3eba926b 800 case RETURN2REST:
s1558382 12:fe8002389810 801 {
s1558382 12:fe8002389810 802 rest(); //For counting purposes
s1558382 12:fe8002389810 803 if(currentRepetitions < 2200)
s1558382 12:fe8002389810 804 {
s1558382 12:fe8002389810 805 reference_m1 = 0;
s1558382 12:fe8002389810 806 reference_m2 = 0;
s1558382 12:fe8002389810 807 }
s1558382 12:fe8002389810 808
s1558382 12:fe8002389810 809 if(currentRepetitions >= 2200)
s1558382 12:fe8002389810 810 {
s1558382 12:fe8002389810 811 currentRepetitions = 0;
s1558382 12:fe8002389810 812 previousRepetitions = 0;
s1558382 14:08c982d2bb2a 813 tableSpot = 0;
s1558382 12:fe8002389810 814 state = REST;
s1558382 12:fe8002389810 815 }
s1558382 12:fe8002389810 816 }
s1558382 12:fe8002389810 817 break;
s1558382 12:fe8002389810 818
s1558382 12:fe8002389810 819 default:
s1558382 12:fe8002389810 820 {
s1558382 12:fe8002389810 821 currentRepetitions = 0;
s1558382 12:fe8002389810 822 previousRepetitions = 0;
s1558382 12:fe8002389810 823 state = REST;
s1558382 12:fe8002389810 824 }
s1558382 1:43fad4d1dee0 825 }
s1558382 0:62fe4a2a8101 826 }
s1558382 0:62fe4a2a8101 827
s1558382 12:fe8002389810 828 void screenUpdate()
s1558382 12:fe8002389810 829 /*--------------------------------------------------
s1558382 15:48f4d8310b38 830 This function prints the commands to the screen.
s1558382 15:48f4d8310b38 831 --------------------------------------------------*/
s1558382 12:fe8002389810 832 {
s1558382 12:fe8002389810 833 if(state == CALIBRATION)
s1558382 12:fe8002389810 834 {
s1558382 12:fe8002389810 835 if(currentRepetitions == 100)
s1558382 12:fe8002389810 836 {
s1558382 12:fe8002389810 837 pc.printf("Welcome to calibration.\n\rYou have 5 seconds to contract your right biceps.\n\r");
s1558382 12:fe8002389810 838 }
s1558382 12:fe8002389810 839 else if(currentRepetitions == 200)
s1558382 12:fe8002389810 840 {
s1558382 12:fe8002389810 841 pc.printf("5\n\r");
s1558382 12:fe8002389810 842 }
s1558382 12:fe8002389810 843 else if(currentRepetitions == 400)
s1558382 12:fe8002389810 844 {
s1558382 12:fe8002389810 845 pc.printf("4\n\r");
s1558382 12:fe8002389810 846 }
s1558382 12:fe8002389810 847 else if(currentRepetitions == 600)
s1558382 12:fe8002389810 848 {
s1558382 12:fe8002389810 849 pc.printf("3\n\r");
s1558382 12:fe8002389810 850 }
s1558382 12:fe8002389810 851 else if(currentRepetitions == 800)
s1558382 12:fe8002389810 852 {
s1558382 12:fe8002389810 853 pc.printf("2\n\r");
s1558382 12:fe8002389810 854 }
s1558382 12:fe8002389810 855 else if(currentRepetitions == 1000)
s1558382 12:fe8002389810 856 {
s1558382 12:fe8002389810 857 pc.printf("1\n\r");
s1558382 12:fe8002389810 858 }
s1558382 12:fe8002389810 859 else if(currentRepetitions == 1050)
s1558382 12:fe8002389810 860 {
s1558382 12:fe8002389810 861 pc.printf("You have 5 seconds to contract your left biceps.\n\r");
s1558382 12:fe8002389810 862 }
s1558382 12:fe8002389810 863 else if(currentRepetitions == 1200)
s1558382 12:fe8002389810 864 {
s1558382 12:fe8002389810 865 pc.printf("5\n\r");
s1558382 12:fe8002389810 866 }
s1558382 12:fe8002389810 867 else if(currentRepetitions == 1400)
s1558382 12:fe8002389810 868 {
s1558382 12:fe8002389810 869 pc.printf("4\n\r");
s1558382 12:fe8002389810 870 }
s1558382 12:fe8002389810 871 else if(currentRepetitions == 1600)
s1558382 12:fe8002389810 872 {
s1558382 12:fe8002389810 873 pc.printf("3\n\r");
s1558382 12:fe8002389810 874 }
s1558382 12:fe8002389810 875 else if(currentRepetitions == 1800)
s1558382 12:fe8002389810 876 {
s1558382 12:fe8002389810 877 pc.printf("2\n\r");
s1558382 12:fe8002389810 878 }
s1558382 12:fe8002389810 879 else if(currentRepetitions == 2000)
s1558382 12:fe8002389810 880 {
s1558382 12:fe8002389810 881 pc.printf("1\n\r");
s1558382 12:fe8002389810 882 }
s1558382 12:fe8002389810 883 else if(currentRepetitions == 2050)
s1558382 12:fe8002389810 884 {
s1558382 12:fe8002389810 885 pc.printf("Calibrations is complete.\n\r");
s1558382 12:fe8002389810 886 /*
s1558382 12:fe8002389810 887 pc.printf("ThresholdRightHigh = %f\n\r.",emgThresholdRightHigh);
s1558382 12:fe8002389810 888 pc.printf("ThresholdLeftHigh = %f\n\r.",emgThresholdLeftHigh);
s1558382 12:fe8002389810 889 pc.printf("ThresholdRightLow = %f\n\r.",emgThresholdRightLow);
s1558382 12:fe8002389810 890 pc.printf("ThresholdLeftLow = %f\n\r.",emgThresholdLeftLow);
s1558382 12:fe8002389810 891 */
s1558382 12:fe8002389810 892 }
s1558382 12:fe8002389810 893 }
s1558382 12:fe8002389810 894
s1558382 12:fe8002389810 895 else if(state == COUNTING)
s1558382 12:fe8002389810 896 {
s1558382 12:fe8002389810 897 if(currentRepetitions == 100)
s1558382 12:fe8002389810 898 {
s1558382 12:fe8002389810 899 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 900 }
s1558382 12:fe8002389810 901 else if(currentRepetitions == 200)
s1558382 12:fe8002389810 902 {
s1558382 12:fe8002389810 903 pc.printf("10\n\r");
s1558382 12:fe8002389810 904 }
s1558382 12:fe8002389810 905 else if(currentRepetitions == 400)
s1558382 12:fe8002389810 906 {
s1558382 12:fe8002389810 907 pc.printf("9\n\r");
s1558382 12:fe8002389810 908 }
s1558382 12:fe8002389810 909 else if(currentRepetitions == 600)
s1558382 12:fe8002389810 910 {
s1558382 12:fe8002389810 911 pc.printf("8\n\r");
s1558382 12:fe8002389810 912 }
s1558382 12:fe8002389810 913 else if(currentRepetitions == 800)
s1558382 12:fe8002389810 914 {
s1558382 12:fe8002389810 915 pc.printf("7\n\r");
s1558382 12:fe8002389810 916 }
s1558382 12:fe8002389810 917 else if(currentRepetitions == 1000)
s1558382 12:fe8002389810 918 {
s1558382 12:fe8002389810 919 pc.printf("6\n\r");
s1558382 12:fe8002389810 920 }
s1558382 12:fe8002389810 921 else if(currentRepetitions == 1200)
s1558382 12:fe8002389810 922 {
s1558382 12:fe8002389810 923 pc.printf("5\n\r");
s1558382 12:fe8002389810 924 }
s1558382 12:fe8002389810 925 else if(currentRepetitions == 1400)
s1558382 12:fe8002389810 926 {
s1558382 12:fe8002389810 927 pc.printf("4\n\r");
s1558382 12:fe8002389810 928 }
s1558382 12:fe8002389810 929 else if(currentRepetitions == 1600)
s1558382 12:fe8002389810 930 {
s1558382 12:fe8002389810 931 pc.printf("3\n\r");
s1558382 12:fe8002389810 932 }
s1558382 12:fe8002389810 933 else if(currentRepetitions == 1800)
s1558382 12:fe8002389810 934 {
s1558382 12:fe8002389810 935 pc.printf("2\n\r");
s1558382 12:fe8002389810 936 }
s1558382 12:fe8002389810 937 else if(currentRepetitions == 2000)
s1558382 12:fe8002389810 938 {
s1558382 12:fe8002389810 939 pc.printf("1\n\r");
s1558382 12:fe8002389810 940 }
s1558382 15:48f4d8310b38 941 else if(currentRepetitions == 2100)
s1558382 12:fe8002389810 942 {
s1558382 12:fe8002389810 943 pc.printf("You have selected table spot nr %i, is this correct?\n\r",tableSpot);
s1558382 12:fe8002389810 944 }
s1558382 12:fe8002389810 945 }
s1558382 1:43fad4d1dee0 946
s1558382 12:fe8002389810 947 else if(state == CONFIRMATION)
s1558382 12:fe8002389810 948 {
s1558382 12:fe8002389810 949 if(currentRepetitions == 100)
s1558382 12:fe8002389810 950 {
s1558382 12:fe8002389810 951 pc.printf("You have 5 seconds to confirm with your left biceps.\n\r");
s1558382 12:fe8002389810 952 }
s1558382 12:fe8002389810 953 else if(currentRepetitions == 200)
s1558382 12:fe8002389810 954 {
s1558382 12:fe8002389810 955 pc.printf("5\n\r");
s1558382 12:fe8002389810 956 }
s1558382 12:fe8002389810 957 else if(currentRepetitions == 400)
s1558382 12:fe8002389810 958 {
s1558382 12:fe8002389810 959 pc.printf("4\n\r");
s1558382 12:fe8002389810 960 }
s1558382 12:fe8002389810 961 else if(currentRepetitions == 600)
s1558382 12:fe8002389810 962 {
s1558382 12:fe8002389810 963 pc.printf("3\n\r");
s1558382 12:fe8002389810 964 }
s1558382 12:fe8002389810 965 else if(currentRepetitions == 800)
s1558382 12:fe8002389810 966 {
s1558382 12:fe8002389810 967 pc.printf("2\n\r");
s1558382 12:fe8002389810 968 }
s1558382 12:fe8002389810 969 else if(currentRepetitions == 1000)
s1558382 12:fe8002389810 970 {
s1558382 12:fe8002389810 971 pc.printf("1\n\r");
s1558382 12:fe8002389810 972 }
s1558382 12:fe8002389810 973 else if(confirm == true && currentRepetitions == 1050)
s1558382 12:fe8002389810 974 {
s1558382 17:ff6ec6c50082 975 pc.printf("Claire will go to table spot nr. %i now.\n\r", tableSpot);
s1558382 12:fe8002389810 976 }
s1558382 12:fe8002389810 977 else if(confirm == false && currentRepetitions == 1050)
s1558382 12:fe8002389810 978 {
s1558382 12:fe8002389810 979 pc.printf("You have cancelled.\n\r");
s1558382 12:fe8002389810 980 }
s1558382 12:fe8002389810 981 }
s1558382 12:fe8002389810 982
s1558382 12:fe8002389810 983 else if(state == GO2TABLESPOT)
s1558382 12:fe8002389810 984 {
s1558382 15:48f4d8310b38 985 if(tableSpot >= 1 && tableSpot <= 6)
s1558382 14:08c982d2bb2a 986 {
s1558382 12:fe8002389810 987 if(currentRepetitions == 100)
s1558382 12:fe8002389810 988 {
s1558382 12:fe8002389810 989 pc.printf("Claire is going to table spot nr. %i.\n\r",tableSpot);
s1558382 12:fe8002389810 990 }
s1558382 17:ff6ec6c50082 991 else if(currentRepetitions == 200)
s1558382 12:fe8002389810 992 {
s1558382 12:fe8002389810 993 pc.printf("10\n\r");
s1558382 12:fe8002389810 994 }
s1558382 12:fe8002389810 995 else if(currentRepetitions == 400)
s1558382 12:fe8002389810 996 {
s1558382 12:fe8002389810 997 pc.printf("9\n\r");
s1558382 12:fe8002389810 998 }
s1558382 12:fe8002389810 999 else if(currentRepetitions == 600)
s1558382 12:fe8002389810 1000 {
s1558382 12:fe8002389810 1001 pc.printf("8\n\r");
s1558382 12:fe8002389810 1002 }
s1558382 12:fe8002389810 1003 else if(currentRepetitions == 800)
s1558382 12:fe8002389810 1004 {
s1558382 12:fe8002389810 1005 pc.printf("7\n\r");
s1558382 12:fe8002389810 1006 }
s1558382 12:fe8002389810 1007 else if(currentRepetitions == 1000)
s1558382 12:fe8002389810 1008 {
s1558382 12:fe8002389810 1009 pc.printf("6\n\r");
s1558382 12:fe8002389810 1010 }
s1558382 12:fe8002389810 1011 else if(currentRepetitions == 1200)
s1558382 12:fe8002389810 1012 {
s1558382 12:fe8002389810 1013 pc.printf("5\n\r");
s1558382 12:fe8002389810 1014 }
s1558382 12:fe8002389810 1015 else if(currentRepetitions == 1400)
s1558382 12:fe8002389810 1016 {
s1558382 12:fe8002389810 1017 pc.printf("4\n\r");
s1558382 12:fe8002389810 1018 }
s1558382 12:fe8002389810 1019 else if(currentRepetitions == 1600)
s1558382 12:fe8002389810 1020 {
s1558382 12:fe8002389810 1021 pc.printf("3\n\r");
s1558382 12:fe8002389810 1022 }
s1558382 12:fe8002389810 1023 else if(currentRepetitions == 1800)
s1558382 12:fe8002389810 1024 {
s1558382 12:fe8002389810 1025 pc.printf("2\n\r");
s1558382 12:fe8002389810 1026 }
s1558382 12:fe8002389810 1027 else if(currentRepetitions == 2000)
s1558382 12:fe8002389810 1028 {
s1558382 12:fe8002389810 1029 pc.printf("1\n\r");
s1558382 12:fe8002389810 1030 }
s1558382 13:b4ed3eba926b 1031 else if(currentRepetitions == 2050)
s1558382 12:fe8002389810 1032 {
s1558382 17:ff6ec6c50082 1033 pc.printf("Claire is at table spot nr. %i. It will be cleaned shortly.\n\r",tableSpot);
s1558382 14:08c982d2bb2a 1034 }
s1558382 14:08c982d2bb2a 1035 }
s1558382 14:08c982d2bb2a 1036
s1558382 14:08c982d2bb2a 1037 else
s1558382 14:08c982d2bb2a 1038 {
s1558382 14:08c982d2bb2a 1039 if(currentRepetitions == 100)
s1558382 14:08c982d2bb2a 1040 {
s1558382 15:48f4d8310b38 1041 pc.printf("This isn't a valid table spot. Please try again.");
s1558382 14:08c982d2bb2a 1042 }
s1558382 14:08c982d2bb2a 1043 }
s1558382 12:fe8002389810 1044 }
s1558382 12:fe8002389810 1045
s1558382 17:ff6ec6c50082 1046 else if(state == READY2CLEAN)
s1558382 12:fe8002389810 1047 {
s1558382 12:fe8002389810 1048 if(currentRepetitions == 100)
s1558382 12:fe8002389810 1049 {
s1558382 17:ff6ec6c50082 1050 pc.printf("You have 5 seconds to confirm with your left biceps that you want to clean the table spot Claire is on.\n\r");
s1558382 12:fe8002389810 1051 }
s1558382 12:fe8002389810 1052 else if(currentRepetitions == 200)
s1558382 12:fe8002389810 1053 {
s1558382 12:fe8002389810 1054 pc.printf("5\n\r");
s1558382 12:fe8002389810 1055 }
s1558382 12:fe8002389810 1056 else if(currentRepetitions == 400)
s1558382 12:fe8002389810 1057 {
s1558382 12:fe8002389810 1058 pc.printf("4\n\r");
s1558382 12:fe8002389810 1059 }
s1558382 12:fe8002389810 1060 else if(currentRepetitions == 600)
s1558382 12:fe8002389810 1061 {
s1558382 12:fe8002389810 1062 pc.printf("3\n\r");
s1558382 12:fe8002389810 1063 }
s1558382 12:fe8002389810 1064 else if(currentRepetitions == 800)
s1558382 12:fe8002389810 1065 {
s1558382 12:fe8002389810 1066 pc.printf("2\n\r");
s1558382 12:fe8002389810 1067 }
s1558382 12:fe8002389810 1068 else if(currentRepetitions == 1000)
s1558382 12:fe8002389810 1069 {
s1558382 12:fe8002389810 1070 pc.printf("1\n\r");
s1558382 12:fe8002389810 1071 }
s1558382 17:ff6ec6c50082 1072 else if(confirm == true && currentRepetitions == 1050)
s1558382 17:ff6ec6c50082 1073 {
s1558382 17:ff6ec6c50082 1074 pc.printf("Table spot nr. %i will be cleaned now.\n\r", tableSpot);
s1558382 17:ff6ec6c50082 1075 }
s1558382 17:ff6ec6c50082 1076 else if(confirm == false && currentRepetitions == 1050)
s1558382 17:ff6ec6c50082 1077 {
s1558382 17:ff6ec6c50082 1078 pc.printf("You have cancelled.\n\r");
s1558382 17:ff6ec6c50082 1079 }
s1558382 12:fe8002389810 1080 }
s1558382 12:fe8002389810 1081
s1558382 17:ff6ec6c50082 1082 else if(state == HANDDOWN)
s1558382 17:ff6ec6c50082 1083 {
s1558382 17:ff6ec6c50082 1084 if(currentRepetitions == 100)
s1558382 17:ff6ec6c50082 1085 {
s1558382 17:ff6ec6c50082 1086 pc.printf("Claire's hand is going down to the table now. \n\r");
s1558382 17:ff6ec6c50082 1087 }
s1558382 17:ff6ec6c50082 1088 }
s1558382 17:ff6ec6c50082 1089
s1558382 17:ff6ec6c50082 1090
s1558382 17:ff6ec6c50082 1091 else if(state == CLEANING)
s1558382 17:ff6ec6c50082 1092 {
s1558382 17:ff6ec6c50082 1093 if(currentRepetitions == 100)
s1558382 17:ff6ec6c50082 1094 {
s1558382 17:ff6ec6c50082 1095 pc.printf("Claire is cleaning table spot nr. %i. It will be done in\n\r.",tableSpot);
s1558382 17:ff6ec6c50082 1096 }
s1558382 17:ff6ec6c50082 1097 else if(currentRepetitions == 200)
s1558382 17:ff6ec6c50082 1098 {
s1558382 17:ff6ec6c50082 1099 pc.printf("10\n\r");
s1558382 17:ff6ec6c50082 1100 }
s1558382 17:ff6ec6c50082 1101 else if(currentRepetitions == 400)
s1558382 17:ff6ec6c50082 1102 {
s1558382 17:ff6ec6c50082 1103 pc.printf("9\n\r");
s1558382 17:ff6ec6c50082 1104 }
s1558382 17:ff6ec6c50082 1105 else if(currentRepetitions == 600)
s1558382 17:ff6ec6c50082 1106 {
s1558382 17:ff6ec6c50082 1107 pc.printf("8\n\r");
s1558382 17:ff6ec6c50082 1108 }
s1558382 17:ff6ec6c50082 1109 else if(currentRepetitions == 800)
s1558382 17:ff6ec6c50082 1110 {
s1558382 17:ff6ec6c50082 1111 pc.printf("7\n\r");
s1558382 17:ff6ec6c50082 1112 }
s1558382 17:ff6ec6c50082 1113 else if(currentRepetitions == 1000)
s1558382 17:ff6ec6c50082 1114 {
s1558382 17:ff6ec6c50082 1115 pc.printf("6\n\r");
s1558382 17:ff6ec6c50082 1116 }
s1558382 17:ff6ec6c50082 1117 else if(currentRepetitions == 1200)
s1558382 17:ff6ec6c50082 1118 {
s1558382 17:ff6ec6c50082 1119 pc.printf("5\n\r");
s1558382 17:ff6ec6c50082 1120 }
s1558382 17:ff6ec6c50082 1121 else if(currentRepetitions == 1400)
s1558382 17:ff6ec6c50082 1122 {
s1558382 17:ff6ec6c50082 1123 pc.printf("4\n\r");
s1558382 17:ff6ec6c50082 1124 }
s1558382 17:ff6ec6c50082 1125 else if(currentRepetitions == 1600)
s1558382 17:ff6ec6c50082 1126 {
s1558382 17:ff6ec6c50082 1127 pc.printf("3\n\r");
s1558382 17:ff6ec6c50082 1128 }
s1558382 17:ff6ec6c50082 1129 else if(currentRepetitions == 1800)
s1558382 17:ff6ec6c50082 1130 {
s1558382 17:ff6ec6c50082 1131 pc.printf("2\n\r");
s1558382 17:ff6ec6c50082 1132 }
s1558382 17:ff6ec6c50082 1133 else if(currentRepetitions == 2000)
s1558382 17:ff6ec6c50082 1134 {
s1558382 17:ff6ec6c50082 1135 pc.printf("1\n\r");
s1558382 17:ff6ec6c50082 1136 }
s1558382 17:ff6ec6c50082 1137 else if(currentRepetitions == 2050)
s1558382 17:ff6ec6c50082 1138 {
s1558382 17:ff6ec6c50082 1139 pc.printf("Claire has cleaned table spot nr. %i.\n\r",tableSpot);
s1558382 17:ff6ec6c50082 1140 }
s1558382 17:ff6ec6c50082 1141 }
s1558382 17:ff6ec6c50082 1142
s1558382 17:ff6ec6c50082 1143 else if(state == CLEANCONFIRM)
s1558382 17:ff6ec6c50082 1144 {
s1558382 17:ff6ec6c50082 1145 if(currentRepetitions == 100)
s1558382 17:ff6ec6c50082 1146 {
s1558382 17:ff6ec6c50082 1147 pc.printf("Is table spot %i cleaned? Contract your left biceps to confirm.\n\r",tableSpot);
s1558382 17:ff6ec6c50082 1148 }
s1558382 17:ff6ec6c50082 1149 else if(currentRepetitions == 200)
s1558382 17:ff6ec6c50082 1150 {
s1558382 17:ff6ec6c50082 1151 pc.printf("5\n\r");
s1558382 17:ff6ec6c50082 1152 }
s1558382 17:ff6ec6c50082 1153 else if(currentRepetitions == 400)
s1558382 17:ff6ec6c50082 1154 {
s1558382 17:ff6ec6c50082 1155 pc.printf("4\n\r");
s1558382 17:ff6ec6c50082 1156 }
s1558382 17:ff6ec6c50082 1157 else if(currentRepetitions == 600)
s1558382 17:ff6ec6c50082 1158 {
s1558382 17:ff6ec6c50082 1159 pc.printf("3\n\r");
s1558382 17:ff6ec6c50082 1160 }
s1558382 17:ff6ec6c50082 1161 else if(currentRepetitions == 800)
s1558382 17:ff6ec6c50082 1162 {
s1558382 17:ff6ec6c50082 1163 pc.printf("2\n\r");
s1558382 17:ff6ec6c50082 1164 }
s1558382 17:ff6ec6c50082 1165 else if(currentRepetitions == 1000)
s1558382 17:ff6ec6c50082 1166 {
s1558382 17:ff6ec6c50082 1167 pc.printf("1\n\r");
s1558382 17:ff6ec6c50082 1168 }
s1558382 17:ff6ec6c50082 1169 else if(confirm == true && currentRepetitions == 1050)
s1558382 17:ff6ec6c50082 1170 {
s1558382 17:ff6ec6c50082 1171 pc.printf("Very good! Claire will return to the starting position.\n\r");
s1558382 17:ff6ec6c50082 1172 }
s1558382 17:ff6ec6c50082 1173 else if(confirm == false && currentRepetitions == 1050)
s1558382 17:ff6ec6c50082 1174 {
s1558382 17:ff6ec6c50082 1175 pc.printf("Claire will clean table spot nr. %i again.\n\r", tableSpot);
s1558382 17:ff6ec6c50082 1176 }
s1558382 17:ff6ec6c50082 1177 }
s1558382 17:ff6ec6c50082 1178
s1558382 17:ff6ec6c50082 1179 else if(state == HANDUP)
s1558382 17:ff6ec6c50082 1180 {
s1558382 17:ff6ec6c50082 1181 if(currentRepetitions == 100)
s1558382 17:ff6ec6c50082 1182 {
s1558382 17:ff6ec6c50082 1183 pc.printf("Claire's hand is going up now. \n\r");
s1558382 17:ff6ec6c50082 1184 }
s1558382 17:ff6ec6c50082 1185 }
s1558382 17:ff6ec6c50082 1186
s1558382 12:fe8002389810 1187 else if(state == RETURN2REST)
s1558382 12:fe8002389810 1188 {
s1558382 12:fe8002389810 1189 if(currentRepetitions == 100)
s1558382 12:fe8002389810 1190 {
s1558382 12:fe8002389810 1191 pc.printf("Claire is going back to the start. Which table spot do you want to clean next?\r\n");
s1558382 12:fe8002389810 1192 }
s1558382 12:fe8002389810 1193 }
s1558382 0:62fe4a2a8101 1194 }
s1558382 5:97977cb8daa3 1195
s1558382 12:fe8002389810 1196 //=========== MAIN ===========================================================
s1558382 1:43fad4d1dee0 1197 int main()
s1558382 0:62fe4a2a8101 1198 {
s1558382 1:43fad4d1dee0 1199 pc.baud(115200);
s1558382 12:fe8002389810 1200
s1558382 12:fe8002389810 1201 bqc1.add(&bq1).add(&bq2);
s1558382 12:fe8002389810 1202 bqc2.add(&bq3);
s1558382 12:fe8002389810 1203
s1558382 12:fe8002389810 1204 state = REST0;
s1558382 19:e1c923e0d0a3 1205 stateTick.attach(&state_activate,ts);
s1558382 20:3a5292c0f26a 1206 screenTick.attach(&screen_activate,ts);
s1558382 12:fe8002389810 1207
s1558382 12:fe8002389810 1208 motor1MagnitudePin.period(1.0/1000.0);
s1558382 12:fe8002389810 1209 motor2MagnitudePin.period(1.0/1000.0);
s1558382 12:fe8002389810 1210 t1.attach(&fn1_activate, 0.0001f);
s1558382 12:fe8002389810 1211 t2.attach(&fn2_activate, 0.0001f);
s1558382 12:fe8002389810 1212 t3.attach(&fn3_activate, 0.0001f);
s1558382 12:fe8002389810 1213 t4.attach(&fn4_activate, 0.0001f);
s1558382 12:fe8002389810 1214 t5.attach(&fn5_activate, 0.0001f);
s1558382 12:fe8002389810 1215 t6.attach(&fn6_activate, 0.0001f);
s1558382 16:ea41e12f2484 1216
s1558382 16:ea41e12f2484 1217 //Motor 3
s1558382 17:ff6ec6c50082 1218 t0.attach(&fn0_activate, m3_pos_ts);
s1558382 17:ff6ec6c50082 1219 t7.attach(&fn7_activate, m3_clean_ts);
s1558382 12:fe8002389810 1220
s1558382 12:fe8002389810 1221 while(1)
s1558382 20:3a5292c0f26a 1222 {
s1558382 12:fe8002389810 1223 if(fn1_go)
s1558382 12:fe8002389810 1224 {
s1558382 12:fe8002389810 1225 fn1_go = false;
s1558382 12:fe8002389810 1226 getAngPosition_m1();
s1558382 12:fe8002389810 1227 }
s1558382 12:fe8002389810 1228
s1558382 12:fe8002389810 1229 if(fn2_go)
s1558382 12:fe8002389810 1230 {
s1558382 12:fe8002389810 1231 fn2_go = false;
s1558382 12:fe8002389810 1232 motor1_Controller(radians_m1);
s1558382 12:fe8002389810 1233 }
s1558382 12:fe8002389810 1234
s1558382 12:fe8002389810 1235 if(fn3_go)
s1558382 12:fe8002389810 1236 {
s1558382 12:fe8002389810 1237 fn3_go = false;
s1558382 12:fe8002389810 1238 control_m1(motor1,radians_m1);
s1558382 12:fe8002389810 1239 }
s1558382 12:fe8002389810 1240
s1558382 12:fe8002389810 1241 if(fn4_go)
s1558382 12:fe8002389810 1242 {
s1558382 12:fe8002389810 1243 fn4_go = false;
s1558382 12:fe8002389810 1244 getAngPosition_m2();
s1558382 12:fe8002389810 1245 }
s1558382 12:fe8002389810 1246
s1558382 12:fe8002389810 1247 if(fn5_go)
s1558382 12:fe8002389810 1248 {
s1558382 12:fe8002389810 1249 fn5_go = false;
s1558382 12:fe8002389810 1250 motor2_Controller(radians_m2);
s1558382 12:fe8002389810 1251 }
s1558382 12:fe8002389810 1252
s1558382 12:fe8002389810 1253 if(fn6_go)
s1558382 12:fe8002389810 1254 {
s1558382 12:fe8002389810 1255 fn6_go = false;
s1558382 12:fe8002389810 1256 control_m2(motor2,radians_m2);
s1558382 12:fe8002389810 1257 }
s1558382 16:ea41e12f2484 1258
s1558382 16:ea41e12f2484 1259 if(fn0_go)
s1558382 16:ea41e12f2484 1260 {
s1558382 16:ea41e12f2484 1261 fn0_go = false;
s1558382 16:ea41e12f2484 1262 motor3_position();
s1558382 16:ea41e12f2484 1263 }
s1558382 16:ea41e12f2484 1264
s1558382 16:ea41e12f2484 1265 if(fn7_go)
s1558382 16:ea41e12f2484 1266 {
s1558382 16:ea41e12f2484 1267 fn7_go = false;
s1558382 16:ea41e12f2484 1268 motor3_clean();
s1558382 16:ea41e12f2484 1269 }
s1558382 19:e1c923e0d0a3 1270
s1558382 19:e1c923e0d0a3 1271 if(state_go)
s1558382 19:e1c923e0d0a3 1272 {
s1558382 19:e1c923e0d0a3 1273 state_go = false;
s1558382 19:e1c923e0d0a3 1274 stateMachine();
s1558382 19:e1c923e0d0a3 1275 }
s1558382 20:3a5292c0f26a 1276
s1558382 20:3a5292c0f26a 1277 if(screen_go)
s1558382 20:3a5292c0f26a 1278 {
s1558382 20:3a5292c0f26a 1279 screen_go = false;
s1558382 20:3a5292c0f26a 1280 screenUpdate();
s1558382 20:3a5292c0f26a 1281 }
s1558382 12:fe8002389810 1282 }
s1558382 12:fe8002389810 1283 }