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