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@16:ea41e12f2484, 2016-11-01 (annotated)

Committer:
s1558382
Date:
Tue Nov 01 13:28:46 2016 +0000
Revision:
16:ea41e12f2484
Parent:
15:48f4d8310b38
Child:
17:ff6ec6c50082
Motor 3 added

Who changed what in which revision?

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