![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Project of Biorobotics
Dependencies: HIDScope MODSERIAL QEI mbed biquadFilter
Fork of TutorialPES by
main.cpp@9:355dd95199c3, 2018-10-30 (annotated)
- Committer:
- ThomBMT
- Date:
- Tue Oct 30 13:57:00 2018 +0000
- Revision:
- 9:355dd95199c3
- Parent:
- 8:e8734a254818
- Child:
- 10:05ad15c48388
Homing state added and calibrationstate tested and working
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JurrienBos | 0:4591ba678a39 | 1 | #include "mbed.h" |
JurrienBos | 0:4591ba678a39 | 2 | #include "MODSERIAL.h" |
ThomBMT | 2:dc9766657afb | 3 | #include "HIDScope.h" |
ThomBMT | 2:dc9766657afb | 4 | #include "QEI.h" |
ThomBMT | 2:dc9766657afb | 5 | |
JurrienBos | 0:4591ba678a39 | 6 | MODSERIAL pc(USBTX, USBRX); |
JurrienBos | 0:4591ba678a39 | 7 | DigitalOut DirectionPin1(D4); |
JurrienBos | 1:4bf64d003f3a | 8 | DigitalOut DirectionPin2(D7); |
JurrienBos | 0:4591ba678a39 | 9 | PwmOut PwmPin1(D5); |
JurrienBos | 1:4bf64d003f3a | 10 | PwmOut PwmPin2(D6); |
JurrienBos | 0:4591ba678a39 | 11 | DigitalIn Knop1(D2); |
ThomBMT | 4:8f67b8327300 | 12 | DigitalIn Knop2(D3); |
ThomBMT | 8:e8734a254818 | 13 | DigitalIn Knop3(PTA4); |
ThomBMT | 9:355dd95199c3 | 14 | DigitalIn Knop4(PTC6); |
JurrienBos | 0:4591ba678a39 | 15 | AnalogIn pot1 (A5); |
JurrienBos | 1:4bf64d003f3a | 16 | AnalogIn pot2 (A4); |
ThomBMT | 2:dc9766657afb | 17 | AnalogIn emg0( A0 ); |
ThomBMT | 2:dc9766657afb | 18 | AnalogIn emg1( A1 ); |
ThomBMT | 2:dc9766657afb | 19 | AnalogIn emg2( A2 ); |
ThomBMT | 2:dc9766657afb | 20 | AnalogIn emg3( A3 ); |
ThomBMT | 9:355dd95199c3 | 21 | DigitalOut led_G(LED_GREEN); |
ThomBMT | 9:355dd95199c3 | 22 | DigitalOut led_R(LED_RED); |
ThomBMT | 9:355dd95199c3 | 23 | DigitalOut led_B(LED_BLUE); |
JurrienBos | 0:4591ba678a39 | 24 | |
ThomBMT | 8:e8734a254818 | 25 | QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); |
ThomBMT | 5:312186a0604d | 26 | QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); |
ThomBMT | 5:312186a0604d | 27 | |
ThomBMT | 4:8f67b8327300 | 28 | Ticker StateTicker; |
ThomBMT | 2:dc9766657afb | 29 | Ticker printTicker; |
ThomBMT | 5:312186a0604d | 30 | |
ThomBMT | 2:dc9766657afb | 31 | HIDScope scope( 4 ); |
JurrienBos | 0:4591ba678a39 | 32 | |
ThomBMT | 3:c8f0fc045505 | 33 | volatile float Bicep_Right = 0.0; |
ThomBMT | 3:c8f0fc045505 | 34 | volatile float Bicep_Left = 0.0; |
ThomBMT | 3:c8f0fc045505 | 35 | volatile float Tricep_Right = 0.0; |
ThomBMT | 3:c8f0fc045505 | 36 | volatile float Tricep_Left = 0.0; |
ThomBMT | 8:e8734a254818 | 37 | |
ThomBMT | 7:439940ae1197 | 38 | volatile const float pi = 3.1415926; |
ThomBMT | 9:355dd95199c3 | 39 | volatile const float rad_count = 0.0007479; // 2pi/8400; |
ThomBMT | 9:355dd95199c3 | 40 | volatile const float maxVelocity = 2.0f * pi; // in rad/s |
ThomBMT | 8:e8734a254818 | 41 | |
ThomBMT | 9:355dd95199c3 | 42 | volatile float referenceVelocity1 = 0.5; // dit is de gecentreerde waarde en dus de nulstand |
ThomBMT | 3:c8f0fc045505 | 43 | volatile float referenceVelocity2 = 0.5; |
ThomBMT | 2:dc9766657afb | 44 | |
ThomBMT | 9:355dd95199c3 | 45 | volatile int i = 0; // Led blink status |
ThomBMT | 9:355dd95199c3 | 46 | volatile int ii = 0; // Calibratie timer |
ThomBMT | 9:355dd95199c3 | 47 | volatile int iii = 0; // Start up timer |
ThomBMT | 9:355dd95199c3 | 48 | |
ThomBMT | 8:e8734a254818 | 49 | volatile float q_1; |
ThomBMT | 8:e8734a254818 | 50 | volatile float q_2; |
ThomBMT | 8:e8734a254818 | 51 | volatile float r_1; |
ThomBMT | 8:e8734a254818 | 52 | volatile float r_2; |
ThomBMT | 8:e8734a254818 | 53 | volatile const float r_3 = 0.035; |
ThomBMT | 8:e8734a254818 | 54 | volatile float w_1; |
ThomBMT | 8:e8734a254818 | 55 | volatile float w_2; |
ThomBMT | 8:e8734a254818 | 56 | |
ThomBMT | 8:e8734a254818 | 57 | volatile float Flex_Bi_R; |
ThomBMT | 8:e8734a254818 | 58 | volatile float Flex_Bi_L; |
ThomBMT | 8:e8734a254818 | 59 | volatile float Flex_Tri_R; |
ThomBMT | 8:e8734a254818 | 60 | volatile float Flex_Tri_L; |
ThomBMT | 9:355dd95199c3 | 61 | volatile float Threshold_Bi_R; |
ThomBMT | 9:355dd95199c3 | 62 | volatile float Threshold_Bi_L; |
ThomBMT | 9:355dd95199c3 | 63 | volatile float Threshold_Tri_R; |
ThomBMT | 9:355dd95199c3 | 64 | volatile float Threshold_Tri_L; |
ThomBMT | 8:e8734a254818 | 65 | |
ThomBMT | 8:e8734a254818 | 66 | enum states{Starting, Calibration, Homing, Function}; |
ThomBMT | 4:8f67b8327300 | 67 | |
ThomBMT | 8:e8734a254818 | 68 | volatile states Active_State = Starting; |
ThomBMT | 4:8f67b8327300 | 69 | |
ThomBMT | 9:355dd95199c3 | 70 | volatile float vx; |
ThomBMT | 9:355dd95199c3 | 71 | volatile float vy; |
ThomBMT | 5:312186a0604d | 72 | volatile int counts1 ; |
ThomBMT | 5:312186a0604d | 73 | volatile int counts2 ; |
ThomBMT | 5:312186a0604d | 74 | volatile float rad_m1; |
ThomBMT | 5:312186a0604d | 75 | volatile float rad_m2; |
ThomBMT | 4:8f67b8327300 | 76 | |
ThomBMT | 3:c8f0fc045505 | 77 | void Encoding() |
ThomBMT | 3:c8f0fc045505 | 78 | { |
ThomBMT | 5:312186a0604d | 79 | |
ThomBMT | 3:c8f0fc045505 | 80 | counts1 = Encoder1.getPulses(); |
ThomBMT | 3:c8f0fc045505 | 81 | counts2 = Encoder2.getPulses(); |
ThomBMT | 4:8f67b8327300 | 82 | |
ThomBMT | 7:439940ae1197 | 83 | rad_m1 = rad_count * (float)counts1; |
ThomBMT | 7:439940ae1197 | 84 | rad_m2 = rad_count * (float)counts2; |
ThomBMT | 3:c8f0fc045505 | 85 | } |
ThomBMT | 2:dc9766657afb | 86 | |
ThomBMT | 9:355dd95199c3 | 87 | void BlinkLed() |
ThomBMT | 9:355dd95199c3 | 88 | { |
ThomBMT | 9:355dd95199c3 | 89 | if(i==1) |
ThomBMT | 9:355dd95199c3 | 90 | { |
ThomBMT | 9:355dd95199c3 | 91 | static int rr = 0; |
ThomBMT | 9:355dd95199c3 | 92 | rr++; |
ThomBMT | 9:355dd95199c3 | 93 | if (rr == 1) |
ThomBMT | 9:355dd95199c3 | 94 | { |
ThomBMT | 9:355dd95199c3 | 95 | led_R = !led_R; |
ThomBMT | 9:355dd95199c3 | 96 | } |
ThomBMT | 9:355dd95199c3 | 97 | else if (rr == 100) |
ThomBMT | 9:355dd95199c3 | 98 | { |
ThomBMT | 9:355dd95199c3 | 99 | rr = 0; |
ThomBMT | 9:355dd95199c3 | 100 | } |
ThomBMT | 9:355dd95199c3 | 101 | } |
ThomBMT | 9:355dd95199c3 | 102 | else if(i==2) |
ThomBMT | 9:355dd95199c3 | 103 | { |
ThomBMT | 9:355dd95199c3 | 104 | static int gg = 0; |
ThomBMT | 9:355dd95199c3 | 105 | gg++; |
ThomBMT | 9:355dd95199c3 | 106 | if (gg == 1) |
ThomBMT | 9:355dd95199c3 | 107 | { |
ThomBMT | 9:355dd95199c3 | 108 | led_G = !led_G; |
ThomBMT | 9:355dd95199c3 | 109 | } |
ThomBMT | 9:355dd95199c3 | 110 | else if (gg == 100) |
ThomBMT | 9:355dd95199c3 | 111 | { |
ThomBMT | 9:355dd95199c3 | 112 | gg = 0; |
ThomBMT | 9:355dd95199c3 | 113 | } |
ThomBMT | 9:355dd95199c3 | 114 | } |
ThomBMT | 9:355dd95199c3 | 115 | else if (i==3) |
ThomBMT | 9:355dd95199c3 | 116 | { |
ThomBMT | 9:355dd95199c3 | 117 | static int bb = 0; |
ThomBMT | 9:355dd95199c3 | 118 | bb++; |
ThomBMT | 9:355dd95199c3 | 119 | if (bb == 1) |
ThomBMT | 9:355dd95199c3 | 120 | { |
ThomBMT | 9:355dd95199c3 | 121 | led_B = !led_B; |
ThomBMT | 9:355dd95199c3 | 122 | } |
ThomBMT | 9:355dd95199c3 | 123 | else if (bb == 100) |
ThomBMT | 9:355dd95199c3 | 124 | { |
ThomBMT | 9:355dd95199c3 | 125 | bb = 0; |
ThomBMT | 9:355dd95199c3 | 126 | } |
ThomBMT | 9:355dd95199c3 | 127 | } |
ThomBMT | 9:355dd95199c3 | 128 | else |
ThomBMT | 9:355dd95199c3 | 129 | { |
ThomBMT | 9:355dd95199c3 | 130 | led_R=led_G=led_B=1; |
ThomBMT | 9:355dd95199c3 | 131 | } |
ThomBMT | 9:355dd95199c3 | 132 | } |
ThomBMT | 9:355dd95199c3 | 133 | |
ThomBMT | 2:dc9766657afb | 134 | void EMG_Read() |
ThomBMT | 2:dc9766657afb | 135 | { |
ThomBMT | 3:c8f0fc045505 | 136 | Bicep_Right = emg0.read(); |
ThomBMT | 3:c8f0fc045505 | 137 | Bicep_Left = emg1.read(); |
ThomBMT | 3:c8f0fc045505 | 138 | Tricep_Right = emg2.read(); |
ThomBMT | 3:c8f0fc045505 | 139 | Tricep_Left = emg3.read(); |
ThomBMT | 2:dc9766657afb | 140 | } |
ThomBMT | 2:dc9766657afb | 141 | |
ThomBMT | 2:dc9766657afb | 142 | void sample() |
ThomBMT | 2:dc9766657afb | 143 | { |
ThomBMT | 2:dc9766657afb | 144 | |
ThomBMT | 2:dc9766657afb | 145 | scope.set(0, emg0.read() ); |
ThomBMT | 2:dc9766657afb | 146 | scope.set(1, emg1.read() ); |
ThomBMT | 2:dc9766657afb | 147 | scope.set(2, emg2.read() ); |
ThomBMT | 2:dc9766657afb | 148 | scope.set(3, emg3.read() ); |
ThomBMT | 2:dc9766657afb | 149 | |
ThomBMT | 2:dc9766657afb | 150 | scope.send(); |
ThomBMT | 2:dc9766657afb | 151 | } |
ThomBMT | 2:dc9766657afb | 152 | |
ThomBMT | 8:e8734a254818 | 153 | void Inverse() |
ThomBMT | 8:e8734a254818 | 154 | { |
ThomBMT | 9:355dd95199c3 | 155 | q_1= rad_m1; // uit Encoder |
ThomBMT | 9:355dd95199c3 | 156 | q_2= rad_m2+(pi/2.0f); // uit Encoder |
ThomBMT | 8:e8734a254818 | 157 | r_1= -0.2f; |
ThomBMT | 8:e8734a254818 | 158 | r_2= -0.2f; |
ThomBMT | 8:e8734a254818 | 159 | |
ThomBMT | 8:e8734a254818 | 160 | float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2); |
ThomBMT | 8:e8734a254818 | 161 | float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3; |
ThomBMT | 8:e8734a254818 | 162 | float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0f*(r_1*cos(q_1))-r_3; |
ThomBMT | 8:e8734a254818 | 163 | float x = (-2.0f)*r_2*sin(q_1)*cos(q_2); |
ThomBMT | 9:355dd95199c3 | 164 | float D = 1.0f/(u*z-x*y); // Determinant |
ThomBMT | 9:355dd95199c3 | 165 | printf("Determinant is %f\r\n", D); |
ThomBMT | 8:e8734a254818 | 166 | |
ThomBMT | 9:355dd95199c3 | 167 | float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix |
ThomBMT | 9:355dd95199c3 | 168 | float b = -D*x; // Inverse jacobian |
ThomBMT | 9:355dd95199c3 | 169 | float c = -D*y; // Inverse jacobian |
ThomBMT | 9:355dd95199c3 | 170 | float d = D*u; // Inverse jacobian |
ThomBMT | 8:e8734a254818 | 171 | |
ThomBMT | 9:355dd95199c3 | 172 | float vx = pot1.read();//0.01f; // uit emg data |
ThomBMT | 9:355dd95199c3 | 173 | float vy = pot2.read();//0.0f; // uit emg data |
ThomBMT | 8:e8734a254818 | 174 | |
ThomBMT | 8:e8734a254818 | 175 | w_1 = vx*a+vy*b; |
ThomBMT | 8:e8734a254818 | 176 | w_2 = vx*c+vy*d; |
ThomBMT | 8:e8734a254818 | 177 | |
ThomBMT | 8:e8734a254818 | 178 | /* |
ThomBMT | 9:355dd95199c3 | 179 | printf("%f\r\n", w_1); |
ThomBMT | 9:355dd95199c3 | 180 | printf("%f\r\n", w_2); |
ThomBMT | 8:e8734a254818 | 181 | */ |
ThomBMT | 8:e8734a254818 | 182 | } |
ThomBMT | 9:355dd95199c3 | 183 | /* |
JurrienBos | 1:4bf64d003f3a | 184 | void velocity1() |
JurrienBos | 0:4591ba678a39 | 185 | { |
ThomBMT | 3:c8f0fc045505 | 186 | if (pot1.read()>0.5f) |
JurrienBos | 0:4591ba678a39 | 187 | { |
JurrienBos | 0:4591ba678a39 | 188 | // Clockwise rotation |
JurrienBos | 1:4bf64d003f3a | 189 | referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; |
JurrienBos | 0:4591ba678a39 | 190 | } |
JurrienBos | 0:4591ba678a39 | 191 | |
ThomBMT | 5:312186a0604d | 192 | else if (pot1.read() == 0.5f) |
JurrienBos | 0:4591ba678a39 | 193 | { |
JurrienBos | 1:4bf64d003f3a | 194 | referenceVelocity1 = pot1.read() * 0.0f; |
JurrienBos | 0:4591ba678a39 | 195 | } |
JurrienBos | 0:4591ba678a39 | 196 | |
JurrienBos | 0:4591ba678a39 | 197 | else if (pot1.read() < 0.5f) |
JurrienBos | 0:4591ba678a39 | 198 | { |
JurrienBos | 0:4591ba678a39 | 199 | // Counterclockwise rotation |
JurrienBos | 1:4bf64d003f3a | 200 | referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ; |
JurrienBos | 0:4591ba678a39 | 201 | } |
JurrienBos | 0:4591ba678a39 | 202 | } |
JurrienBos | 0:4591ba678a39 | 203 | |
JurrienBos | 1:4bf64d003f3a | 204 | void velocity2() |
JurrienBos | 1:4bf64d003f3a | 205 | { |
JurrienBos | 1:4bf64d003f3a | 206 | if (pot2.read()>0.5f) |
JurrienBos | 1:4bf64d003f3a | 207 | { |
JurrienBos | 1:4bf64d003f3a | 208 | // Clockwise rotation |
JurrienBos | 1:4bf64d003f3a | 209 | referenceVelocity2 = (pot2.read()-0.5f) * 2.0f; |
JurrienBos | 1:4bf64d003f3a | 210 | } |
JurrienBos | 1:4bf64d003f3a | 211 | |
JurrienBos | 1:4bf64d003f3a | 212 | else if (pot2.read() == 0.5f) |
JurrienBos | 1:4bf64d003f3a | 213 | { |
JurrienBos | 1:4bf64d003f3a | 214 | referenceVelocity2 = pot2.read() * 0.0f; |
JurrienBos | 1:4bf64d003f3a | 215 | } |
JurrienBos | 1:4bf64d003f3a | 216 | |
JurrienBos | 1:4bf64d003f3a | 217 | else if (pot2.read() < 0.5f) |
JurrienBos | 1:4bf64d003f3a | 218 | { |
JurrienBos | 1:4bf64d003f3a | 219 | // Counterclockwise rotation |
JurrienBos | 1:4bf64d003f3a | 220 | referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ; |
JurrienBos | 1:4bf64d003f3a | 221 | } |
JurrienBos | 1:4bf64d003f3a | 222 | } |
ThomBMT | 9:355dd95199c3 | 223 | */ |
JurrienBos | 1:4bf64d003f3a | 224 | void motor1() |
ThomBMT | 8:e8734a254818 | 225 | { |
ThomBMT | 8:e8734a254818 | 226 | float u_v1 = w_1; //referenceVelocity1 |
ThomBMT | 8:e8734a254818 | 227 | float u = u_v1/ (2.0f * pi); |
JurrienBos | 0:4591ba678a39 | 228 | DirectionPin1 = u < 0.0f; |
JurrienBos | 0:4591ba678a39 | 229 | PwmPin1 = fabs(u); |
JurrienBos | 0:4591ba678a39 | 230 | } |
JurrienBos | 0:4591ba678a39 | 231 | |
JurrienBos | 1:4bf64d003f3a | 232 | void motor2() |
JurrienBos | 1:4bf64d003f3a | 233 | { |
ThomBMT | 8:e8734a254818 | 234 | float u_v2 = w_2; //referenceVelocity2 |
ThomBMT | 8:e8734a254818 | 235 | float u = u_v2/ (2.0f * pi); |
JurrienBos | 1:4bf64d003f3a | 236 | DirectionPin2 = u > 0.0f; |
JurrienBos | 1:4bf64d003f3a | 237 | PwmPin2 = fabs(u); |
JurrienBos | 1:4bf64d003f3a | 238 | } |
ThomBMT | 9:355dd95199c3 | 239 | |
ThomBMT | 8:e8734a254818 | 240 | void Calibrating() |
ThomBMT | 8:e8734a254818 | 241 | { |
ThomBMT | 8:e8734a254818 | 242 | static float n = 0.0; |
ThomBMT | 8:e8734a254818 | 243 | static float m = 0.0; |
ThomBMT | 8:e8734a254818 | 244 | static float l = 0.0; |
ThomBMT | 8:e8734a254818 | 245 | static float k = 0.0; |
ThomBMT | 9:355dd95199c3 | 246 | |
ThomBMT | 9:355dd95199c3 | 247 | //static int ii; |
ThomBMT | 9:355dd95199c3 | 248 | ii++; |
ThomBMT | 9:355dd95199c3 | 249 | |
ThomBMT | 9:355dd95199c3 | 250 | if (ii<=2500) |
ThomBMT | 8:e8734a254818 | 251 | { |
ThomBMT | 9:355dd95199c3 | 252 | if (ii == 0) |
ThomBMT | 9:355dd95199c3 | 253 | { |
ThomBMT | 9:355dd95199c3 | 254 | pc.printf("Relax your muscles please. \r\n"); |
ThomBMT | 9:355dd95199c3 | 255 | } |
ThomBMT | 9:355dd95199c3 | 256 | else if (ii == 2250) |
ThomBMT | 9:355dd95199c3 | 257 | { |
ThomBMT | 9:355dd95199c3 | 258 | pc.printf("Flex your right bicep now please.\r\n"); |
ThomBMT | 9:355dd95199c3 | 259 | } |
ThomBMT | 9:355dd95199c3 | 260 | //chillen |
ThomBMT | 8:e8734a254818 | 261 | } |
ThomBMT | 9:355dd95199c3 | 262 | else if (ii>2500 && ii<5000) // |
ThomBMT | 9:355dd95199c3 | 263 | { |
ThomBMT | 9:355dd95199c3 | 264 | n = n + emg0.read();// dit wordt de variable naam na het filter |
ThomBMT | 9:355dd95199c3 | 265 | } |
ThomBMT | 9:355dd95199c3 | 266 | else if (ii == 5000) |
ThomBMT | 8:e8734a254818 | 267 | { |
ThomBMT | 8:e8734a254818 | 268 | Flex_Bi_R = n / 2500.0f; |
ThomBMT | 9:355dd95199c3 | 269 | pc.printf("You can relax your right bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_R); |
ThomBMT | 8:e8734a254818 | 270 | } |
ThomBMT | 9:355dd95199c3 | 271 | else if (ii>5000 && ii<=6000) |
ThomBMT | 8:e8734a254818 | 272 | { |
ThomBMT | 9:355dd95199c3 | 273 | if (ii == 5750) |
ThomBMT | 9:355dd95199c3 | 274 | { |
ThomBMT | 9:355dd95199c3 | 275 | pc.printf("Flex your left bicep now please. \r\n"); |
ThomBMT | 9:355dd95199c3 | 276 | } |
ThomBMT | 8:e8734a254818 | 277 | //chillen |
ThomBMT | 8:e8734a254818 | 278 | } |
ThomBMT | 9:355dd95199c3 | 279 | else if(ii>6000 && ii<8500) |
ThomBMT | 8:e8734a254818 | 280 | { |
ThomBMT | 8:e8734a254818 | 281 | m = m + emg1.read(); |
ThomBMT | 8:e8734a254818 | 282 | } |
ThomBMT | 9:355dd95199c3 | 283 | else if (ii == 8500) |
ThomBMT | 8:e8734a254818 | 284 | { |
ThomBMT | 8:e8734a254818 | 285 | Flex_Bi_L = m / 2500.0f; |
ThomBMT | 9:355dd95199c3 | 286 | pc.printf("You can relax your left bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_L); |
ThomBMT | 8:e8734a254818 | 287 | } |
ThomBMT | 9:355dd95199c3 | 288 | else if (ii>8500 && ii<=9500) |
ThomBMT | 8:e8734a254818 | 289 | { |
ThomBMT | 9:355dd95199c3 | 290 | if (ii == 9250) |
ThomBMT | 9:355dd95199c3 | 291 | { |
ThomBMT | 9:355dd95199c3 | 292 | pc.printf("Flex your right tricep now please. \r\n"); |
ThomBMT | 9:355dd95199c3 | 293 | } |
ThomBMT | 8:e8734a254818 | 294 | //chillen |
ThomBMT | 8:e8734a254818 | 295 | } |
ThomBMT | 9:355dd95199c3 | 296 | else if (ii>9500 && ii<12000) |
ThomBMT | 8:e8734a254818 | 297 | { |
ThomBMT | 8:e8734a254818 | 298 | l = l + emg2.read(); |
ThomBMT | 8:e8734a254818 | 299 | } |
ThomBMT | 9:355dd95199c3 | 300 | else if (ii == 12000) |
ThomBMT | 8:e8734a254818 | 301 | { |
ThomBMT | 8:e8734a254818 | 302 | Flex_Tri_R = l / 2500.0f; |
ThomBMT | 9:355dd95199c3 | 303 | pc.printf("You can relax your right tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Tri_R); |
ThomBMT | 8:e8734a254818 | 304 | } |
ThomBMT | 9:355dd95199c3 | 305 | else if (ii>12000 && ii <=13000) |
ThomBMT | 8:e8734a254818 | 306 | { |
ThomBMT | 9:355dd95199c3 | 307 | if (ii == 12750) |
ThomBMT | 9:355dd95199c3 | 308 | { |
ThomBMT | 9:355dd95199c3 | 309 | pc.printf("Flex your left tricep now please. \r\n"); |
ThomBMT | 9:355dd95199c3 | 310 | } |
ThomBMT | 8:e8734a254818 | 311 | //chillen |
ThomBMT | 8:e8734a254818 | 312 | } |
ThomBMT | 9:355dd95199c3 | 313 | else if (ii>13000 && ii<15500) |
ThomBMT | 8:e8734a254818 | 314 | { |
ThomBMT | 8:e8734a254818 | 315 | k = k + emg3.read(); |
ThomBMT | 8:e8734a254818 | 316 | } |
ThomBMT | 9:355dd95199c3 | 317 | else if (ii == 15500) |
ThomBMT | 8:e8734a254818 | 318 | { |
ThomBMT | 8:e8734a254818 | 319 | Flex_Tri_L = k / 2500.0f; |
ThomBMT | 9:355dd95199c3 | 320 | pc.printf("You can relax your left tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\nThe calibration has been completed, the system is now operatable. \r\n",Flex_Tri_L); |
ThomBMT | 9:355dd95199c3 | 321 | } |
ThomBMT | 9:355dd95199c3 | 322 | |
ThomBMT | 9:355dd95199c3 | 323 | Threshold_Bi_R = 0.75f * Flex_Bi_R; |
ThomBMT | 9:355dd95199c3 | 324 | Threshold_Bi_L = 0.75f * Flex_Bi_L; |
ThomBMT | 9:355dd95199c3 | 325 | Threshold_Tri_R = 0.75f * Flex_Tri_R; |
ThomBMT | 9:355dd95199c3 | 326 | Threshold_Tri_L = 0.75f * Flex_Tri_L; |
ThomBMT | 9:355dd95199c3 | 327 | |
ThomBMT | 9:355dd95199c3 | 328 | if (ii == 16500) |
ThomBMT | 9:355dd95199c3 | 329 | { |
ThomBMT | 9:355dd95199c3 | 330 | pc.printf("\r\nThreshold value right bicep = %f\r\nThreshold value left bicep = %f\r\nThreshold value right tricep = %f\r\nThreshold value left tricep = %f\r\n\r\n",Threshold_Bi_R,Threshold_Bi_L,Threshold_Tri_R,Threshold_Tri_L); |
ThomBMT | 9:355dd95199c3 | 331 | } |
ThomBMT | 9:355dd95199c3 | 332 | else if (ii == 20000) |
ThomBMT | 9:355dd95199c3 | 333 | { |
ThomBMT | 9:355dd95199c3 | 334 | pc.printf("\r\nAutomatic switch to Homing State\r\n"); |
ThomBMT | 9:355dd95199c3 | 335 | Active_State = Homing; |
ThomBMT | 9:355dd95199c3 | 336 | } |
ThomBMT | 9:355dd95199c3 | 337 | } |
ThomBMT | 9:355dd95199c3 | 338 | |
ThomBMT | 9:355dd95199c3 | 339 | void Start_Up() |
ThomBMT | 9:355dd95199c3 | 340 | { |
ThomBMT | 9:355dd95199c3 | 341 | i++; |
ThomBMT | 9:355dd95199c3 | 342 | iii++; |
ThomBMT | 9:355dd95199c3 | 343 | if (iii == 1) |
ThomBMT | 9:355dd95199c3 | 344 | { |
ThomBMT | 9:355dd95199c3 | 345 | pc.printf("System is starting...\r\nWaiting for further input...\r\n"); |
ThomBMT | 9:355dd95199c3 | 346 | } |
ThomBMT | 9:355dd95199c3 | 347 | |
ThomBMT | 9:355dd95199c3 | 348 | else if (iii == 30000) |
ThomBMT | 9:355dd95199c3 | 349 | { |
ThomBMT | 9:355dd95199c3 | 350 | pc.printf("1 minute without input..\r\nReseting start-up...\r\n"); |
ThomBMT | 9:355dd95199c3 | 351 | iii = 0; |
ThomBMT | 9:355dd95199c3 | 352 | } |
ThomBMT | 9:355dd95199c3 | 353 | else if (iii == 40001) // sleeping state is only added for designing purposes and will most likely never be used |
ThomBMT | 9:355dd95199c3 | 354 | { // when working with patients. Furthermore it cannot be reached automaticly |
ThomBMT | 9:355dd95199c3 | 355 | pc.printf("Sleeping... Press button 4 to wake me up!\r\n\r\n"); |
ThomBMT | 9:355dd95199c3 | 356 | iii++; |
ThomBMT | 9:355dd95199c3 | 357 | } |
ThomBMT | 9:355dd95199c3 | 358 | else if (iii == 45000) |
ThomBMT | 9:355dd95199c3 | 359 | { |
ThomBMT | 9:355dd95199c3 | 360 | iii = 40000; |
ThomBMT | 8:e8734a254818 | 361 | } |
ThomBMT | 8:e8734a254818 | 362 | } |
ThomBMT | 8:e8734a254818 | 363 | |
ThomBMT | 8:e8734a254818 | 364 | void OFF() |
ThomBMT | 8:e8734a254818 | 365 | { |
ThomBMT | 8:e8734a254818 | 366 | PwmPin1 = PwmPin2 = 0; |
ThomBMT | 8:e8734a254818 | 367 | } |
JurrienBos | 0:4591ba678a39 | 368 | |
ThomBMT | 9:355dd95199c3 | 369 | void Going_Home() |
ThomBMT | 9:355dd95199c3 | 370 | { |
ThomBMT | 9:355dd95199c3 | 371 | //Here we will reset the position of the arm back to the home state |
ThomBMT | 9:355dd95199c3 | 372 | |
ThomBMT | 9:355dd95199c3 | 373 | if (counts1 == 0) // this 0 is a filler value and can later be determined to the angle of the |
ThomBMT | 9:355dd95199c3 | 374 | { // 0-state of the arm |
ThomBMT | 9:355dd95199c3 | 375 | PwmPin1=0.0f; |
ThomBMT | 9:355dd95199c3 | 376 | } |
ThomBMT | 9:355dd95199c3 | 377 | else if (counts1 > 0) |
ThomBMT | 9:355dd95199c3 | 378 | { |
ThomBMT | 9:355dd95199c3 | 379 | DirectionPin1 = true; //uitzoeken of dit klopt, is afhankelijk welke richting opgedraaid moet worden.. |
ThomBMT | 9:355dd95199c3 | 380 | PwmPin1 = 0.6f; |
ThomBMT | 9:355dd95199c3 | 381 | } |
ThomBMT | 9:355dd95199c3 | 382 | else if (counts1 < 0) |
ThomBMT | 9:355dd95199c3 | 383 | { |
ThomBMT | 9:355dd95199c3 | 384 | DirectionPin1 = false; |
ThomBMT | 9:355dd95199c3 | 385 | PwmPin1 = 0.6f; |
ThomBMT | 9:355dd95199c3 | 386 | } |
ThomBMT | 9:355dd95199c3 | 387 | |
ThomBMT | 9:355dd95199c3 | 388 | if (counts2 == 0) //Homing for M1 naar begin staat |
ThomBMT | 9:355dd95199c3 | 389 | { |
ThomBMT | 9:355dd95199c3 | 390 | PwmPin2=0.0f; |
ThomBMT | 9:355dd95199c3 | 391 | } |
ThomBMT | 9:355dd95199c3 | 392 | else if (counts2 > 0) |
ThomBMT | 9:355dd95199c3 | 393 | { |
ThomBMT | 9:355dd95199c3 | 394 | DirectionPin2 = true; //uitzoeken of dit klopt, is afhankelijk welke richting opgedraaid moet worden.. |
ThomBMT | 9:355dd95199c3 | 395 | PwmPin2 = 0.6f; |
ThomBMT | 9:355dd95199c3 | 396 | } |
ThomBMT | 9:355dd95199c3 | 397 | else if (counts2 < 0) |
ThomBMT | 9:355dd95199c3 | 398 | { |
ThomBMT | 9:355dd95199c3 | 399 | DirectionPin2 = false; |
ThomBMT | 9:355dd95199c3 | 400 | PwmPin2 = 0.6f; |
ThomBMT | 9:355dd95199c3 | 401 | } |
ThomBMT | 9:355dd95199c3 | 402 | |
ThomBMT | 9:355dd95199c3 | 403 | if (counts1 == 0 && counts2 == 0) |
ThomBMT | 9:355dd95199c3 | 404 | { |
ThomBMT | 9:355dd95199c3 | 405 | pc.printf("Homing is completed\r\nAutomatic switch to the Functioning State\r\n"); |
ThomBMT | 9:355dd95199c3 | 406 | Active_State = Function; |
ThomBMT | 9:355dd95199c3 | 407 | } |
ThomBMT | 9:355dd95199c3 | 408 | } |
ThomBMT | 9:355dd95199c3 | 409 | |
ThomBMT | 2:dc9766657afb | 410 | void Printing() |
ThomBMT | 2:dc9766657afb | 411 | { |
ThomBMT | 8:e8734a254818 | 412 | float v1 = PwmPin1 * maxVelocity; |
ThomBMT | 8:e8734a254818 | 413 | float v2 = PwmPin2 * maxVelocity; |
ThomBMT | 9:355dd95199c3 | 414 | |
ThomBMT | 9:355dd95199c3 | 415 | if (Active_State == Function) |
ThomBMT | 9:355dd95199c3 | 416 | { |
ThomBMT | 9:355dd95199c3 | 417 | pc.printf("q1 = %f [rad] \r\nq2 = %f [rad] \r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n\r\n\r\n\r\n", rad_m1, rad_m2, v1, v2); |
ThomBMT | 9:355dd95199c3 | 418 | } |
ThomBMT | 9:355dd95199c3 | 419 | |
ThomBMT | 9:355dd95199c3 | 420 | pc.printf("%f %f",vx, vy); |
ThomBMT | 2:dc9766657afb | 421 | } |
ThomBMT | 2:dc9766657afb | 422 | |
ThomBMT | 4:8f67b8327300 | 423 | void StateMachine() |
ThomBMT | 4:8f67b8327300 | 424 | { |
ThomBMT | 4:8f67b8327300 | 425 | switch (Active_State) |
ThomBMT | 4:8f67b8327300 | 426 | { |
ThomBMT | 8:e8734a254818 | 427 | case Starting: |
ThomBMT | 8:e8734a254818 | 428 | OFF(); |
ThomBMT | 9:355dd95199c3 | 429 | Start_Up(); |
ThomBMT | 9:355dd95199c3 | 430 | BlinkLed(); |
ThomBMT | 9:355dd95199c3 | 431 | |
ThomBMT | 9:355dd95199c3 | 432 | if (!Knop4 == true) |
ThomBMT | 9:355dd95199c3 | 433 | { |
ThomBMT | 9:355dd95199c3 | 434 | Active_State = Calibration; |
ThomBMT | 9:355dd95199c3 | 435 | pc.printf("Entering Calibration State \r\n"); |
ThomBMT | 9:355dd95199c3 | 436 | } |
ThomBMT | 9:355dd95199c3 | 437 | else if (!Knop3 == true) |
ThomBMT | 9:355dd95199c3 | 438 | { |
ThomBMT | 9:355dd95199c3 | 439 | Active_State = Homing; |
ThomBMT | 9:355dd95199c3 | 440 | pc.printf("Entering Homing State \r\n"); |
ThomBMT | 9:355dd95199c3 | 441 | } |
ThomBMT | 9:355dd95199c3 | 442 | |
ThomBMT | 8:e8734a254818 | 443 | break; |
ThomBMT | 8:e8734a254818 | 444 | |
ThomBMT | 4:8f67b8327300 | 445 | case Calibration: |
ThomBMT | 5:312186a0604d | 446 | //calibration actions |
ThomBMT | 5:312186a0604d | 447 | //pc.printf("Calibration State"); |
ThomBMT | 8:e8734a254818 | 448 | |
ThomBMT | 8:e8734a254818 | 449 | Calibrating(); |
ThomBMT | 8:e8734a254818 | 450 | OFF(); |
ThomBMT | 9:355dd95199c3 | 451 | |
ThomBMT | 9:355dd95199c3 | 452 | if (!Knop1 && !Knop2) |
ThomBMT | 5:312186a0604d | 453 | { |
ThomBMT | 9:355dd95199c3 | 454 | pc.printf("Switched to Sleeping State\r\n"); |
ThomBMT | 9:355dd95199c3 | 455 | Active_State = Starting; |
ThomBMT | 9:355dd95199c3 | 456 | iii = 40001; |
ThomBMT | 9:355dd95199c3 | 457 | } |
ThomBMT | 9:355dd95199c3 | 458 | else if (Knop1==false) |
ThomBMT | 9:355dd95199c3 | 459 | { |
ThomBMT | 9:355dd95199c3 | 460 | pc.printf("Manual switch to Homing state \r\n"); |
ThomBMT | 5:312186a0604d | 461 | Active_State = Homing; |
ThomBMT | 5:312186a0604d | 462 | } |
ThomBMT | 9:355dd95199c3 | 463 | |
ThomBMT | 9:355dd95199c3 | 464 | |
ThomBMT | 5:312186a0604d | 465 | |
ThomBMT | 5:312186a0604d | 466 | sample(); |
ThomBMT | 5:312186a0604d | 467 | EMG_Read(); |
ThomBMT | 5:312186a0604d | 468 | Encoding(); |
ThomBMT | 4:8f67b8327300 | 469 | break; |
ThomBMT | 4:8f67b8327300 | 470 | |
ThomBMT | 4:8f67b8327300 | 471 | case Homing: |
ThomBMT | 5:312186a0604d | 472 | //Homing actions |
ThomBMT | 5:312186a0604d | 473 | //pc.printf("Homing State"); |
ThomBMT | 9:355dd95199c3 | 474 | Going_Home(); |
ThomBMT | 9:355dd95199c3 | 475 | |
ThomBMT | 9:355dd95199c3 | 476 | if (!Knop1 && !Knop2) |
ThomBMT | 5:312186a0604d | 477 | { |
ThomBMT | 9:355dd95199c3 | 478 | pc.printf("Switched to Sleeping State\r\n"); |
ThomBMT | 9:355dd95199c3 | 479 | Active_State = Starting; |
ThomBMT | 9:355dd95199c3 | 480 | iii = 40000; |
ThomBMT | 9:355dd95199c3 | 481 | } |
ThomBMT | 9:355dd95199c3 | 482 | else if (Knop2==false) |
ThomBMT | 9:355dd95199c3 | 483 | { |
ThomBMT | 9:355dd95199c3 | 484 | pc.printf("Manual switch to Funtioning State \r\n"); |
ThomBMT | 5:312186a0604d | 485 | Active_State = Function; |
ThomBMT | 5:312186a0604d | 486 | } |
ThomBMT | 9:355dd95199c3 | 487 | else if (Knop3==false) |
ThomBMT | 9:355dd95199c3 | 488 | { |
ThomBMT | 9:355dd95199c3 | 489 | Active_State = Calibration; |
ThomBMT | 9:355dd95199c3 | 490 | pc.printf("Re-entering Calibration State \r\n"); |
ThomBMT | 9:355dd95199c3 | 491 | } |
ThomBMT | 9:355dd95199c3 | 492 | |
ThomBMT | 5:312186a0604d | 493 | |
ThomBMT | 5:312186a0604d | 494 | sample(); |
ThomBMT | 5:312186a0604d | 495 | EMG_Read(); |
ThomBMT | 5:312186a0604d | 496 | Encoding(); |
ThomBMT | 4:8f67b8327300 | 497 | break; |
ThomBMT | 4:8f67b8327300 | 498 | |
ThomBMT | 4:8f67b8327300 | 499 | case Function: |
ThomBMT | 5:312186a0604d | 500 | //pc.printf("Funtioning State"); |
ThomBMT | 8:e8734a254818 | 501 | |
ThomBMT | 9:355dd95199c3 | 502 | if (Knop4==false) |
ThomBMT | 8:e8734a254818 | 503 | { |
ThomBMT | 9:355dd95199c3 | 504 | pc.printf("Re-entering Calibration State \r\n"); |
ThomBMT | 5:312186a0604d | 505 | Active_State = Calibration; |
ThomBMT | 9:355dd95199c3 | 506 | ii=0; |
ThomBMT | 9:355dd95199c3 | 507 | } |
ThomBMT | 9:355dd95199c3 | 508 | else if (Knop3==false) |
ThomBMT | 9:355dd95199c3 | 509 | { |
ThomBMT | 9:355dd95199c3 | 510 | pc.printf("Re-entering Homing State \r\n"); |
ThomBMT | 9:355dd95199c3 | 511 | Active_State = Homing; |
ThomBMT | 9:355dd95199c3 | 512 | } |
ThomBMT | 9:355dd95199c3 | 513 | else if (!Knop1 && !Knop2) |
ThomBMT | 9:355dd95199c3 | 514 | { |
ThomBMT | 9:355dd95199c3 | 515 | pc.printf("Switched to Sleeping State\r\n"); |
ThomBMT | 9:355dd95199c3 | 516 | Active_State = Starting; |
ThomBMT | 9:355dd95199c3 | 517 | iii = 40000; |
ThomBMT | 5:312186a0604d | 518 | } |
ThomBMT | 8:e8734a254818 | 519 | |
ThomBMT | 5:312186a0604d | 520 | sample(); |
ThomBMT | 5:312186a0604d | 521 | EMG_Read(); |
ThomBMT | 5:312186a0604d | 522 | Encoding(); |
ThomBMT | 9:355dd95199c3 | 523 | //velocity1(); |
ThomBMT | 9:355dd95199c3 | 524 | //velocity2(); |
ThomBMT | 5:312186a0604d | 525 | motor1(); |
ThomBMT | 8:e8734a254818 | 526 | motor2(); |
ThomBMT | 4:8f67b8327300 | 527 | break; |
ThomBMT | 4:8f67b8327300 | 528 | |
ThomBMT | 4:8f67b8327300 | 529 | default: |
ThomBMT | 4:8f67b8327300 | 530 | pc.printf("UNKNOWN COMMAND"); |
ThomBMT | 4:8f67b8327300 | 531 | } |
ThomBMT | 4:8f67b8327300 | 532 | } |
ThomBMT | 4:8f67b8327300 | 533 | |
JurrienBos | 0:4591ba678a39 | 534 | int main() |
JurrienBos | 0:4591ba678a39 | 535 | { |
JurrienBos | 0:4591ba678a39 | 536 | pc.baud(115200); |
ThomBMT | 4:8f67b8327300 | 537 | PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz |
ThomBMT | 2:dc9766657afb | 538 | |
ThomBMT | 8:e8734a254818 | 539 | StateTicker.attach(&StateMachine, 0.002); |
ThomBMT | 4:8f67b8327300 | 540 | |
ThomBMT | 8:e8734a254818 | 541 | printTicker.attach(&Printing, 4.0); |
ThomBMT | 2:dc9766657afb | 542 | |
JurrienBos | 0:4591ba678a39 | 543 | while(true) |
ThomBMT | 5:312186a0604d | 544 | { |
JurrienBos | 0:4591ba678a39 | 545 | } |
JurrienBos | 1:4bf64d003f3a | 546 | } |