Thom Kuenen / Mbed 2 deprecated Biorobotics_V1

Biorobotics 2018: Project group 16

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@1:ba14d8f4d444, 2018-11-01 (annotated)

Committer:
ThomBMT
Date:
Thu Nov 01 10:08:23 2018 +0000
Revision:
1:ba14d8f4d444
Parent:
0:94cf6b327972
Child:
2:bc6043623fb7
Now everything is added but some functions are not needed and will now be taken out of the script; ; Can go back to this version in case of crashes

Who changed what in which revision?

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