Thom Kuenen / Mbed 2 deprecated Turning_Motor_V3

Project of Biorobotics

Dependencies:   HIDScope MODSERIAL QEI mbed biquadFilter

Fork of TutorialPES by Jurriën Bos

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?

UserRevisionLine numberNew 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 }