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@11:d525527c0b7d, 2018-11-01 (annotated)

Committer:
ThomBMT
Date:
Thu Nov 01 08:16:39 2018 +0000
Revision:
11:d525527c0b7d
Parent:
10:05ad15c48388 
V_4;

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 10:05ad15c48388 5 #include "BiQuad.h"
ThomBMT 2:dc9766657afb 6
JurrienBos 0:4591ba678a39 7 MODSERIAL pc(USBTX, USBRX);
JurrienBos 0:4591ba678a39 8 DigitalOut DirectionPin1(D4);
JurrienBos 1:4bf64d003f3a 9 DigitalOut DirectionPin2(D7);
JurrienBos 0:4591ba678a39 10 PwmOut PwmPin1(D5);
JurrienBos 1:4bf64d003f3a 11 PwmOut PwmPin2(D6);
JurrienBos 0:4591ba678a39 12 DigitalIn Knop1(D2);
ThomBMT 4:8f67b8327300 13 DigitalIn Knop2(D3);
ThomBMT 8:e8734a254818 14 DigitalIn Knop3(PTA4);
ThomBMT 9:355dd95199c3 15 DigitalIn Knop4(PTC6);
JurrienBos 0:4591ba678a39 16 AnalogIn pot1 (A5);
JurrienBos 1:4bf64d003f3a 17 AnalogIn pot2 (A4);
ThomBMT 2:dc9766657afb 18 AnalogIn emg0( A0 );
ThomBMT 2:dc9766657afb 19 AnalogIn emg1( A1 );
ThomBMT 2:dc9766657afb 20 AnalogIn emg2( A2 );
ThomBMT 2:dc9766657afb 21 AnalogIn emg3( A3 );
ThomBMT 9:355dd95199c3 22 DigitalOut led_G(LED_GREEN);
ThomBMT 9:355dd95199c3 23 DigitalOut led_R(LED_RED);
ThomBMT 9:355dd95199c3 24 DigitalOut led_B(LED_BLUE);
JurrienBos 0:4591ba678a39 25
ThomBMT 8:e8734a254818 26 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
ThomBMT 5:312186a0604d 27 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
ThomBMT 5:312186a0604d 28
ThomBMT 10:05ad15c48388 29 BiQuadChain bqc1;
ThomBMT 10:05ad15c48388 30 BiQuadChain bqc2;
ThomBMT 10:05ad15c48388 31 BiQuadChain bqc3;
ThomBMT 10:05ad15c48388 32 BiQuadChain bqc4;
ThomBMT 10:05ad15c48388 33 BiQuadChain bqc5;
ThomBMT 10:05ad15c48388 34 BiQuadChain bqc6;
ThomBMT 10:05ad15c48388 35 BiQuadChain bqc7;
ThomBMT 10:05ad15c48388 36 BiQuadChain bqc8;
ThomBMT 10:05ad15c48388 37
ThomBMT 10:05ad15c48388 38 BiQuad BqNotch1_1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
ThomBMT 10:05ad15c48388 39 BiQuad BqNotch2_1( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
ThomBMT 10:05ad15c48388 40 BiQuad BqNotch1_2( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
ThomBMT 10:05ad15c48388 41 BiQuad BqNotch2_2( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
ThomBMT 10:05ad15c48388 42 BiQuad BqNotch1_3( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
ThomBMT 10:05ad15c48388 43 BiQuad BqNotch2_3( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
ThomBMT 10:05ad15c48388 44 BiQuad BqNotch1_4( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
ThomBMT 10:05ad15c48388 45 BiQuad BqNotch2_4( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
ThomBMT 10:05ad15c48388 46 BiQuad BqHP1( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
ThomBMT 10:05ad15c48388 47 BiQuad BqHP2( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
ThomBMT 10:05ad15c48388 48 BiQuad BqHP3( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
ThomBMT 10:05ad15c48388 49 BiQuad BqHP4( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
ThomBMT 10:05ad15c48388 50
ThomBMT 10:05ad15c48388 51 BiQuad BqLP1( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
ThomBMT 10:05ad15c48388 52 BiQuad BqLP2( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
ThomBMT 10:05ad15c48388 53 BiQuad BqLP3( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
ThomBMT 10:05ad15c48388 54 BiQuad BqLP4( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
ThomBMT 10:05ad15c48388 55
ThomBMT 10:05ad15c48388 56 BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
ThomBMT 10:05ad15c48388 57
ThomBMT 4:8f67b8327300 58 Ticker StateTicker;
ThomBMT 2:dc9766657afb 59 Ticker printTicker;
ThomBMT 5:312186a0604d 60
ThomBMT 2:dc9766657afb 61 HIDScope scope( 4 );
JurrienBos 0:4591ba678a39 62
ThomBMT 10:05ad15c48388 63 volatile float Bicep_Right = 0.0;
ThomBMT 10:05ad15c48388 64 volatile float Bicep_Left = 0.0;
ThomBMT 10:05ad15c48388 65 volatile float Tricep_Right = 0.0;
ThomBMT 10:05ad15c48388 66 volatile float Tricep_Left = 0.0;
ThomBMT 10:05ad15c48388 67 volatile float FilterHP_Bi_R;
ThomBMT 10:05ad15c48388 68 volatile float Filterabs_Bi_R;
ThomBMT 10:05ad15c48388 69 volatile float Filtered_Bi_R;
ThomBMT 10:05ad15c48388 70 volatile float FilterHP_Bi_L;
ThomBMT 10:05ad15c48388 71 volatile float Filterabs_Bi_L;
ThomBMT 10:05ad15c48388 72 volatile float Filtered_Bi_L;
ThomBMT 10:05ad15c48388 73 volatile float FilterHP_Tri_R;
ThomBMT 10:05ad15c48388 74 volatile float Filterabs_Tri_R;
ThomBMT 10:05ad15c48388 75 volatile float Filtered_Tri_R;
ThomBMT 10:05ad15c48388 76 volatile float FilterHP_Tri_L;
ThomBMT 10:05ad15c48388 77 volatile float Filterabs_Tri_L;
ThomBMT 10:05ad15c48388 78 volatile float Filtered_Tri_L;
ThomBMT 10:05ad15c48388 79
ThomBMT 10:05ad15c48388 80 volatile float error_1_integral = 0;
ThomBMT 10:05ad15c48388 81 volatile float error_2_integral = 0;
ThomBMT 10:05ad15c48388 82 volatile float error_1_prev; // initialization with this value only done once!
ThomBMT 10:05ad15c48388 83 volatile float error_2_prev;
ThomBMT 8:e8734a254818 84
ThomBMT 7:439940ae1197 85 volatile const float pi = 3.1415926;
ThomBMT 9:355dd95199c3 86 volatile const float rad_count = 0.0007479; // 2pi/8400;
ThomBMT 9:355dd95199c3 87 volatile const float maxVelocity = 2.0f * pi; // in rad/s
ThomBMT 10:05ad15c48388 88 volatile const float r_3 = 0.035;
ThomBMT 8:e8734a254818 89
ThomBMT 9:355dd95199c3 90 volatile float referenceVelocity1 = 0.5; // dit is de gecentreerde waarde en dus de nulstand
ThomBMT 3:c8f0fc045505 91 volatile float referenceVelocity2 = 0.5;
ThomBMT 2:dc9766657afb 92
ThomBMT 9:355dd95199c3 93 volatile int i = 0; // Led blink status
ThomBMT 9:355dd95199c3 94 volatile int ii = 0; // Calibratie timer
ThomBMT 9:355dd95199c3 95 volatile int iii = 0; // Start up timer
ThomBMT 9:355dd95199c3 96
ThomBMT 10:05ad15c48388 97 volatile float Kp = 17.5;
ThomBMT 10:05ad15c48388 98 volatile float Ki = 1.02;
ThomBMT 10:05ad15c48388 99 volatile float Kd = 23.2;
ThomBMT 10:05ad15c48388 100 volatile float Ts = 0.01; // Sample time in seconds
ThomBMT 10:05ad15c48388 101 volatile float error_1;
ThomBMT 10:05ad15c48388 102 volatile float error_2;
ThomBMT 10:05ad15c48388 103 volatile float U_1;
ThomBMT 10:05ad15c48388 104 volatile float U_2;
ThomBMT 10:05ad15c48388 105
ThomBMT 8:e8734a254818 106 volatile float q_1;
ThomBMT 8:e8734a254818 107 volatile float q_2;
ThomBMT 8:e8734a254818 108 volatile float r_1;
ThomBMT 8:e8734a254818 109 volatile float r_2;
ThomBMT 8:e8734a254818 110 volatile float w_1;
ThomBMT 8:e8734a254818 111 volatile float w_2;
ThomBMT 8:e8734a254818 112
ThomBMT 8:e8734a254818 113 volatile float Flex_Bi_R;
ThomBMT 8:e8734a254818 114 volatile float Flex_Bi_L;
ThomBMT 8:e8734a254818 115 volatile float Flex_Tri_R;
ThomBMT 8:e8734a254818 116 volatile float Flex_Tri_L;
ThomBMT 10:05ad15c48388 117 volatile float Threshold_Value;
ThomBMT 9:355dd95199c3 118 volatile float Threshold_Bi_R;
ThomBMT 9:355dd95199c3 119 volatile float Threshold_Bi_L;
ThomBMT 9:355dd95199c3 120 volatile float Threshold_Tri_R;
ThomBMT 9:355dd95199c3 121 volatile float Threshold_Tri_L;
ThomBMT 8:e8734a254818 122
ThomBMT 10:05ad15c48388 123 enum states{Starting, Calibration, Homing_M1, Homing_M2, Post_Homing, Function, Safe};
ThomBMT 4:8f67b8327300 124
ThomBMT 8:e8734a254818 125 volatile states Active_State = Starting;
ThomBMT 4:8f67b8327300 126
ThomBMT 9:355dd95199c3 127 volatile float vx;
ThomBMT 9:355dd95199c3 128 volatile float vy;
ThomBMT 10:05ad15c48388 129 volatile int counts1;
ThomBMT 10:05ad15c48388 130 volatile int counts2;
ThomBMT 5:312186a0604d 131 volatile float rad_m1;
ThomBMT 5:312186a0604d 132 volatile float rad_m2;
ThomBMT 4:8f67b8327300 133
ThomBMT 3:c8f0fc045505 134 void Encoding()
ThomBMT 3:c8f0fc045505 135 {
ThomBMT 5:312186a0604d 136
ThomBMT 3:c8f0fc045505 137 counts1 = Encoder1.getPulses();
ThomBMT 3:c8f0fc045505 138 counts2 = Encoder2.getPulses();
ThomBMT 4:8f67b8327300 139
ThomBMT 7:439940ae1197 140 rad_m1 = rad_count * (float)counts1;
ThomBMT 7:439940ae1197 141 rad_m2 = rad_count * (float)counts2;
ThomBMT 3:c8f0fc045505 142 }
ThomBMT 2:dc9766657afb 143
ThomBMT 10:05ad15c48388 144 void Filter()
ThomBMT 10:05ad15c48388 145 {
ThomBMT 10:05ad15c48388 146 FilterHP_Bi_R = bqc1.step( emg0.read() );
ThomBMT 10:05ad15c48388 147 Filterabs_Bi_R = fabs(FilterHP_Bi_R);
ThomBMT 10:05ad15c48388 148 Filtered_Bi_R = bqc2.step( Filterabs_Bi_R );
ThomBMT 10:05ad15c48388 149
ThomBMT 10:05ad15c48388 150 FilterHP_Bi_L = bqc3.step( emg1.read() );
ThomBMT 10:05ad15c48388 151 Filterabs_Bi_L = fabs(FilterHP_Bi_L);
ThomBMT 10:05ad15c48388 152 Filtered_Bi_L = bqc4.step( Filterabs_Bi_L );
ThomBMT 10:05ad15c48388 153
ThomBMT 10:05ad15c48388 154 FilterHP_Tri_R = bqc5.step( emg2.read() );
ThomBMT 10:05ad15c48388 155 Filterabs_Tri_R = fabs(FilterHP_Tri_R);
ThomBMT 10:05ad15c48388 156 Filtered_Tri_R = bqc6.step( Filterabs_Tri_R );
ThomBMT 10:05ad15c48388 157
ThomBMT 10:05ad15c48388 158 FilterHP_Tri_L = bqc7.step( emg3.read() );
ThomBMT 10:05ad15c48388 159 Filterabs_Tri_L = fabs(FilterHP_Tri_L);
ThomBMT 10:05ad15c48388 160 Filtered_Tri_L = bqc8.step( Filterabs_Tri_L );
ThomBMT 10:05ad15c48388 161 }
ThomBMT 10:05ad15c48388 162
ThomBMT 9:355dd95199c3 163 void BlinkLed()
ThomBMT 9:355dd95199c3 164 {
ThomBMT 9:355dd95199c3 165 if(i==1)
ThomBMT 9:355dd95199c3 166 {
ThomBMT 10:05ad15c48388 167 led_G=led_B=1;
ThomBMT 9:355dd95199c3 168 static int rr = 0;
ThomBMT 9:355dd95199c3 169 rr++;
ThomBMT 9:355dd95199c3 170 if (rr == 1)
ThomBMT 9:355dd95199c3 171 {
ThomBMT 9:355dd95199c3 172 led_R = !led_R;
ThomBMT 9:355dd95199c3 173 }
ThomBMT 10:05ad15c48388 174 else if (rr == 50)
ThomBMT 9:355dd95199c3 175 {
ThomBMT 9:355dd95199c3 176 rr = 0;
ThomBMT 9:355dd95199c3 177 }
ThomBMT 9:355dd95199c3 178 }
ThomBMT 9:355dd95199c3 179 else if(i==2)
ThomBMT 9:355dd95199c3 180 {
ThomBMT 10:05ad15c48388 181 led_R=led_B=1;
ThomBMT 10:05ad15c48388 182
ThomBMT 9:355dd95199c3 183 static int gg = 0;
ThomBMT 9:355dd95199c3 184 gg++;
ThomBMT 9:355dd95199c3 185 if (gg == 1)
ThomBMT 9:355dd95199c3 186 {
ThomBMT 9:355dd95199c3 187 led_G = !led_G;
ThomBMT 9:355dd95199c3 188 }
ThomBMT 10:05ad15c48388 189 else if (gg == 250)
ThomBMT 9:355dd95199c3 190 {
ThomBMT 9:355dd95199c3 191 gg = 0;
ThomBMT 9:355dd95199c3 192 }
ThomBMT 9:355dd95199c3 193 }
ThomBMT 9:355dd95199c3 194 else if (i==3)
ThomBMT 9:355dd95199c3 195 {
ThomBMT 10:05ad15c48388 196 led_R=1;
ThomBMT 9:355dd95199c3 197 static int bb = 0;
ThomBMT 9:355dd95199c3 198 bb++;
ThomBMT 9:355dd95199c3 199 if (bb == 1)
ThomBMT 9:355dd95199c3 200 {
ThomBMT 9:355dd95199c3 201 led_B = !led_B;
ThomBMT 9:355dd95199c3 202 }
ThomBMT 10:05ad15c48388 203 else if (bb == 500)
ThomBMT 9:355dd95199c3 204 {
ThomBMT 9:355dd95199c3 205 bb = 0;
ThomBMT 9:355dd95199c3 206 }
ThomBMT 9:355dd95199c3 207 }
ThomBMT 9:355dd95199c3 208 else
ThomBMT 9:355dd95199c3 209 {
ThomBMT 9:355dd95199c3 210 led_R=led_G=led_B=1;
ThomBMT 9:355dd95199c3 211 }
ThomBMT 9:355dd95199c3 212 }
ThomBMT 9:355dd95199c3 213
ThomBMT 2:dc9766657afb 214 void EMG_Read()
ThomBMT 2:dc9766657afb 215 {
ThomBMT 3:c8f0fc045505 216 Bicep_Right = emg0.read();
ThomBMT 3:c8f0fc045505 217 Bicep_Left = emg1.read();
ThomBMT 3:c8f0fc045505 218 Tricep_Right = emg2.read();
ThomBMT 3:c8f0fc045505 219 Tricep_Left = emg3.read();
ThomBMT 2:dc9766657afb 220 }
ThomBMT 2:dc9766657afb 221
ThomBMT 2:dc9766657afb 222 void sample()
ThomBMT 2:dc9766657afb 223 {
ThomBMT 2:dc9766657afb 224
ThomBMT 10:05ad15c48388 225 scope.set(0, Filtered_Bi_R*10.0f );
ThomBMT 10:05ad15c48388 226 scope.set(1, Filtered_Bi_L*10.0f );
ThomBMT 10:05ad15c48388 227 scope.set(2, Filtered_Tri_R*10.0f );
ThomBMT 10:05ad15c48388 228 scope.set(3, Filtered_Tri_L*10.0f );
ThomBMT 2:dc9766657afb 229
ThomBMT 2:dc9766657afb 230 scope.send();
ThomBMT 2:dc9766657afb 231 }
ThomBMT 2:dc9766657afb 232
ThomBMT 8:e8734a254818 233 void Inverse()
ThomBMT 8:e8734a254818 234 {
ThomBMT 10:05ad15c48388 235 q_1= rad_m1+(pi/6.0f); // uit Encoder
ThomBMT 10:05ad15c48388 236 q_2= rad_m2+(pi/6.0f); // uit Encoder
ThomBMT 8:e8734a254818 237 r_1= -0.2f;
ThomBMT 8:e8734a254818 238 r_2= -0.2f;
ThomBMT 8:e8734a254818 239
ThomBMT 8:e8734a254818 240 float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
ThomBMT 8:e8734a254818 241 float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3;
ThomBMT 8:e8734a254818 242 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 243 float x = (-2.0f)*r_2*sin(q_1)*cos(q_2);
ThomBMT 9:355dd95199c3 244 float D = 1.0f/(u*z-x*y); // Determinant
ThomBMT 10:05ad15c48388 245 //printf("Determinant is %f\r\n", D);
ThomBMT 8:e8734a254818 246
ThomBMT 9:355dd95199c3 247 float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
ThomBMT 9:355dd95199c3 248 float b = -D*x; // Inverse jacobian
ThomBMT 9:355dd95199c3 249 float c = -D*y; // Inverse jacobian
ThomBMT 9:355dd95199c3 250 float d = D*u; // Inverse jacobian
ThomBMT 8:e8734a254818 251
ThomBMT 10:05ad15c48388 252 vx = pot1.read()/5.0f; // uit emg data
ThomBMT 10:05ad15c48388 253 vy = pot2.read()/5.0f; // uit emg data
ThomBMT 8:e8734a254818 254
ThomBMT 8:e8734a254818 255 w_1 = vx*a+vy*b;
ThomBMT 8:e8734a254818 256 w_2 = vx*c+vy*d;
ThomBMT 8:e8734a254818 257
ThomBMT 8:e8734a254818 258 /*
ThomBMT 10:05ad15c48388 259 printf("%f\r\n", vx);
ThomBMT 10:05ad15c48388 260 printf("%f\r\n", vy);
ThomBMT 10:05ad15c48388 261 */
ThomBMT 8:e8734a254818 262 }
ThomBMT 10:05ad15c48388 263
ThomBMT 10:05ad15c48388 264 void PID_controller()
ThomBMT 10:05ad15c48388 265 {
ThomBMT 10:05ad15c48388 266 error_1 = (w_1*0.002f) - rad_m1;
ThomBMT 10:05ad15c48388 267 error_2 = (w_2*0.002f) - rad_m2;
ThomBMT 10:05ad15c48388 268
ThomBMT 10:05ad15c48388 269 error_1_prev = error_1;
ThomBMT 10:05ad15c48388 270 error_2_prev = error_2;
ThomBMT 10:05ad15c48388 271
ThomBMT 10:05ad15c48388 272 // Proportional part:
ThomBMT 10:05ad15c48388 273 float u_k_1 = Kp * error_1;
ThomBMT 10:05ad15c48388 274 float u_k_2 = Kp * error_2;
ThomBMT 10:05ad15c48388 275
ThomBMT 10:05ad15c48388 276 // Integral part
ThomBMT 10:05ad15c48388 277 error_1_integral = error_1_integral + error_1 * Ts;
ThomBMT 10:05ad15c48388 278 error_2_integral = error_2_integral + error_2 * Ts;
ThomBMT 10:05ad15c48388 279 float u_i_1 = Ki * error_1_integral;
ThomBMT 10:05ad15c48388 280 float u_i_2 = Ki * error_2_integral;
ThomBMT 10:05ad15c48388 281
ThomBMT 10:05ad15c48388 282 // Derivative part
ThomBMT 10:05ad15c48388 283 float error_1_derivative = (error_1 - error_1_prev)/Ts;
ThomBMT 10:05ad15c48388 284 float error_2_derivative = (error_2 - error_2_prev)/Ts;
ThomBMT 10:05ad15c48388 285 float filtered_error_1_derivative = LowPassFilter.step(error_1_derivative);
ThomBMT 10:05ad15c48388 286 float filtered_error_2_derivative = LowPassFilter.step(error_2_derivative);
ThomBMT 10:05ad15c48388 287 float u_d_1 = Kd * filtered_error_1_derivative;
ThomBMT 10:05ad15c48388 288 float u_d_2 = Kd * filtered_error_2_derivative;
ThomBMT 10:05ad15c48388 289 error_1_prev = error_1;
ThomBMT 10:05ad15c48388 290 error_2_prev = error_2;
ThomBMT 10:05ad15c48388 291
ThomBMT 10:05ad15c48388 292 // Sum all parts and return it
ThomBMT 10:05ad15c48388 293 U_1 = u_k_1 + u_i_1 + u_d_1;
ThomBMT 10:05ad15c48388 294 U_2 = u_k_2 + u_i_2 + u_d_2;
ThomBMT 10:05ad15c48388 295 }
ThomBMT 10:05ad15c48388 296
JurrienBos 1:4bf64d003f3a 297 void velocity1()
ThomBMT 10:05ad15c48388 298 {
ThomBMT 3:c8f0fc045505 299 if (pot1.read()>0.5f)
JurrienBos 0:4591ba678a39 300 {
JurrienBos 0:4591ba678a39 301 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 302 referenceVelocity1 = (pot1.read()-0.5f) * 2.0f;
JurrienBos 0:4591ba678a39 303 }
JurrienBos 0:4591ba678a39 304
ThomBMT 5:312186a0604d 305 else if (pot1.read() == 0.5f)
JurrienBos 0:4591ba678a39 306 {
JurrienBos 1:4bf64d003f3a 307 referenceVelocity1 = pot1.read() * 0.0f;
JurrienBos 0:4591ba678a39 308 }
JurrienBos 0:4591ba678a39 309
JurrienBos 0:4591ba678a39 310 else if (pot1.read() < 0.5f)
JurrienBos 0:4591ba678a39 311 {
JurrienBos 0:4591ba678a39 312 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 313 referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
JurrienBos 0:4591ba678a39 314 }
ThomBMT 10:05ad15c48388 315 }
JurrienBos 0:4591ba678a39 316
JurrienBos 1:4bf64d003f3a 317 void velocity2()
ThomBMT 10:05ad15c48388 318 {
JurrienBos 1:4bf64d003f3a 319 if (pot2.read()>0.5f)
JurrienBos 1:4bf64d003f3a 320 {
JurrienBos 1:4bf64d003f3a 321 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 322 referenceVelocity2 = (pot2.read()-0.5f) * 2.0f;
JurrienBos 1:4bf64d003f3a 323 }
JurrienBos 1:4bf64d003f3a 324
JurrienBos 1:4bf64d003f3a 325 else if (pot2.read() == 0.5f)
JurrienBos 1:4bf64d003f3a 326 {
JurrienBos 1:4bf64d003f3a 327 referenceVelocity2 = pot2.read() * 0.0f;
JurrienBos 1:4bf64d003f3a 328 }
JurrienBos 1:4bf64d003f3a 329
JurrienBos 1:4bf64d003f3a 330 else if (pot2.read() < 0.5f)
JurrienBos 1:4bf64d003f3a 331 {
JurrienBos 1:4bf64d003f3a 332 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 333 referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
JurrienBos 1:4bf64d003f3a 334 }
ThomBMT 10:05ad15c48388 335 }
ThomBMT 10:05ad15c48388 336
JurrienBos 1:4bf64d003f3a 337 void motor1()
ThomBMT 10:05ad15c48388 338 {
ThomBMT 10:05ad15c48388 339 float u_v1 = referenceVelocity1 ; //w_1
ThomBMT 10:05ad15c48388 340 float u = u_v1;// (2.0f * pi);
JurrienBos 0:4591ba678a39 341 DirectionPin1 = u < 0.0f;
JurrienBos 0:4591ba678a39 342 PwmPin1 = fabs(u);
ThomBMT 10:05ad15c48388 343 }
JurrienBos 0:4591ba678a39 344
JurrienBos 1:4bf64d003f3a 345 void motor2()
ThomBMT 10:05ad15c48388 346 {
ThomBMT 10:05ad15c48388 347 float u_v2 = referenceVelocity2 ; //w_2
ThomBMT 10:05ad15c48388 348 float u = u_v2;// (2.0f * pi);
JurrienBos 1:4bf64d003f3a 349 DirectionPin2 = u > 0.0f;
JurrienBos 1:4bf64d003f3a 350 PwmPin2 = fabs(u);
ThomBMT 10:05ad15c48388 351 }
ThomBMT 9:355dd95199c3 352
ThomBMT 8:e8734a254818 353 void Calibrating()
ThomBMT 8:e8734a254818 354 {
ThomBMT 8:e8734a254818 355 static float n = 0.0;
ThomBMT 8:e8734a254818 356 static float m = 0.0;
ThomBMT 8:e8734a254818 357 static float l = 0.0;
ThomBMT 8:e8734a254818 358 static float k = 0.0;
ThomBMT 9:355dd95199c3 359
ThomBMT 9:355dd95199c3 360 //static int ii;
ThomBMT 9:355dd95199c3 361 ii++;
ThomBMT 9:355dd95199c3 362
ThomBMT 9:355dd95199c3 363 if (ii<=2500)
ThomBMT 8:e8734a254818 364 {
ThomBMT 11:d525527c0b7d 365 if (ii == 1)
ThomBMT 9:355dd95199c3 366 {
ThomBMT 9:355dd95199c3 367 pc.printf("Relax your muscles please. \r\n");
ThomBMT 10:05ad15c48388 368 i = 2;
ThomBMT 9:355dd95199c3 369 }
ThomBMT 9:355dd95199c3 370 else if (ii == 2250)
ThomBMT 9:355dd95199c3 371 {
ThomBMT 9:355dd95199c3 372 pc.printf("Flex your right bicep now please.\r\n");
ThomBMT 10:05ad15c48388 373 i = 3;
ThomBMT 9:355dd95199c3 374 }
ThomBMT 10:05ad15c48388 375 //chillen
ThomBMT 8:e8734a254818 376 }
ThomBMT 9:355dd95199c3 377 else if (ii>2500 && ii<5000) //
ThomBMT 9:355dd95199c3 378 {
ThomBMT 10:05ad15c48388 379 n = n + Filtered_Bi_R; // dit wordt de variable naam na het filter
ThomBMT 10:05ad15c48388 380 i = 1;
ThomBMT 9:355dd95199c3 381 }
ThomBMT 9:355dd95199c3 382 else if (ii == 5000)
ThomBMT 8:e8734a254818 383 {
ThomBMT 8:e8734a254818 384 Flex_Bi_R = n / 2500.0f;
ThomBMT 9:355dd95199c3 385 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 10:05ad15c48388 386 i = 2;
ThomBMT 8:e8734a254818 387 }
ThomBMT 9:355dd95199c3 388 else if (ii>5000 && ii<=6000)
ThomBMT 8:e8734a254818 389 {
ThomBMT 9:355dd95199c3 390 if (ii == 5750)
ThomBMT 9:355dd95199c3 391 {
ThomBMT 9:355dd95199c3 392 pc.printf("Flex your left bicep now please. \r\n");
ThomBMT 10:05ad15c48388 393 i = 3;
ThomBMT 9:355dd95199c3 394 }
ThomBMT 8:e8734a254818 395 //chillen
ThomBMT 8:e8734a254818 396 }
ThomBMT 9:355dd95199c3 397 else if(ii>6000 && ii<8500)
ThomBMT 8:e8734a254818 398 {
ThomBMT 10:05ad15c48388 399 m = m + Filtered_Bi_L;
ThomBMT 10:05ad15c48388 400 i = 1;
ThomBMT 8:e8734a254818 401 }
ThomBMT 9:355dd95199c3 402 else if (ii == 8500)
ThomBMT 8:e8734a254818 403 {
ThomBMT 8:e8734a254818 404 Flex_Bi_L = m / 2500.0f;
ThomBMT 9:355dd95199c3 405 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 10:05ad15c48388 406 i = 2;
ThomBMT 8:e8734a254818 407 }
ThomBMT 9:355dd95199c3 408 else if (ii>8500 && ii<=9500)
ThomBMT 8:e8734a254818 409 {
ThomBMT 9:355dd95199c3 410 if (ii == 9250)
ThomBMT 9:355dd95199c3 411 {
ThomBMT 9:355dd95199c3 412 pc.printf("Flex your right tricep now please. \r\n");
ThomBMT 10:05ad15c48388 413 i = 3;
ThomBMT 9:355dd95199c3 414 }
ThomBMT 8:e8734a254818 415 //chillen
ThomBMT 8:e8734a254818 416 }
ThomBMT 9:355dd95199c3 417 else if (ii>9500 && ii<12000)
ThomBMT 8:e8734a254818 418 {
ThomBMT 10:05ad15c48388 419 l = l + Filtered_Tri_R;
ThomBMT 10:05ad15c48388 420 i = 1;
ThomBMT 8:e8734a254818 421 }
ThomBMT 9:355dd95199c3 422 else if (ii == 12000)
ThomBMT 8:e8734a254818 423 {
ThomBMT 8:e8734a254818 424 Flex_Tri_R = l / 2500.0f;
ThomBMT 9:355dd95199c3 425 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 10:05ad15c48388 426 i = 2;
ThomBMT 8:e8734a254818 427 }
ThomBMT 9:355dd95199c3 428 else if (ii>12000 && ii <=13000)
ThomBMT 8:e8734a254818 429 {
ThomBMT 9:355dd95199c3 430 if (ii == 12750)
ThomBMT 9:355dd95199c3 431 {
ThomBMT 9:355dd95199c3 432 pc.printf("Flex your left tricep now please. \r\n");
ThomBMT 10:05ad15c48388 433 i = 3;
ThomBMT 9:355dd95199c3 434 }
ThomBMT 8:e8734a254818 435 //chillen
ThomBMT 8:e8734a254818 436 }
ThomBMT 9:355dd95199c3 437 else if (ii>13000 && ii<15500)
ThomBMT 8:e8734a254818 438 {
ThomBMT 10:05ad15c48388 439 k = k + Filtered_Tri_L;
ThomBMT 10:05ad15c48388 440 i = 1;
ThomBMT 8:e8734a254818 441 }
ThomBMT 9:355dd95199c3 442 else if (ii == 15500)
ThomBMT 8:e8734a254818 443 {
ThomBMT 8:e8734a254818 444 Flex_Tri_L = k / 2500.0f;
ThomBMT 9:355dd95199c3 445 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 10:05ad15c48388 446 i = 2;
ThomBMT 9:355dd95199c3 447 }
ThomBMT 10:05ad15c48388 448
ThomBMT 10:05ad15c48388 449 Threshold_Value = 0.8f;
ThomBMT 9:355dd95199c3 450
ThomBMT 10:05ad15c48388 451 Threshold_Bi_R = Threshold_Value * Flex_Bi_R;
ThomBMT 10:05ad15c48388 452 Threshold_Bi_L = Threshold_Value * Flex_Bi_L;
ThomBMT 10:05ad15c48388 453 Threshold_Tri_R = Threshold_Value * Flex_Tri_R;
ThomBMT 10:05ad15c48388 454 Threshold_Tri_L = Threshold_Value * Flex_Tri_L;
ThomBMT 9:355dd95199c3 455
ThomBMT 9:355dd95199c3 456 if (ii == 16500)
ThomBMT 9:355dd95199c3 457 {
ThomBMT 9:355dd95199c3 458 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 459 }
ThomBMT 9:355dd95199c3 460 else if (ii == 20000)
ThomBMT 9:355dd95199c3 461 {
ThomBMT 9:355dd95199c3 462 pc.printf("\r\nAutomatic switch to Homing State\r\n");
ThomBMT 10:05ad15c48388 463 Active_State = Homing_M1;
ThomBMT 10:05ad15c48388 464 i = 0;
ThomBMT 9:355dd95199c3 465 }
ThomBMT 9:355dd95199c3 466 }
ThomBMT 9:355dd95199c3 467
ThomBMT 10:05ad15c48388 468
ThomBMT 10:05ad15c48388 469
ThomBMT 9:355dd95199c3 470 void Start_Up()
ThomBMT 9:355dd95199c3 471 {
ThomBMT 9:355dd95199c3 472 i++;
ThomBMT 9:355dd95199c3 473 iii++;
ThomBMT 9:355dd95199c3 474 if (iii == 1)
ThomBMT 9:355dd95199c3 475 {
ThomBMT 10:05ad15c48388 476 pc.printf("\r\n\r\nSystem is starting...\r\nWaiting for further input...\r\n");
ThomBMT 9:355dd95199c3 477 }
ThomBMT 9:355dd95199c3 478
ThomBMT 9:355dd95199c3 479 else if (iii == 30000)
ThomBMT 9:355dd95199c3 480 {
ThomBMT 9:355dd95199c3 481 pc.printf("1 minute without input..\r\nReseting start-up...\r\n");
ThomBMT 9:355dd95199c3 482 iii = 0;
ThomBMT 9:355dd95199c3 483 }
ThomBMT 9:355dd95199c3 484 else if (iii == 40001) // sleeping state is only added for designing purposes and will most likely never be used
ThomBMT 9:355dd95199c3 485 { // when working with patients. Furthermore it cannot be reached automaticly
ThomBMT 9:355dd95199c3 486 pc.printf("Sleeping... Press button 4 to wake me up!\r\n\r\n");
ThomBMT 9:355dd95199c3 487 iii++;
ThomBMT 9:355dd95199c3 488 }
ThomBMT 9:355dd95199c3 489 else if (iii == 45000)
ThomBMT 9:355dd95199c3 490 {
ThomBMT 9:355dd95199c3 491 iii = 40000;
ThomBMT 8:e8734a254818 492 }
ThomBMT 8:e8734a254818 493 }
ThomBMT 8:e8734a254818 494
ThomBMT 10:05ad15c48388 495 void OFF_m1()
ThomBMT 8:e8734a254818 496 {
ThomBMT 10:05ad15c48388 497 PwmPin1 = 0;
ThomBMT 10:05ad15c48388 498 }
ThomBMT 10:05ad15c48388 499 void OFF_m2()
ThomBMT 10:05ad15c48388 500 {
ThomBMT 10:05ad15c48388 501 PwmPin2 = 0;
ThomBMT 8:e8734a254818 502 }
JurrienBos 0:4591ba678a39 503
ThomBMT 10:05ad15c48388 504 void Going_Home_Motor1()
ThomBMT 9:355dd95199c3 505 {
ThomBMT 10:05ad15c48388 506 if (counts1 == 0)
ThomBMT 10:05ad15c48388 507 {
ThomBMT 10:05ad15c48388 508 Active_State = Homing_M2;
ThomBMT 9:355dd95199c3 509 }
ThomBMT 9:355dd95199c3 510 else if (counts1 > 0)
ThomBMT 10:05ad15c48388 511 {
ThomBMT 10:05ad15c48388 512 PwmPin1 = 0.8f;
ThomBMT 9:355dd95199c3 513 DirectionPin1 = false;
ThomBMT 9:355dd95199c3 514 }
ThomBMT 10:05ad15c48388 515 else
ThomBMT 9:355dd95199c3 516 {
ThomBMT 10:05ad15c48388 517 PwmPin1 = 0.8f;
ThomBMT 10:05ad15c48388 518 DirectionPin1 = true;
ThomBMT 10:05ad15c48388 519 }
ThomBMT 10:05ad15c48388 520 }
ThomBMT 10:05ad15c48388 521
ThomBMT 10:05ad15c48388 522 void Going_Home_Motor2()
ThomBMT 10:05ad15c48388 523 {
ThomBMT 10:05ad15c48388 524 if (counts2 == 0)
ThomBMT 10:05ad15c48388 525 {
ThomBMT 10:05ad15c48388 526 Active_State = Post_Homing;
ThomBMT 9:355dd95199c3 527 }
ThomBMT 9:355dd95199c3 528 else if (counts2 > 0)
ThomBMT 9:355dd95199c3 529 {
ThomBMT 10:05ad15c48388 530 PwmPin2 = 0.8f;
ThomBMT 10:05ad15c48388 531 DirectionPin2 = true;
ThomBMT 9:355dd95199c3 532 }
ThomBMT 10:05ad15c48388 533 else
ThomBMT 9:355dd95199c3 534 {
ThomBMT 10:05ad15c48388 535 PwmPin2 = 0.8f;
ThomBMT 9:355dd95199c3 536 DirectionPin2 = false;
ThomBMT 9:355dd95199c3 537 }
ThomBMT 9:355dd95199c3 538 }
ThomBMT 9:355dd95199c3 539
ThomBMT 2:dc9766657afb 540 void Printing()
ThomBMT 2:dc9766657afb 541 {
ThomBMT 8:e8734a254818 542 float v1 = PwmPin1 * maxVelocity;
ThomBMT 8:e8734a254818 543 float v2 = PwmPin2 * maxVelocity;
ThomBMT 9:355dd95199c3 544
ThomBMT 10:05ad15c48388 545 if (Active_State == Function || Active_State == Homing_M1)
ThomBMT 9:355dd95199c3 546 {
ThomBMT 10:05ad15c48388 547 pc.printf("q1 = %f [rad] \r\nq2 = %f [rad] \r\ncount1= %i\r\ncount2= %i\r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n\r\n\r\n\r\n", rad_m1, rad_m2,counts1, counts2, v1, v2);
ThomBMT 9:355dd95199c3 548 }
ThomBMT 10:05ad15c48388 549 }
ThomBMT 10:05ad15c48388 550
ThomBMT 10:05ad15c48388 551 void Test()
ThomBMT 10:05ad15c48388 552 {/*
ThomBMT 10:05ad15c48388 553 if (fabs(rad_m1) == 0.01f)
ThomBMT 10:05ad15c48388 554 {
ThomBMT 10:05ad15c48388 555 PwmPin1 = 0.0f;
ThomBMT 10:05ad15c48388 556 }
ThomBMT 10:05ad15c48388 557 else if (rad_m1 < 0.01f)
ThomBMT 10:05ad15c48388 558 {
ThomBMT 10:05ad15c48388 559 DirectionPin1 = true;
ThomBMT 10:05ad15c48388 560 PwmPin1=0.5f;
ThomBMT 10:05ad15c48388 561 }
ThomBMT 10:05ad15c48388 562 else if (rad_m1 > -0.01f)
ThomBMT 10:05ad15c48388 563 {
ThomBMT 10:05ad15c48388 564 DirectionPin1 = false;
ThomBMT 10:05ad15c48388 565 PwmPin1=0.5f;
ThomBMT 10:05ad15c48388 566 }
ThomBMT 10:05ad15c48388 567 if (fabs(rad_m2) == 0.01f)
ThomBMT 10:05ad15c48388 568 {
ThomBMT 10:05ad15c48388 569 PwmPin2 = 0.0f;
ThomBMT 10:05ad15c48388 570 }
ThomBMT 10:05ad15c48388 571 else if (rad_m2 < 0.01f)
ThomBMT 10:05ad15c48388 572 {
ThomBMT 10:05ad15c48388 573 DirectionPin2 = false;
ThomBMT 10:05ad15c48388 574 }*/
ThomBMT 10:05ad15c48388 575
ThomBMT 10:05ad15c48388 576
ThomBMT 10:05ad15c48388 577
ThomBMT 10:05ad15c48388 578 if (counts1 == 0)
ThomBMT 10:05ad15c48388 579 {
ThomBMT 10:05ad15c48388 580 PwmPin1 = 0.0f;
ThomBMT 10:05ad15c48388 581 }
ThomBMT 10:05ad15c48388 582 else if (counts1 > 0)
ThomBMT 10:05ad15c48388 583 {
ThomBMT 10:05ad15c48388 584 DirectionPin1 = true;
ThomBMT 10:05ad15c48388 585 PwmPin1 = 0.4f * ((float)counts1/1000.0f);
ThomBMT 10:05ad15c48388 586 }
ThomBMT 10:05ad15c48388 587 else if (counts1 < 0)
ThomBMT 10:05ad15c48388 588 {
ThomBMT 10:05ad15c48388 589 DirectionPin1 = false;
ThomBMT 10:05ad15c48388 590 PwmPin1 = 0.4f * ((float)counts1/1000.0f);
ThomBMT 10:05ad15c48388 591 }
ThomBMT 10:05ad15c48388 592
ThomBMT 10:05ad15c48388 593 if (counts2 == 0)
ThomBMT 10:05ad15c48388 594 {
ThomBMT 10:05ad15c48388 595 PwmPin2 = 0.0f;
ThomBMT 10:05ad15c48388 596 }
ThomBMT 10:05ad15c48388 597 if (counts2 < 0)
ThomBMT 10:05ad15c48388 598 {
ThomBMT 10:05ad15c48388 599 DirectionPin2 = false;
ThomBMT 10:05ad15c48388 600 PwmPin2 = 0.4f * ((float)counts2/1000.0f);
ThomBMT 10:05ad15c48388 601 }
ThomBMT 10:05ad15c48388 602 else if (counts2 > 0)
ThomBMT 10:05ad15c48388 603 {
ThomBMT 10:05ad15c48388 604 DirectionPin2 = true;
ThomBMT 10:05ad15c48388 605 PwmPin2 = 0.4f * ((float)counts2/1000.0f);
ThomBMT 10:05ad15c48388 606 }
ThomBMT 10:05ad15c48388 607
ThomBMT 10:05ad15c48388 608 }
ThomBMT 10:05ad15c48388 609
ThomBMT 10:05ad15c48388 610 void EMG_test()
ThomBMT 10:05ad15c48388 611 {
ThomBMT 10:05ad15c48388 612 led_G=led_R=led_B=1;
ThomBMT 10:05ad15c48388 613 /*
ThomBMT 10:05ad15c48388 614 Threshold_Bi_R = 0.75f * Flex_Bi_R;
ThomBMT 10:05ad15c48388 615 Threshold_Bi_L = 0.75f * Flex_Bi_L;
ThomBMT 10:05ad15c48388 616 Threshold_Tri_R = 0.75f * Flex_Tri_R;
ThomBMT 10:05ad15c48388 617 Threshold_Tri_L = 0.75f * Flex_Tri_L;
ThomBMT 10:05ad15c48388 618 */
ThomBMT 10:05ad15c48388 619 if (Filtered_Bi_R >= Threshold_Bi_R)
ThomBMT 10:05ad15c48388 620 {
ThomBMT 10:05ad15c48388 621 led_R = 0;
ThomBMT 10:05ad15c48388 622 }
ThomBMT 10:05ad15c48388 623 if (Filtered_Bi_L >= Threshold_Bi_L)
ThomBMT 10:05ad15c48388 624 {
ThomBMT 10:05ad15c48388 625 led_B = 0;
ThomBMT 10:05ad15c48388 626 }
ThomBMT 10:05ad15c48388 627 if (Filtered_Tri_R >= Threshold_Tri_R)
ThomBMT 10:05ad15c48388 628 {
ThomBMT 10:05ad15c48388 629 led_G = 0;
ThomBMT 10:05ad15c48388 630 }
ThomBMT 10:05ad15c48388 631 if (Filtered_Tri_L >= Threshold_Tri_L)
ThomBMT 10:05ad15c48388 632 {
ThomBMT 10:05ad15c48388 633 led_B = 0;
ThomBMT 10:05ad15c48388 634 led_R = 0;
ThomBMT 10:05ad15c48388 635 }
ThomBMT 10:05ad15c48388 636
ThomBMT 2:dc9766657afb 637 }
ThomBMT 2:dc9766657afb 638
ThomBMT 4:8f67b8327300 639 void StateMachine()
ThomBMT 4:8f67b8327300 640 {
ThomBMT 4:8f67b8327300 641 switch (Active_State)
ThomBMT 4:8f67b8327300 642 {
ThomBMT 8:e8734a254818 643 case Starting:
ThomBMT 9:355dd95199c3 644 Start_Up();
ThomBMT 9:355dd95199c3 645
ThomBMT 9:355dd95199c3 646 if (!Knop4 == true)
ThomBMT 9:355dd95199c3 647 {
ThomBMT 9:355dd95199c3 648 Active_State = Calibration;
ThomBMT 9:355dd95199c3 649 pc.printf("Entering Calibration State \r\n");
ThomBMT 9:355dd95199c3 650 }
ThomBMT 9:355dd95199c3 651 else if (!Knop3 == true)
ThomBMT 9:355dd95199c3 652 {
ThomBMT 10:05ad15c48388 653 Active_State = Homing_M1;
ThomBMT 9:355dd95199c3 654 pc.printf("Entering Homing State \r\n");
ThomBMT 9:355dd95199c3 655 }
ThomBMT 10:05ad15c48388 656 else if (fabs(rad_m1)>(3.0f *pi) || fabs(rad_m2)>(3.0f *pi)) // pi/4 is a safe value, can/will be editted
ThomBMT 10:05ad15c48388 657 {
ThomBMT 10:05ad15c48388 658 pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
ThomBMT 10:05ad15c48388 659 Active_State = Safe;
ThomBMT 10:05ad15c48388 660 }
ThomBMT 9:355dd95199c3 661
ThomBMT 8:e8734a254818 662 break;
ThomBMT 8:e8734a254818 663
ThomBMT 4:8f67b8327300 664 case Calibration:
ThomBMT 5:312186a0604d 665 //calibration actions
ThomBMT 5:312186a0604d 666 //pc.printf("Calibration State");
ThomBMT 8:e8734a254818 667
ThomBMT 10:05ad15c48388 668 Filter();
ThomBMT 8:e8734a254818 669 Calibrating();
ThomBMT 10:05ad15c48388 670 OFF_m1();
ThomBMT 10:05ad15c48388 671 OFF_m2();
ThomBMT 10:05ad15c48388 672 BlinkLed();
ThomBMT 9:355dd95199c3 673
ThomBMT 9:355dd95199c3 674 if (!Knop1 && !Knop2)
ThomBMT 5:312186a0604d 675 {
ThomBMT 9:355dd95199c3 676 pc.printf("Switched to Sleeping State\r\n");
ThomBMT 9:355dd95199c3 677 Active_State = Starting;
ThomBMT 9:355dd95199c3 678 iii = 40001;
ThomBMT 9:355dd95199c3 679 }
ThomBMT 9:355dd95199c3 680 else if (Knop1==false)
ThomBMT 9:355dd95199c3 681 {
ThomBMT 9:355dd95199c3 682 pc.printf("Manual switch to Homing state \r\n");
ThomBMT 10:05ad15c48388 683 Active_State = Homing_M1;
ThomBMT 5:312186a0604d 684 }
ThomBMT 9:355dd95199c3 685
ThomBMT 9:355dd95199c3 686
ThomBMT 10:05ad15c48388 687 Inverse();
ThomBMT 5:312186a0604d 688 sample();
ThomBMT 5:312186a0604d 689 EMG_Read();
ThomBMT 5:312186a0604d 690 Encoding();
ThomBMT 4:8f67b8327300 691 break;
ThomBMT 4:8f67b8327300 692
ThomBMT 10:05ad15c48388 693 case Homing_M1:
ThomBMT 5:312186a0604d 694 //Homing actions
ThomBMT 5:312186a0604d 695 //pc.printf("Homing State");
ThomBMT 10:05ad15c48388 696 Going_Home_Motor1();
ThomBMT 10:05ad15c48388 697 OFF_m2();
ThomBMT 9:355dd95199c3 698
ThomBMT 9:355dd95199c3 699 if (!Knop1 && !Knop2)
ThomBMT 5:312186a0604d 700 {
ThomBMT 9:355dd95199c3 701 pc.printf("Switched to Sleeping State\r\n");
ThomBMT 9:355dd95199c3 702 Active_State = Starting;
ThomBMT 9:355dd95199c3 703 iii = 40000;
ThomBMT 9:355dd95199c3 704 }
ThomBMT 9:355dd95199c3 705 else if (Knop2==false)
ThomBMT 9:355dd95199c3 706 {
ThomBMT 9:355dd95199c3 707 pc.printf("Manual switch to Funtioning State \r\n");
ThomBMT 5:312186a0604d 708 Active_State = Function;
ThomBMT 5:312186a0604d 709 }
ThomBMT 10:05ad15c48388 710 else if (Knop4==false)
ThomBMT 9:355dd95199c3 711 {
ThomBMT 9:355dd95199c3 712 Active_State = Calibration;
ThomBMT 9:355dd95199c3 713 pc.printf("Re-entering Calibration State \r\n");
ThomBMT 9:355dd95199c3 714 }
ThomBMT 10:05ad15c48388 715 else if (fabs(rad_m1)>(3.0f *pi) || fabs(rad_m2)>(3.0f *pi)) // pi/4 is a safe value, can/will be editted
ThomBMT 10:05ad15c48388 716 {
ThomBMT 10:05ad15c48388 717 pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
ThomBMT 10:05ad15c48388 718 Active_State = Safe;
ThomBMT 10:05ad15c48388 719 }
ThomBMT 10:05ad15c48388 720 else if (counts1 == 0)
ThomBMT 10:05ad15c48388 721 {
ThomBMT 10:05ad15c48388 722 Active_State = Homing_M2;
ThomBMT 10:05ad15c48388 723 }
ThomBMT 9:355dd95199c3 724
ThomBMT 10:05ad15c48388 725 Inverse();
ThomBMT 5:312186a0604d 726 sample();
ThomBMT 5:312186a0604d 727 EMG_Read();
ThomBMT 5:312186a0604d 728 Encoding();
ThomBMT 4:8f67b8327300 729 break;
ThomBMT 4:8f67b8327300 730
ThomBMT 10:05ad15c48388 731 case Homing_M2:
ThomBMT 10:05ad15c48388 732
ThomBMT 10:05ad15c48388 733 Going_Home_Motor2();
ThomBMT 10:05ad15c48388 734 OFF_m1();
ThomBMT 10:05ad15c48388 735
ThomBMT 10:05ad15c48388 736 if (counts2 == 0 && counts1 == 0)
ThomBMT 10:05ad15c48388 737 {
ThomBMT 10:05ad15c48388 738 Active_State = Post_Homing;
ThomBMT 10:05ad15c48388 739 }
ThomBMT 10:05ad15c48388 740 else if (counts2 == 0 && counts1 == !0);
ThomBMT 10:05ad15c48388 741 {
ThomBMT 10:05ad15c48388 742 // dit zou niet moeten kunnen maar er gebeuren wel meer rare dingen
ThomBMT 10:05ad15c48388 743 Active_State = Homing_M1;
ThomBMT 10:05ad15c48388 744 }
ThomBMT 10:05ad15c48388 745
ThomBMT 10:05ad15c48388 746 Inverse();
ThomBMT 10:05ad15c48388 747 sample();
ThomBMT 10:05ad15c48388 748 EMG_Read();
ThomBMT 10:05ad15c48388 749 Encoding();
ThomBMT 10:05ad15c48388 750
ThomBMT 10:05ad15c48388 751 break;
ThomBMT 10:05ad15c48388 752
ThomBMT 10:05ad15c48388 753 case Post_Homing:
ThomBMT 10:05ad15c48388 754
ThomBMT 10:05ad15c48388 755 static int mm = 0;
ThomBMT 10:05ad15c48388 756 mm++;
ThomBMT 10:05ad15c48388 757 if (mm == 1000);
ThomBMT 10:05ad15c48388 758 {
ThomBMT 10:05ad15c48388 759 Active_State = Function;
ThomBMT 10:05ad15c48388 760 pc.printf("Homing was succesfull\r\n\r\nAutomatic switch to Funtioning state\r\n\r\n");
ThomBMT 10:05ad15c48388 761 mm=0; // reseting the state
ThomBMT 10:05ad15c48388 762 }
ThomBMT 10:05ad15c48388 763 break;
ThomBMT 10:05ad15c48388 764
ThomBMT 4:8f67b8327300 765 case Function:
ThomBMT 5:312186a0604d 766 //pc.printf("Funtioning State");
ThomBMT 8:e8734a254818 767
ThomBMT 9:355dd95199c3 768 if (Knop4==false)
ThomBMT 8:e8734a254818 769 {
ThomBMT 9:355dd95199c3 770 pc.printf("Re-entering Calibration State \r\n");
ThomBMT 5:312186a0604d 771 Active_State = Calibration;
ThomBMT 9:355dd95199c3 772 ii=0;
ThomBMT 9:355dd95199c3 773 }
ThomBMT 9:355dd95199c3 774 else if (Knop3==false)
ThomBMT 9:355dd95199c3 775 {
ThomBMT 9:355dd95199c3 776 pc.printf("Re-entering Homing State \r\n");
ThomBMT 10:05ad15c48388 777 Active_State = Homing_M1;
ThomBMT 9:355dd95199c3 778 }
ThomBMT 9:355dd95199c3 779 else if (!Knop1 && !Knop2)
ThomBMT 9:355dd95199c3 780 {
ThomBMT 9:355dd95199c3 781 pc.printf("Switched to Sleeping State\r\n");
ThomBMT 9:355dd95199c3 782 Active_State = Starting;
ThomBMT 9:355dd95199c3 783 iii = 40000;
ThomBMT 5:312186a0604d 784 }
ThomBMT 10:05ad15c48388 785 else if (fabs(rad_m1)>(3.0f *pi) || fabs(rad_m2)>(3.0f *pi)) // pi/4 is a safe value, can/will be editted
ThomBMT 10:05ad15c48388 786 {
ThomBMT 10:05ad15c48388 787 pc.printf("SAFE MODUS ACTIVE!\r\n RESET MANDATORY!\r\n");
ThomBMT 10:05ad15c48388 788 Active_State = Safe;
ThomBMT 10:05ad15c48388 789 }
ThomBMT 10:05ad15c48388 790
ThomBMT 10:05ad15c48388 791 EMG_test();
ThomBMT 10:05ad15c48388 792 Filter();
ThomBMT 10:05ad15c48388 793 Inverse();
ThomBMT 5:312186a0604d 794 sample();
ThomBMT 5:312186a0604d 795 EMG_Read();
ThomBMT 5:312186a0604d 796 Encoding();
ThomBMT 10:05ad15c48388 797 velocity1();
ThomBMT 10:05ad15c48388 798 velocity2();
ThomBMT 5:312186a0604d 799 motor1();
ThomBMT 10:05ad15c48388 800 motor2();
ThomBMT 10:05ad15c48388 801 PID_controller();
ThomBMT 4:8f67b8327300 802 break;
ThomBMT 10:05ad15c48388 803
ThomBMT 10:05ad15c48388 804 case Safe:
ThomBMT 10:05ad15c48388 805 OFF_m1();
ThomBMT 10:05ad15c48388 806 OFF_m2();
ThomBMT 10:05ad15c48388 807 break;
ThomBMT 4:8f67b8327300 808
ThomBMT 4:8f67b8327300 809 default:
ThomBMT 4:8f67b8327300 810 pc.printf("UNKNOWN COMMAND");
ThomBMT 4:8f67b8327300 811 }
ThomBMT 4:8f67b8327300 812 }
ThomBMT 4:8f67b8327300 813
JurrienBos 0:4591ba678a39 814 int main()
JurrienBos 0:4591ba678a39 815 {
JurrienBos 0:4591ba678a39 816 pc.baud(115200);
ThomBMT 4:8f67b8327300 817 PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz
ThomBMT 2:dc9766657afb 818
ThomBMT 10:05ad15c48388 819 bqc1.add( &BqNotch1_1 ).add( &BqNotch2_1 ).add( &BqHP1 ); //Oh wat lelijk...
ThomBMT 10:05ad15c48388 820 bqc2.add(&BqLP1);
ThomBMT 10:05ad15c48388 821 bqc3.add( &BqNotch1_2 ).add( &BqNotch2_2 ).add( &BqHP2 );
ThomBMT 10:05ad15c48388 822 bqc4.add(&BqLP2);
ThomBMT 10:05ad15c48388 823 bqc5.add( &BqNotch1_3 ).add( &BqNotch2_3 ).add( &BqHP3 );
ThomBMT 10:05ad15c48388 824 bqc6.add(&BqLP3);
ThomBMT 10:05ad15c48388 825 bqc7.add( &BqNotch1_4 ).add( &BqNotch2_4 ).add( &BqHP4 );
ThomBMT 10:05ad15c48388 826 bqc8.add(&BqLP4);
ThomBMT 10:05ad15c48388 827
ThomBMT 8:e8734a254818 828 StateTicker.attach(&StateMachine, 0.002);
ThomBMT 4:8f67b8327300 829
ThomBMT 10:05ad15c48388 830 printTicker.attach(&Printing, 2);
ThomBMT 2:dc9766657afb 831
JurrienBos 0:4591ba678a39 832 while(true)
ThomBMT 5:312186a0604d 833 {
JurrienBos 0:4591ba678a39 834 }
JurrienBos 1:4bf64d003f3a 835 }