Project of Biorobotics
Dependencies: HIDScope MODSERIAL QEI mbed biquadFilter
Fork of TutorialPES by
main.cpp@8:e8734a254818, 2018-10-29 (annotated)
- Committer:
- ThomBMT
- Date:
- Mon Oct 29 14:02:21 2018 +0000
- Revision:
- 8:e8734a254818
- Parent:
- 7:439940ae1197
- Child:
- 9:355dd95199c3
Calibration AND Inverse kinamatics are added and work (probably)
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); |
JurrienBos | 0:4591ba678a39 | 14 | AnalogIn pot1 (A5); |
JurrienBos | 1:4bf64d003f3a | 15 | AnalogIn pot2 (A4); |
ThomBMT | 2:dc9766657afb | 16 | AnalogIn emg0( A0 ); |
ThomBMT | 2:dc9766657afb | 17 | AnalogIn emg1( A1 ); |
ThomBMT | 2:dc9766657afb | 18 | AnalogIn emg2( A2 ); |
ThomBMT | 2:dc9766657afb | 19 | AnalogIn emg3( A3 ); |
ThomBMT | 8:e8734a254818 | 20 | DigitalOut led(LED_GREEN); |
JurrienBos | 0:4591ba678a39 | 21 | |
ThomBMT | 8:e8734a254818 | 22 | QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); |
ThomBMT | 5:312186a0604d | 23 | QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); |
ThomBMT | 5:312186a0604d | 24 | |
ThomBMT | 4:8f67b8327300 | 25 | Ticker StateTicker; |
ThomBMT | 2:dc9766657afb | 26 | Ticker printTicker; |
ThomBMT | 5:312186a0604d | 27 | |
ThomBMT | 2:dc9766657afb | 28 | HIDScope scope( 4 ); |
JurrienBos | 0:4591ba678a39 | 29 | |
ThomBMT | 3:c8f0fc045505 | 30 | volatile float Bicep_Right = 0.0; |
ThomBMT | 3:c8f0fc045505 | 31 | volatile float Bicep_Left = 0.0; |
ThomBMT | 3:c8f0fc045505 | 32 | volatile float Tricep_Right = 0.0; |
ThomBMT | 3:c8f0fc045505 | 33 | volatile float Tricep_Left = 0.0; |
ThomBMT | 8:e8734a254818 | 34 | |
ThomBMT | 3:c8f0fc045505 | 35 | volatile const float maxVelocity = 8.4; // in rad/s |
ThomBMT | 7:439940ae1197 | 36 | volatile const float pi = 3.1415926; |
ThomBMT | 7:439940ae1197 | 37 | volatile const float rad_count = 0.0007479; // 2pi/8400; |
ThomBMT | 8:e8734a254818 | 38 | |
ThomBMT | 3:c8f0fc045505 | 39 | volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand |
ThomBMT | 3:c8f0fc045505 | 40 | volatile float referenceVelocity2 = 0.5; |
ThomBMT | 2:dc9766657afb | 41 | |
ThomBMT | 8:e8734a254818 | 42 | volatile float q_1; |
ThomBMT | 8:e8734a254818 | 43 | volatile float q_2; |
ThomBMT | 8:e8734a254818 | 44 | volatile float r_1; |
ThomBMT | 8:e8734a254818 | 45 | volatile float r_2; |
ThomBMT | 8:e8734a254818 | 46 | volatile const float r_3 = 0.035; |
ThomBMT | 8:e8734a254818 | 47 | volatile float w_1; |
ThomBMT | 8:e8734a254818 | 48 | volatile float w_2; |
ThomBMT | 8:e8734a254818 | 49 | |
ThomBMT | 8:e8734a254818 | 50 | volatile float Flex_Bi_R; |
ThomBMT | 8:e8734a254818 | 51 | volatile float Flex_Bi_L; |
ThomBMT | 8:e8734a254818 | 52 | volatile float Flex_Tri_R; |
ThomBMT | 8:e8734a254818 | 53 | volatile float Flex_Tri_L; |
ThomBMT | 8:e8734a254818 | 54 | |
ThomBMT | 8:e8734a254818 | 55 | |
ThomBMT | 8:e8734a254818 | 56 | enum states{Starting, Calibration, Homing, Function}; |
ThomBMT | 4:8f67b8327300 | 57 | |
ThomBMT | 8:e8734a254818 | 58 | volatile states Active_State = Starting; |
ThomBMT | 4:8f67b8327300 | 59 | |
ThomBMT | 5:312186a0604d | 60 | volatile int counts1 ; |
ThomBMT | 5:312186a0604d | 61 | volatile int counts2 ; |
ThomBMT | 5:312186a0604d | 62 | volatile float rad_m1; |
ThomBMT | 5:312186a0604d | 63 | volatile float rad_m2; |
ThomBMT | 4:8f67b8327300 | 64 | |
ThomBMT | 3:c8f0fc045505 | 65 | void Encoding() |
ThomBMT | 3:c8f0fc045505 | 66 | { |
ThomBMT | 5:312186a0604d | 67 | |
ThomBMT | 3:c8f0fc045505 | 68 | counts1 = Encoder1.getPulses(); |
ThomBMT | 3:c8f0fc045505 | 69 | counts2 = Encoder2.getPulses(); |
ThomBMT | 4:8f67b8327300 | 70 | |
ThomBMT | 7:439940ae1197 | 71 | rad_m1 = rad_count * (float)counts1; |
ThomBMT | 7:439940ae1197 | 72 | rad_m2 = rad_count * (float)counts2; |
ThomBMT | 3:c8f0fc045505 | 73 | } |
ThomBMT | 2:dc9766657afb | 74 | |
ThomBMT | 2:dc9766657afb | 75 | void EMG_Read() |
ThomBMT | 2:dc9766657afb | 76 | { |
ThomBMT | 3:c8f0fc045505 | 77 | Bicep_Right = emg0.read(); |
ThomBMT | 3:c8f0fc045505 | 78 | Bicep_Left = emg1.read(); |
ThomBMT | 3:c8f0fc045505 | 79 | Tricep_Right = emg2.read(); |
ThomBMT | 3:c8f0fc045505 | 80 | Tricep_Left = emg3.read(); |
ThomBMT | 2:dc9766657afb | 81 | } |
ThomBMT | 2:dc9766657afb | 82 | |
ThomBMT | 2:dc9766657afb | 83 | void sample() |
ThomBMT | 2:dc9766657afb | 84 | { |
ThomBMT | 2:dc9766657afb | 85 | |
ThomBMT | 2:dc9766657afb | 86 | scope.set(0, emg0.read() ); |
ThomBMT | 2:dc9766657afb | 87 | scope.set(1, emg1.read() ); |
ThomBMT | 2:dc9766657afb | 88 | scope.set(2, emg2.read() ); |
ThomBMT | 2:dc9766657afb | 89 | scope.set(3, emg3.read() ); |
ThomBMT | 2:dc9766657afb | 90 | |
ThomBMT | 2:dc9766657afb | 91 | scope.send(); |
ThomBMT | 2:dc9766657afb | 92 | } |
ThomBMT | 2:dc9766657afb | 93 | |
ThomBMT | 8:e8734a254818 | 94 | void Inverse() |
ThomBMT | 8:e8734a254818 | 95 | { |
ThomBMT | 8:e8734a254818 | 96 | q_1= rad_m1; // uit Encoder |
ThomBMT | 8:e8734a254818 | 97 | q_2= rad_m2+(pi/2.0f); // uit Encoder |
ThomBMT | 8:e8734a254818 | 98 | r_1= -0.2f; |
ThomBMT | 8:e8734a254818 | 99 | r_2= -0.2f; |
ThomBMT | 8:e8734a254818 | 100 | |
ThomBMT | 8:e8734a254818 | 101 | float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2); |
ThomBMT | 8:e8734a254818 | 102 | float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3; |
ThomBMT | 8:e8734a254818 | 103 | 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 | 104 | float x = (-2.0f)*r_2*sin(q_1)*cos(q_2); |
ThomBMT | 8:e8734a254818 | 105 | float D = 1.0f/(u*z-x*y); // Determinant |
ThomBMT | 8:e8734a254818 | 106 | printf("Determinant is %f\n", D); |
ThomBMT | 8:e8734a254818 | 107 | |
ThomBMT | 8:e8734a254818 | 108 | float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix |
ThomBMT | 8:e8734a254818 | 109 | float b = -D*x; // Inverse jacobian |
ThomBMT | 8:e8734a254818 | 110 | float c = -D*y; // Inverse jacobian |
ThomBMT | 8:e8734a254818 | 111 | float d = D*u; // Inverse jacobian |
ThomBMT | 8:e8734a254818 | 112 | |
ThomBMT | 8:e8734a254818 | 113 | float vx = 0.01f; // uit emg data |
ThomBMT | 8:e8734a254818 | 114 | float vy = 0.0f; // uit emg data |
ThomBMT | 8:e8734a254818 | 115 | |
ThomBMT | 8:e8734a254818 | 116 | w_1 = vx*a+vy*b; |
ThomBMT | 8:e8734a254818 | 117 | w_2 = vx*c+vy*d; |
ThomBMT | 8:e8734a254818 | 118 | |
ThomBMT | 8:e8734a254818 | 119 | /* |
ThomBMT | 8:e8734a254818 | 120 | printf("%f\n", w_1); |
ThomBMT | 8:e8734a254818 | 121 | printf("%f\n", w_2); |
ThomBMT | 8:e8734a254818 | 122 | */ |
ThomBMT | 8:e8734a254818 | 123 | } |
JurrienBos | 1:4bf64d003f3a | 124 | |
JurrienBos | 1:4bf64d003f3a | 125 | void velocity1() |
JurrienBos | 0:4591ba678a39 | 126 | { |
ThomBMT | 3:c8f0fc045505 | 127 | if (pot1.read()>0.5f) |
JurrienBos | 0:4591ba678a39 | 128 | { |
JurrienBos | 0:4591ba678a39 | 129 | // Clockwise rotation |
JurrienBos | 1:4bf64d003f3a | 130 | referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; |
JurrienBos | 0:4591ba678a39 | 131 | } |
JurrienBos | 0:4591ba678a39 | 132 | |
ThomBMT | 5:312186a0604d | 133 | else if (pot1.read() == 0.5f) |
JurrienBos | 0:4591ba678a39 | 134 | { |
JurrienBos | 1:4bf64d003f3a | 135 | referenceVelocity1 = pot1.read() * 0.0f; |
JurrienBos | 0:4591ba678a39 | 136 | } |
JurrienBos | 0:4591ba678a39 | 137 | |
JurrienBos | 0:4591ba678a39 | 138 | else if (pot1.read() < 0.5f) |
JurrienBos | 0:4591ba678a39 | 139 | { |
JurrienBos | 0:4591ba678a39 | 140 | // Counterclockwise rotation |
JurrienBos | 1:4bf64d003f3a | 141 | referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ; |
JurrienBos | 0:4591ba678a39 | 142 | } |
JurrienBos | 0:4591ba678a39 | 143 | } |
JurrienBos | 0:4591ba678a39 | 144 | |
JurrienBos | 1:4bf64d003f3a | 145 | void velocity2() |
JurrienBos | 1:4bf64d003f3a | 146 | { |
JurrienBos | 1:4bf64d003f3a | 147 | if (pot2.read()>0.5f) |
JurrienBos | 1:4bf64d003f3a | 148 | { |
JurrienBos | 1:4bf64d003f3a | 149 | // Clockwise rotation |
JurrienBos | 1:4bf64d003f3a | 150 | referenceVelocity2 = (pot2.read()-0.5f) * 2.0f; |
JurrienBos | 1:4bf64d003f3a | 151 | } |
JurrienBos | 1:4bf64d003f3a | 152 | |
JurrienBos | 1:4bf64d003f3a | 153 | else if (pot2.read() == 0.5f) |
JurrienBos | 1:4bf64d003f3a | 154 | { |
JurrienBos | 1:4bf64d003f3a | 155 | referenceVelocity2 = pot2.read() * 0.0f; |
JurrienBos | 1:4bf64d003f3a | 156 | } |
JurrienBos | 1:4bf64d003f3a | 157 | |
JurrienBos | 1:4bf64d003f3a | 158 | else if (pot2.read() < 0.5f) |
JurrienBos | 1:4bf64d003f3a | 159 | { |
JurrienBos | 1:4bf64d003f3a | 160 | // Counterclockwise rotation |
JurrienBos | 1:4bf64d003f3a | 161 | referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ; |
JurrienBos | 1:4bf64d003f3a | 162 | } |
JurrienBos | 1:4bf64d003f3a | 163 | } |
JurrienBos | 1:4bf64d003f3a | 164 | |
JurrienBos | 1:4bf64d003f3a | 165 | void motor1() |
ThomBMT | 8:e8734a254818 | 166 | { |
ThomBMT | 8:e8734a254818 | 167 | float u_v1 = w_1; //referenceVelocity1 |
ThomBMT | 8:e8734a254818 | 168 | float u = u_v1/ (2.0f * pi); |
JurrienBos | 0:4591ba678a39 | 169 | DirectionPin1 = u < 0.0f; |
JurrienBos | 0:4591ba678a39 | 170 | PwmPin1 = fabs(u); |
JurrienBos | 0:4591ba678a39 | 171 | } |
JurrienBos | 0:4591ba678a39 | 172 | |
JurrienBos | 1:4bf64d003f3a | 173 | void motor2() |
JurrienBos | 1:4bf64d003f3a | 174 | { |
ThomBMT | 8:e8734a254818 | 175 | float u_v2 = w_2; //referenceVelocity2 |
ThomBMT | 8:e8734a254818 | 176 | float u = u_v2/ (2.0f * pi); |
JurrienBos | 1:4bf64d003f3a | 177 | DirectionPin2 = u > 0.0f; |
JurrienBos | 1:4bf64d003f3a | 178 | PwmPin2 = fabs(u); |
JurrienBos | 1:4bf64d003f3a | 179 | } |
ThomBMT | 8:e8734a254818 | 180 | |
ThomBMT | 8:e8734a254818 | 181 | void Calibrating() |
ThomBMT | 8:e8734a254818 | 182 | { |
ThomBMT | 8:e8734a254818 | 183 | static float n = 0.0; |
ThomBMT | 8:e8734a254818 | 184 | static float m = 0.0; |
ThomBMT | 8:e8734a254818 | 185 | static float l = 0.0; |
ThomBMT | 8:e8734a254818 | 186 | static float k = 0.0; |
ThomBMT | 8:e8734a254818 | 187 | |
ThomBMT | 8:e8734a254818 | 188 | for(int ii=0; ii<=20000; ii++) |
ThomBMT | 8:e8734a254818 | 189 | { |
ThomBMT | 8:e8734a254818 | 190 | if (ii <2500) |
ThomBMT | 8:e8734a254818 | 191 | { |
ThomBMT | 8:e8734a254818 | 192 | n = n + emg0.read(); |
ThomBMT | 8:e8734a254818 | 193 | } |
ThomBMT | 8:e8734a254818 | 194 | else if (ii == 2500) |
ThomBMT | 8:e8734a254818 | 195 | { |
ThomBMT | 8:e8734a254818 | 196 | Flex_Bi_R = n / 2500.0f; |
ThomBMT | 8:e8734a254818 | 197 | } |
ThomBMT | 8:e8734a254818 | 198 | else if (ii>2500 && ii<=3500) |
ThomBMT | 8:e8734a254818 | 199 | { |
ThomBMT | 8:e8734a254818 | 200 | //chillen |
ThomBMT | 8:e8734a254818 | 201 | } |
ThomBMT | 8:e8734a254818 | 202 | else if(ii>3500 && ii<6000) |
ThomBMT | 8:e8734a254818 | 203 | { |
ThomBMT | 8:e8734a254818 | 204 | m = m + emg1.read(); |
ThomBMT | 8:e8734a254818 | 205 | } |
ThomBMT | 8:e8734a254818 | 206 | else if (ii == 6000) |
ThomBMT | 8:e8734a254818 | 207 | { |
ThomBMT | 8:e8734a254818 | 208 | Flex_Bi_L = m / 2500.0f; |
ThomBMT | 8:e8734a254818 | 209 | } |
ThomBMT | 8:e8734a254818 | 210 | else if (ii>6000 && ii<=7000) |
ThomBMT | 8:e8734a254818 | 211 | { |
ThomBMT | 8:e8734a254818 | 212 | //chillen |
ThomBMT | 8:e8734a254818 | 213 | } |
ThomBMT | 8:e8734a254818 | 214 | else if (ii>7000 && ii<9500) |
ThomBMT | 8:e8734a254818 | 215 | { |
ThomBMT | 8:e8734a254818 | 216 | l = l + emg2.read(); |
ThomBMT | 8:e8734a254818 | 217 | } |
ThomBMT | 8:e8734a254818 | 218 | else if (ii == 9500) |
ThomBMT | 8:e8734a254818 | 219 | { |
ThomBMT | 8:e8734a254818 | 220 | Flex_Tri_R = l / 2500.0f; |
ThomBMT | 8:e8734a254818 | 221 | } |
ThomBMT | 8:e8734a254818 | 222 | else if (ii>9500 && ii <=10500) |
ThomBMT | 8:e8734a254818 | 223 | { |
ThomBMT | 8:e8734a254818 | 224 | //chillen |
ThomBMT | 8:e8734a254818 | 225 | } |
ThomBMT | 8:e8734a254818 | 226 | else if (ii>10500 && ii<13000) |
ThomBMT | 8:e8734a254818 | 227 | { |
ThomBMT | 8:e8734a254818 | 228 | k = k + emg3.read(); |
ThomBMT | 8:e8734a254818 | 229 | } |
ThomBMT | 8:e8734a254818 | 230 | else if (ii == 13000) |
ThomBMT | 8:e8734a254818 | 231 | { |
ThomBMT | 8:e8734a254818 | 232 | Flex_Tri_L = k / 2500.0f; |
ThomBMT | 8:e8734a254818 | 233 | } |
ThomBMT | 8:e8734a254818 | 234 | } |
ThomBMT | 8:e8734a254818 | 235 | } |
ThomBMT | 8:e8734a254818 | 236 | |
ThomBMT | 8:e8734a254818 | 237 | void OFF() |
ThomBMT | 8:e8734a254818 | 238 | { |
ThomBMT | 8:e8734a254818 | 239 | PwmPin1 = PwmPin2 = 0; |
ThomBMT | 8:e8734a254818 | 240 | } |
JurrienBos | 0:4591ba678a39 | 241 | |
ThomBMT | 2:dc9766657afb | 242 | void Printing() |
ThomBMT | 2:dc9766657afb | 243 | { |
ThomBMT | 8:e8734a254818 | 244 | float v1 = PwmPin1 * maxVelocity; |
ThomBMT | 8:e8734a254818 | 245 | float v2 = PwmPin2 * maxVelocity; |
ThomBMT | 8:e8734a254818 | 246 | |
ThomBMT | 8:e8734a254818 | 247 | pc.printf("q1 = %f [rad] \n q2 = %f [rad] \n q1dot = %f [rad/s] \n q2dot = %f [rad/s] \n\n\n\n\n", rad_m1, rad_m2, v1, v2); |
ThomBMT | 2:dc9766657afb | 248 | } |
ThomBMT | 2:dc9766657afb | 249 | |
ThomBMT | 4:8f67b8327300 | 250 | void StateMachine() |
ThomBMT | 4:8f67b8327300 | 251 | { |
ThomBMT | 4:8f67b8327300 | 252 | switch (Active_State) |
ThomBMT | 4:8f67b8327300 | 253 | { |
ThomBMT | 8:e8734a254818 | 254 | case Starting: |
ThomBMT | 8:e8734a254818 | 255 | OFF(); |
ThomBMT | 8:e8734a254818 | 256 | for (int i = 0; i<=20; i++) |
ThomBMT | 8:e8734a254818 | 257 | { |
ThomBMT | 8:e8734a254818 | 258 | led = !led; |
ThomBMT | 8:e8734a254818 | 259 | wait(0.05); |
ThomBMT | 8:e8734a254818 | 260 | |
ThomBMT | 8:e8734a254818 | 261 | if (i == 0) |
ThomBMT | 8:e8734a254818 | 262 | { |
ThomBMT | 8:e8734a254818 | 263 | pc.printf("Starting up..\n"); |
ThomBMT | 8:e8734a254818 | 264 | } |
ThomBMT | 8:e8734a254818 | 265 | else if (i == 20) |
ThomBMT | 8:e8734a254818 | 266 | { |
ThomBMT | 8:e8734a254818 | 267 | Active_State = Calibration; |
ThomBMT | 8:e8734a254818 | 268 | pc.printf("Entering Calibration state \n"); |
ThomBMT | 8:e8734a254818 | 269 | } |
ThomBMT | 8:e8734a254818 | 270 | } |
ThomBMT | 8:e8734a254818 | 271 | break; |
ThomBMT | 8:e8734a254818 | 272 | |
ThomBMT | 4:8f67b8327300 | 273 | case Calibration: |
ThomBMT | 5:312186a0604d | 274 | //calibration actions |
ThomBMT | 5:312186a0604d | 275 | //pc.printf("Calibration State"); |
ThomBMT | 8:e8734a254818 | 276 | |
ThomBMT | 8:e8734a254818 | 277 | Calibrating(); |
ThomBMT | 8:e8734a254818 | 278 | OFF(); |
ThomBMT | 5:312186a0604d | 279 | if (Knop1==false) |
ThomBMT | 5:312186a0604d | 280 | { |
ThomBMT | 5:312186a0604d | 281 | pc.printf("Entering Homing state \n"); |
ThomBMT | 5:312186a0604d | 282 | Active_State = Homing; |
ThomBMT | 5:312186a0604d | 283 | } |
ThomBMT | 5:312186a0604d | 284 | |
ThomBMT | 5:312186a0604d | 285 | sample(); |
ThomBMT | 5:312186a0604d | 286 | EMG_Read(); |
ThomBMT | 5:312186a0604d | 287 | Encoding(); |
ThomBMT | 4:8f67b8327300 | 288 | break; |
ThomBMT | 4:8f67b8327300 | 289 | |
ThomBMT | 4:8f67b8327300 | 290 | case Homing: |
ThomBMT | 5:312186a0604d | 291 | //Homing actions |
ThomBMT | 5:312186a0604d | 292 | //pc.printf("Homing State"); |
ThomBMT | 5:312186a0604d | 293 | if (Knop2==false) |
ThomBMT | 5:312186a0604d | 294 | { |
ThomBMT | 5:312186a0604d | 295 | pc.printf("Entering Funtioning State \n"); |
ThomBMT | 5:312186a0604d | 296 | Active_State = Function; |
ThomBMT | 5:312186a0604d | 297 | } |
ThomBMT | 5:312186a0604d | 298 | |
ThomBMT | 5:312186a0604d | 299 | sample(); |
ThomBMT | 5:312186a0604d | 300 | EMG_Read(); |
ThomBMT | 5:312186a0604d | 301 | Encoding(); |
ThomBMT | 4:8f67b8327300 | 302 | break; |
ThomBMT | 4:8f67b8327300 | 303 | |
ThomBMT | 4:8f67b8327300 | 304 | case Function: |
ThomBMT | 5:312186a0604d | 305 | //pc.printf("Funtioning State"); |
ThomBMT | 8:e8734a254818 | 306 | |
ThomBMT | 8:e8734a254818 | 307 | if (Knop3==false) |
ThomBMT | 8:e8734a254818 | 308 | { |
ThomBMT | 5:312186a0604d | 309 | pc.printf("Re-entering Calibration State \n"); |
ThomBMT | 5:312186a0604d | 310 | Active_State = Calibration; |
ThomBMT | 5:312186a0604d | 311 | } |
ThomBMT | 8:e8734a254818 | 312 | |
ThomBMT | 5:312186a0604d | 313 | sample(); |
ThomBMT | 5:312186a0604d | 314 | EMG_Read(); |
ThomBMT | 5:312186a0604d | 315 | Encoding(); |
ThomBMT | 5:312186a0604d | 316 | velocity1(); |
ThomBMT | 5:312186a0604d | 317 | velocity2(); |
ThomBMT | 5:312186a0604d | 318 | motor1(); |
ThomBMT | 8:e8734a254818 | 319 | motor2(); |
ThomBMT | 4:8f67b8327300 | 320 | break; |
ThomBMT | 4:8f67b8327300 | 321 | |
ThomBMT | 4:8f67b8327300 | 322 | default: |
ThomBMT | 4:8f67b8327300 | 323 | pc.printf("UNKNOWN COMMAND"); |
ThomBMT | 4:8f67b8327300 | 324 | } |
ThomBMT | 4:8f67b8327300 | 325 | } |
ThomBMT | 4:8f67b8327300 | 326 | |
JurrienBos | 0:4591ba678a39 | 327 | int main() |
JurrienBos | 0:4591ba678a39 | 328 | { |
JurrienBos | 0:4591ba678a39 | 329 | pc.baud(115200); |
ThomBMT | 4:8f67b8327300 | 330 | PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz |
ThomBMT | 2:dc9766657afb | 331 | |
ThomBMT | 8:e8734a254818 | 332 | StateTicker.attach(&StateMachine, 0.002); |
ThomBMT | 4:8f67b8327300 | 333 | |
ThomBMT | 8:e8734a254818 | 334 | printTicker.attach(&Printing, 4.0); |
ThomBMT | 2:dc9766657afb | 335 | |
JurrienBos | 0:4591ba678a39 | 336 | while(true) |
ThomBMT | 5:312186a0604d | 337 | { |
JurrienBos | 0:4591ba678a39 | 338 | } |
JurrienBos | 1:4bf64d003f3a | 339 | } |