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@19:e1c923e0d0a3, 2016-11-02 (annotated)

Committer:
s1558382
Date:
Wed Nov 02 13:32:14 2016 +0000
Revision:
19:e1c923e0d0a3
Parent:
18:5ddef43222ea
Child:
20:3a5292c0f26a
New angles

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