Ralph Gerlings / Mbed 2 deprecated cpfromralph

new fork sure

Dependencies:   HIDScope MODSERIAL PID QEI biquadFilter mbed

Fork of cpfromralph by Remi van Veen

main.cpp@35:82b432f5aef7, 2017-11-03 (annotated)

Committer:
ralphg_92
Date:
Fri Nov 03 01:33:27 2017 +0000
Revision:
35:82b432f5aef7
Parent:
34:6e74f6629a0e
Child:
36:2fbc53a2747c
now with  a while loop instead of a useless if loop

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
ralphg_92 29:09c1567d6148 3 #include "MODSERIAL.h"
relvorelvo 21:136a1ab8163c 4 #include "BiQuad.h"
ralphg_92 33:97e69c32a768 5 #include "QEI.h"
vsluiter 0:32bb76391d89 6
ralphg_92 32:a779b1131977 7 //Define Objects
ralphg_92 32:a779b1131977 8 AnalogIn emg1_in( A0 ); // Read Out The Signal
relvorelvo 23:e5db011bd410 9 AnalogIn emg2_in( A1 );
relvorelvo 23:e5db011bd410 10 AnalogIn emg3_in( A2 );
ralphg_92 27:ca07f895f999 11 AnalogIn emg4_in( A3 );
ralphg_92 32:a779b1131977 12 DigitalIn max_reader12( SW2 ); // Define Button Press
ralphg_92 27:ca07f895f999 13 DigitalIn max_reader34( SW3 );
ralphg_92 31:d346f9244b4a 14
ralphg_92 31:d346f9244b4a 15 DigitalOut motor1direction( D4 );
ralphg_92 31:d346f9244b4a 16 PwmOut motor1pwm( D5);
ralphg_92 31:d346f9244b4a 17 PwmOut motor2pwm( D6 );
ralphg_92 31:d346f9244b4a 18 DigitalOut motor2direction( D7 );
ralphg_92 33:97e69c32a768 19 QEI Encoder1(D10, D11, NC, 64); // Encoder
ralphg_92 33:97e69c32a768 20 QEI Encoder2(D12, D13, NC, 64);
relvorelvo 23:e5db011bd410 21
relvorelvo 23:e5db011bd410 22 Ticker main_timer;
relvorelvo 23:e5db011bd410 23 Ticker max_read1;
relvorelvo 23:e5db011bd410 24 Ticker max_read3;
ralphg_92 31:d346f9244b4a 25 Ticker Motorcontrol;
ralphg_92 34:6e74f6629a0e 26 //Ticker PIDtimer;
ralphg_92 33:97e69c32a768 27 HIDScope scope( 4 );
relvorelvo 23:e5db011bd410 28 DigitalOut red(LED_RED);
relvorelvo 23:e5db011bd410 29 DigitalOut blue(LED_BLUE);
relvorelvo 23:e5db011bd410 30 DigitalOut green(LED_GREEN);
ralphg_92 33:97e69c32a768 31 Serial pc(USBTX, USBRX);
relvorelvo 21:136a1ab8163c 32
ralphg_92 33:97e69c32a768 33 // EMG Variables & Constants
relvorelvo 23:e5db011bd410 34 //Right Biceps
relvorelvo 23:e5db011bd410 35 double emg1;
relvorelvo 23:e5db011bd410 36 double emg1highfilter;
relvorelvo 23:e5db011bd410 37 double emg1notchfilter;
relvorelvo 23:e5db011bd410 38 double emg1abs;
relvorelvo 23:e5db011bd410 39 double emg1lowfilter;
ralphg_92 27:ca07f895f999 40 double max1;
relvorelvo 24:26659f1de039 41 double maxpart1;
relvorelvo 23:e5db011bd410 42 // Left Biceps
relvorelvo 23:e5db011bd410 43 double emg2;
relvorelvo 23:e5db011bd410 44 double emg2highfilter;
relvorelvo 23:e5db011bd410 45 double emg2notchfilter;
relvorelvo 23:e5db011bd410 46 double emg2abs;
relvorelvo 23:e5db011bd410 47 double emg2lowfilter;
ralphg_92 27:ca07f895f999 48 double max2;
relvorelvo 24:26659f1de039 49 double maxpart2;
ralphg_92 27:ca07f895f999 50 // Left Lower Arm
relvorelvo 23:e5db011bd410 51 double emg3;
relvorelvo 23:e5db011bd410 52 double emg3highfilter;
relvorelvo 23:e5db011bd410 53 double emg3notchfilter;
relvorelvo 23:e5db011bd410 54 double emg3abs;
relvorelvo 23:e5db011bd410 55 double emg3lowfilter;
relvorelvo 23:e5db011bd410 56 double max3;
relvorelvo 23:e5db011bd410 57 double maxpart3;
ralphg_92 27:ca07f895f999 58 // Right Lower Arm
ralphg_92 27:ca07f895f999 59 double emg4;
ralphg_92 27:ca07f895f999 60 double emg4highfilter;
ralphg_92 27:ca07f895f999 61 double emg4notchfilter;
ralphg_92 27:ca07f895f999 62 double emg4abs;
ralphg_92 27:ca07f895f999 63 double emg4lowfilter;
ralphg_92 27:ca07f895f999 64 double max4;
ralphg_92 27:ca07f895f999 65 double maxpart4;
ralphg_92 33:97e69c32a768 66 // Moving Average Floats
ralphg_92 32:a779b1131977 67 // Right Biceps Movavg
ralphg_92 32:a779b1131977 68 float RB0, RB1, RB2, RB3, RB4, RB5, RB6, RB7, RB8, RB9, RB10, RB11, RB12,
ralphg_92 32:a779b1131977 69 RB13, RB14, RB15, RB16, RB17, RB18, RB19, RB20, RB21, RB22, RB23, RB24,
ralphg_92 32:a779b1131977 70 RB25, RB26, RB27, RB28, RB29, RB30, RB31, RB32, RB33, RB34, RB35, RB36,
ralphg_92 32:a779b1131977 71 RB37, RB38, RB39, RB40, RB41, RB42, RB43, RB44, RB45, RB46, RB47, RB48,
ralphg_92 32:a779b1131977 72 RB49, RB50, RB51, RB52, RB53, RB54, RB55, RB56, RB57, RB58, RB59, RB60,
ralphg_92 32:a779b1131977 73 RB61, RB62, RB63, RB64, RB65, RB66, RB67, RB68, RB69, RB70, RB71, RB72,
ralphg_92 32:a779b1131977 74 RB73, RB74, RB75, RB76, RB77, RB78, RB79, RB80, RB81, RB82, RB83, RB84,
ralphg_92 32:a779b1131977 75 RB85, RB86, RB87, MOVAVG_RB;
ralphg_92 32:a779b1131977 76 // Left Biceps Movavg
ralphg_92 32:a779b1131977 77 float LB0, LB1, LB2, LB3, LB4, LB5, LB6, LB7, LB8, LB9, LB10, LB11, LB12,
ralphg_92 32:a779b1131977 78 LB13, LB14, LB15, LB16, LB17, LB18, LB19, LB20, LB21, LB22, LB23, LB24,
ralphg_92 32:a779b1131977 79 LB25, LB26, LB27, LB28, LB29, LB30, LB31, LB32, LB33, LB34, LB35, LB36,
ralphg_92 32:a779b1131977 80 LB37, LB38, LB39, LB40, LB41, LB42, LB43, LB44, LB45, LB46, LB47, LB48,
ralphg_92 32:a779b1131977 81 LB49, LB50, LB51, LB52, LB53, LB54, LB55, LB56, LB57, LB58, LB59, LB60,
ralphg_92 32:a779b1131977 82 LB61, LB62, LB63, LB64, LB65, LB66, LB67, LB68, LB69, LB70, LB71, LB72,
ralphg_92 32:a779b1131977 83 LB73, LB74, LB75, LB76, LB77, LB78, LB79, LB80, LB81, LB82, LB83, LB84,
ralphg_92 32:a779b1131977 84 LB85, LB86, LB87, MOVAVG_LB;
ralphg_92 32:a779b1131977 85 // Left Lower Arm Movavg
ralphg_92 32:a779b1131977 86 float LL0, LL1, LL2, LL3, LL4, LL5, LL6, LL7, LL8, LL9, LL10, LL11, LL12,
ralphg_92 32:a779b1131977 87 LL13, LL14, LL15, LL16, LL17, LL18, LL19, LL20, LL21, LL22, LL23, LL24,
ralphg_92 32:a779b1131977 88 LL25, LL26, LL27, LL28, LL29, LL30, LL31, LL32, LL33, LL34, LL35, LL36,
ralphg_92 32:a779b1131977 89 LL37, LL38, LL39, LL40, LL41, LL42, LL43, LL44, LL45, LL46, LL47, LL48,
ralphg_92 32:a779b1131977 90 LL49, LL50, LL51, LL52, LL53, LL54, LL55, LL56, LL57, LL58, LL59, LL60,
ralphg_92 32:a779b1131977 91 LL61, LL62, LL63, LL64, LL65, LL66, LL67, LL68, LL69, LL70, LL71, LL72,
ralphg_92 32:a779b1131977 92 LL73, LL74, LL75, LL76, LL77, LL78, LL79, LL80, LL81, LL82, LL83, LL84,
ralphg_92 32:a779b1131977 93 LL85, LL86, LL87, MOVAVG_LL;
ralphg_92 32:a779b1131977 94 // Right Lower Arm Movavg
ralphg_92 32:a779b1131977 95 float RL0, RL1, RL2, RL3, RL4, RL5, RL6, RL7, RL8, RL9, RL10, RL11, RL12,
ralphg_92 32:a779b1131977 96 RL13, RL14, RL15, RL16, RL17, RL18, RL19, RL20, RL21, RL22, RL23, RL24,
ralphg_92 32:a779b1131977 97 RL25, RL26, RL27, RL28, RL29, RL30, RL31, RL32, RL33, RL34, RL35, RL36,
ralphg_92 32:a779b1131977 98 RL37, RL38, RL39, RL40, RL41, RL42, RL43, RL44, RL45, RL46, RL47, RL48,
ralphg_92 32:a779b1131977 99 RL49, RL50, RL51, RL52, RL53, RL54, RL55, RL56, RL57, RL58, RL59, RL60,
ralphg_92 32:a779b1131977 100 RL61, RL62, RL63, RL64, RL65, RL66, RL67, RL68, RL69, RL70, RL71, RL72,
ralphg_92 32:a779b1131977 101 RL73, RL74, RL75, RL76, RL77, RL78, RL79, RL80, RL81, RL82, RL83, RL84,
ralphg_92 32:a779b1131977 102 RL85, RL86, RL87, MOVAVG_RL;
ralphg_92 32:a779b1131977 103 float AV = 0.02; //multiplication value for adding each movavg value
ralphg_92 32:a779b1131977 104 // This value also adds a very slight gain to every value
ralphg_92 33:97e69c32a768 105
ralphg_92 33:97e69c32a768 106 // RKI Variables & Constants
ralphg_92 34:6e74f6629a0e 107 double setpoint1 = 2.35;
ralphg_92 34:6e74f6629a0e 108 double setpoint2 = 0.785;
ralphg_92 34:6e74f6629a0e 109 double x = 10.77;
ralphg_92 34:6e74f6629a0e 110 double y = 15.73;
ralphg_92 33:97e69c32a768 111 float pi = 3.14159265359;
ralphg_92 33:97e69c32a768 112 float a = 22.25; //originally 22.25, makes for xinitial=10.766874 // length of arm 1
ralphg_92 33:97e69c32a768 113 float b = 26.5; //originally 16.48 makes for yinitial=15.733 // length of arm 2
ralphg_92 34:6e74f6629a0e 114 double alpha_old = 2.35; //INITIAL ANGLES IN RADIANS
ralphg_92 34:6e74f6629a0e 115 double beta_old = 0.785;
ralphg_92 33:97e69c32a768 116 float delta1;
ralphg_92 33:97e69c32a768 117 float delta2;
ralphg_92 33:97e69c32a768 118 float diffmotor_a;
ralphg_92 33:97e69c32a768 119 float diffmotor_b;
ralphg_92 33:97e69c32a768 120
ralphg_92 33:97e69c32a768 121 // PID Variables & Constants
ralphg_92 33:97e69c32a768 122 double Kp = 0.5;// you can set these constants however you like depending on trial & error
ralphg_92 33:97e69c32a768 123 double Ki = 0.1;
ralphg_92 33:97e69c32a768 124 double Kd = 0.1;
ralphg_92 33:97e69c32a768 125
ralphg_92 33:97e69c32a768 126 float last_error1 = 0;
ralphg_92 33:97e69c32a768 127 float new_error1 = 0;
ralphg_92 33:97e69c32a768 128 float change_error1 = 0;
ralphg_92 33:97e69c32a768 129 float total_error1 = 0;
ralphg_92 33:97e69c32a768 130 float pid_term1 = 0;
ralphg_92 33:97e69c32a768 131 float pid_term_scaled1 = 0;
ralphg_92 31:d346f9244b4a 132
ralphg_92 33:97e69c32a768 133 float last_error2 = 0;
ralphg_92 33:97e69c32a768 134 float new_error2 = 0;
ralphg_92 33:97e69c32a768 135 float change_error2 = 0;
ralphg_92 33:97e69c32a768 136 float total_error2 = 0;
ralphg_92 33:97e69c32a768 137 float pid_term2 = 0;
ralphg_92 33:97e69c32a768 138 float pid_term_scaled2 = 0;
ralphg_92 33:97e69c32a768 139
ralphg_92 33:97e69c32a768 140 // Motor Variables & Constants
ralphg_92 33:97e69c32a768 141 //float MV1 = 0;
ralphg_92 33:97e69c32a768 142 //float MV2 = 0;
ralphg_92 33:97e69c32a768 143 double count1 = 0;
ralphg_92 33:97e69c32a768 144 double count2 = 0;
ralphg_92 33:97e69c32a768 145 double angle1 = 2.35;
ralphg_92 33:97e69c32a768 146 double angle2 = 0.785;
ralphg_92 29:09c1567d6148 147
relvorelvo 25:07187cf76863 148 // BiQuad Filter Settings
relvorelvo 23:e5db011bd410 149 // Right Biceps
ralphg_92 31:d346f9244b4a 150 BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); // Filter at 10 Hz
ralphg_92 31:d346f9244b4a 151 BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); // Filter at 50 Hz
ralphg_92 31:d346f9244b4a 152 BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); // Filter at 15 Hz
relvorelvo 23:e5db011bd410 153 // Left Biceps
relvorelvo 23:e5db011bd410 154 BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 155 BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 156 BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 157 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 158 BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
relvorelvo 23:e5db011bd410 159 BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
relvorelvo 24:26659f1de039 160 BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
ralphg_92 27:ca07f895f999 161 // Right Lower Arm OR Triceps
ralphg_92 27:ca07f895f999 162 BiQuad filterhigh4(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
ralphg_92 27:ca07f895f999 163 BiQuad filternotch4(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
ralphg_92 27:ca07f895f999 164 BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
relvorelvo 23:e5db011bd410 165 //
relvorelvo 21:136a1ab8163c 166
ralphg_92 33:97e69c32a768 167 // RKI Calculations
ralphg_92 33:97e69c32a768 168 //
ralphg_92 33:97e69c32a768 169 //
ralphg_92 33:97e69c32a768 170
ralphg_92 33:97e69c32a768 171 float inversekinematics1(float x, float y){
ralphg_92 33:97e69c32a768 172 // cosine law:
ralphg_92 33:97e69c32a768 173 float c = sqrtf(x*x + y*y); // here comes the relevant math, see my inverse kinematics explaination using cosine law
ralphg_92 33:97e69c32a768 174 float alpha1 = acosf((a*a + c*c - b*b)/(2*a*c));
ralphg_92 33:97e69c32a768 175
ralphg_92 33:97e69c32a768 176 // remaining parts of alpha:
ralphg_92 33:97e69c32a768 177 float alpha2 = atan2f(y,x);
ralphg_92 33:97e69c32a768 178 float alpha = alpha1 + alpha2;
ralphg_92 33:97e69c32a768 179 return alpha;
ralphg_92 33:97e69c32a768 180 }
ralphg_92 33:97e69c32a768 181
ralphg_92 33:97e69c32a768 182 float inversekinematics2(float x, float y){
ralphg_92 33:97e69c32a768 183 // cosine law:
ralphg_92 33:97e69c32a768 184 float c = sqrtf(x*x + y*y); // here comes the relevant math, see my inverse kinematics explaination using cosine law
ralphg_92 33:97e69c32a768 185 float beta = acosf((a*a + b*b - c*c)/(2*a*b));
ralphg_92 33:97e69c32a768 186 return beta;
ralphg_92 33:97e69c32a768 187 }
ralphg_92 33:97e69c32a768 188
ralphg_92 29:09c1567d6148 189 // Finding max values for correct motor switch if the button is pressed
ralphg_92 33:97e69c32a768 190 //
ralphg_92 33:97e69c32a768 191 //
ralphg_92 31:d346f9244b4a 192 // calibration of both biceps
ralphg_92 29:09c1567d6148 193 void get_max1(){
ralphg_92 29:09c1567d6148 194 if (max_reader12==0){
ralphg_92 29:09c1567d6148 195 green = !green;
ralphg_92 29:09c1567d6148 196 red = 1;
ralphg_92 29:09c1567d6148 197 blue = 1;
ralphg_92 31:d346f9244b4a 198 for(int n=0;n<2000;n++){ // measure 2000 samples and filter it
ralphg_92 29:09c1567d6148 199
ralphg_92 31:d346f9244b4a 200 emg1 = emg1_in.read(); // read out emg
ralphg_92 31:d346f9244b4a 201 emg1highfilter = filterhigh1.step(emg1); // high pass filtered
ralphg_92 31:d346f9244b4a 202 emg1notchfilter = filternotch1.step(emg1highfilter); // notch filtered
ralphg_92 31:d346f9244b4a 203 emg1abs = fabs(emg1notchfilter); // take the absolute value
ralphg_92 31:d346f9244b4a 204 emg1lowfilter = filterlow1.step(emg1abs); // low pass filtered
ralphg_92 32:a779b1131977 205 RB0 = emg1lowfilter;
ralphg_92 32:a779b1131977 206 // Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
ralphg_92 32:a779b1131977 207 MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
ralphg_92 32:a779b1131977 208 +RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
ralphg_92 32:a779b1131977 209 *AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
ralphg_92 32:a779b1131977 210 *AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
ralphg_92 32:a779b1131977 211 +RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
ralphg_92 32:a779b1131977 212 +RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
ralphg_92 32:a779b1131977 213 +RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
ralphg_92 32:a779b1131977 214 +RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
ralphg_92 32:a779b1131977 215 +RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
ralphg_92 32:a779b1131977 216 +RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
ralphg_92 32:a779b1131977 217 RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
ralphg_92 32:a779b1131977 218 RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;
ralphg_92 32:a779b1131977 219 RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69;
ralphg_92 32:a779b1131977 220 RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63;
ralphg_92 32:a779b1131977 221 RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57;
ralphg_92 32:a779b1131977 222 RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51;
ralphg_92 32:a779b1131977 223 RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45;
ralphg_92 32:a779b1131977 224 RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39;
ralphg_92 32:a779b1131977 225 RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33;
ralphg_92 32:a779b1131977 226 RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27;
ralphg_92 32:a779b1131977 227 RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21;
ralphg_92 32:a779b1131977 228 RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15;
ralphg_92 32:a779b1131977 229 RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9;
ralphg_92 32:a779b1131977 230 RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2;
ralphg_92 32:a779b1131977 231 RB2 = RB1; RB1 = RB0;
ralphg_92 29:09c1567d6148 232
ralphg_92 31:d346f9244b4a 233 emg2 = emg2_in.read(); // read out emg
ralphg_92 31:d346f9244b4a 234 emg2highfilter = filterhigh2.step(emg2); // high pass filtered
ralphg_92 31:d346f9244b4a 235 emg2notchfilter = filternotch2.step(emg2highfilter); // notch filtered
ralphg_92 31:d346f9244b4a 236 emg2abs = fabs(emg2notchfilter); // take the absolute value
ralphg_92 31:d346f9244b4a 237 emg2lowfilter = filterlow2.step(emg2abs); // low pass filtered
ralphg_92 32:a779b1131977 238 LB0 = emg2lowfilter;
ralphg_92 32:a779b1131977 239 // same story as with the right biceps
ralphg_92 32:a779b1131977 240 MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
ralphg_92 32:a779b1131977 241 +LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
ralphg_92 32:a779b1131977 242 *AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
ralphg_92 32:a779b1131977 243 *AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
ralphg_92 32:a779b1131977 244 +LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
ralphg_92 32:a779b1131977 245 +LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
ralphg_92 32:a779b1131977 246 +LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
ralphg_92 32:a779b1131977 247 +LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
ralphg_92 32:a779b1131977 248 +LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
ralphg_92 32:a779b1131977 249 +LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
ralphg_92 32:a779b1131977 250 LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
ralphg_92 32:a779b1131977 251 LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
ralphg_92 32:a779b1131977 252 LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69;
ralphg_92 32:a779b1131977 253 LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63;
ralphg_92 32:a779b1131977 254 LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57;
ralphg_92 32:a779b1131977 255 LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51;
ralphg_92 32:a779b1131977 256 LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45;
ralphg_92 32:a779b1131977 257 LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39;
ralphg_92 32:a779b1131977 258 LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33;
ralphg_92 32:a779b1131977 259 LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27;
ralphg_92 32:a779b1131977 260 LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21;
ralphg_92 32:a779b1131977 261 LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15;
ralphg_92 32:a779b1131977 262 LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9;
ralphg_92 32:a779b1131977 263 LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2;
ralphg_92 32:a779b1131977 264 LB2 = LB1; LB1 = LB0;
ralphg_92 29:09c1567d6148 265
ralphg_92 32:a779b1131977 266 if (max1<MOVAVG_RB){
ralphg_92 32:a779b1131977 267 max1 = MOVAVG_RB; // set the max value at the highest measured value
ralphg_92 29:09c1567d6148 268 }
ralphg_92 32:a779b1131977 269 if (max2<MOVAVG_LB){
ralphg_92 32:a779b1131977 270 max2 = MOVAVG_LB; // set the max value at the highest measured value
ralphg_92 29:09c1567d6148 271 }
ralphg_92 31:d346f9244b4a 272 wait(0.001f); // measure at 1000Hz
ralphg_92 29:09c1567d6148 273 }
ralphg_92 29:09c1567d6148 274 wait(0.2f);
ralphg_92 29:09c1567d6148 275 green = 1;
ralphg_92 29:09c1567d6148 276 }
ralphg_92 33:97e69c32a768 277 maxpart1 = 0.15*max1; // set cut off voltage at 13% of max for right biceps
ralphg_92 32:a779b1131977 278 maxpart2 = 0.15*max2; // set cut off voltage at 13% of max for left biceps
ralphg_92 29:09c1567d6148 279 }
ralphg_92 29:09c1567d6148 280
ralphg_92 31:d346f9244b4a 281 // calibration of both lower arms
relvorelvo 23:e5db011bd410 282 void get_max3(){
ralphg_92 27:ca07f895f999 283 if (max_reader34==0){
relvorelvo 23:e5db011bd410 284 green = 1;
relvorelvo 23:e5db011bd410 285 blue = 1;
relvorelvo 23:e5db011bd410 286 red = !red;
relvorelvo 23:e5db011bd410 287 for(int n=0;n<2000;n++){
relvorelvo 23:e5db011bd410 288
relvorelvo 23:e5db011bd410 289 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 290 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 291 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 292 emg3abs = fabs(emg3notchfilter);
relvorelvo 23:e5db011bd410 293 emg3lowfilter = filterlow3.step(emg3abs);
ralphg_92 32:a779b1131977 294 LL0 = emg3lowfilter;
ralphg_92 32:a779b1131977 295 // same story as with the right biceps
ralphg_92 32:a779b1131977 296 MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
ralphg_92 32:a779b1131977 297 +LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
ralphg_92 32:a779b1131977 298 *AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
ralphg_92 32:a779b1131977 299 *AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
ralphg_92 32:a779b1131977 300 +LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
ralphg_92 32:a779b1131977 301 +LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
ralphg_92 32:a779b1131977 302 +LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
ralphg_92 32:a779b1131977 303 +LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
ralphg_92 32:a779b1131977 304 +LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
ralphg_92 32:a779b1131977 305 +LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
ralphg_92 32:a779b1131977 306 LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
ralphg_92 32:a779b1131977 307 LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
ralphg_92 32:a779b1131977 308 LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69;
ralphg_92 32:a779b1131977 309 LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63;
ralphg_92 32:a779b1131977 310 LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57;
ralphg_92 32:a779b1131977 311 LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51;
ralphg_92 32:a779b1131977 312 LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45;
ralphg_92 32:a779b1131977 313 LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39;
ralphg_92 32:a779b1131977 314 LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33;
ralphg_92 32:a779b1131977 315 LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27;
ralphg_92 32:a779b1131977 316 LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21;
ralphg_92 32:a779b1131977 317 LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15;
ralphg_92 32:a779b1131977 318 LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9;
ralphg_92 32:a779b1131977 319 LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2;
ralphg_92 32:a779b1131977 320 LL2 = LL1; LL1 = LL0;
relvorelvo 23:e5db011bd410 321
ralphg_92 27:ca07f895f999 322 emg4 = emg4_in.read();
ralphg_92 27:ca07f895f999 323 emg4highfilter = filterhigh4.step(emg4);
ralphg_92 27:ca07f895f999 324 emg4notchfilter = filternotch4.step(emg4highfilter);
ralphg_92 27:ca07f895f999 325 emg4abs = fabs(emg4notchfilter);
ralphg_92 27:ca07f895f999 326 emg4lowfilter = filterlow4.step(emg4abs);
ralphg_92 32:a779b1131977 327 RL0 = emg4lowfilter;
ralphg_92 32:a779b1131977 328 // same story as with the right biceps
ralphg_92 32:a779b1131977 329 MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
ralphg_92 32:a779b1131977 330 +RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
ralphg_92 32:a779b1131977 331 *AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
ralphg_92 32:a779b1131977 332 *AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
ralphg_92 32:a779b1131977 333 +RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
ralphg_92 32:a779b1131977 334 +RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
ralphg_92 32:a779b1131977 335 +RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
ralphg_92 32:a779b1131977 336 +RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
ralphg_92 32:a779b1131977 337 +RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
ralphg_92 32:a779b1131977 338 +RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
ralphg_92 32:a779b1131977 339 RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
ralphg_92 32:a779b1131977 340 RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
ralphg_92 32:a779b1131977 341 RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69;
ralphg_92 32:a779b1131977 342 RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63;
ralphg_92 32:a779b1131977 343 RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57;
ralphg_92 32:a779b1131977 344 RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51;
ralphg_92 32:a779b1131977 345 RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45;
ralphg_92 32:a779b1131977 346 RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39;
ralphg_92 32:a779b1131977 347 RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33;
ralphg_92 32:a779b1131977 348 RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27;
ralphg_92 32:a779b1131977 349 RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21;
ralphg_92 32:a779b1131977 350 RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15;
ralphg_92 32:a779b1131977 351 RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9;
ralphg_92 32:a779b1131977 352 RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2;
ralphg_92 32:a779b1131977 353 RL2 = RL1; RL1 = RL0;
ralphg_92 27:ca07f895f999 354
ralphg_92 32:a779b1131977 355 if (max3<MOVAVG_LL){
ralphg_92 32:a779b1131977 356 max3 = MOVAVG_LL; // set the max value at the highest measured value
relvorelvo 23:e5db011bd410 357 }
ralphg_92 32:a779b1131977 358 if (max4<MOVAVG_RL){
ralphg_92 32:a779b1131977 359 max4 = MOVAVG_RL; // set the max value at the highest measured value
ralphg_92 27:ca07f895f999 360 }
relvorelvo 23:e5db011bd410 361 wait(0.001f);
relvorelvo 23:e5db011bd410 362 }
relvorelvo 23:e5db011bd410 363 wait(0.2f);
relvorelvo 23:e5db011bd410 364 red = 1;
relvorelvo 23:e5db011bd410 365 }
ralphg_92 32:a779b1131977 366 maxpart3 = 0.17*max3; // set cut off voltage at 15% of max for left lower arm
ralphg_92 32:a779b1131977 367 maxpart4 = 0.17*max4; // set cut off voltage at 15% of max for right lower arm
vsluiter 2:e314bb3b2d99 368 }
vsluiter 0:32bb76391d89 369
ralphg_92 33:97e69c32a768 370 // EMG Filtering & Scope
ralphg_92 33:97e69c32a768 371 //
ralphg_92 33:97e69c32a768 372 //
ralphg_92 34:6e74f6629a0e 373 void filter(/*double setpoint1, double setpoint2*/) {
relvorelvo 23:e5db011bd410 374 // Right Biceps
relvorelvo 23:e5db011bd410 375 emg1 = emg1_in.read();
relvorelvo 23:e5db011bd410 376 emg1highfilter = filterhigh1.step(emg1);
relvorelvo 23:e5db011bd410 377 emg1notchfilter = filternotch1.step(emg1highfilter);
relvorelvo 23:e5db011bd410 378 emg1abs = fabs(emg1notchfilter);
ralphg_92 31:d346f9244b4a 379 emg1lowfilter = filterlow1.step(emg1abs); // Final Right Biceps values to be sent
ralphg_92 32:a779b1131977 380 RB0 = emg1lowfilter;
ralphg_92 32:a779b1131977 381 // Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
ralphg_92 32:a779b1131977 382 MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
ralphg_92 32:a779b1131977 383 +RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
ralphg_92 32:a779b1131977 384 *AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
ralphg_92 32:a779b1131977 385 *AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
ralphg_92 32:a779b1131977 386 +RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
ralphg_92 32:a779b1131977 387 +RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
ralphg_92 32:a779b1131977 388 +RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
ralphg_92 32:a779b1131977 389 +RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
ralphg_92 32:a779b1131977 390 +RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
ralphg_92 32:a779b1131977 391 +RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
ralphg_92 32:a779b1131977 392 RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
ralphg_92 32:a779b1131977 393 RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;
ralphg_92 32:a779b1131977 394 RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69;
ralphg_92 32:a779b1131977 395 RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63;
ralphg_92 32:a779b1131977 396 RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57;
ralphg_92 32:a779b1131977 397 RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51;
ralphg_92 32:a779b1131977 398 RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45;
ralphg_92 32:a779b1131977 399 RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39;
ralphg_92 32:a779b1131977 400 RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33;
ralphg_92 32:a779b1131977 401 RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27;
ralphg_92 32:a779b1131977 402 RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21;
ralphg_92 32:a779b1131977 403 RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15;
ralphg_92 32:a779b1131977 404 RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9;
ralphg_92 32:a779b1131977 405 RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2;
ralphg_92 32:a779b1131977 406 RB2 = RB1; RB1 = RB0;
relvorelvo 23:e5db011bd410 407 // Left Biceps
relvorelvo 23:e5db011bd410 408 emg2 = emg2_in.read();
relvorelvo 23:e5db011bd410 409 emg2highfilter = filterhigh2.step(emg2);
relvorelvo 23:e5db011bd410 410 emg2notchfilter = filternotch2.step(emg2highfilter);
relvorelvo 23:e5db011bd410 411 emg2abs = fabs(emg2notchfilter);
ralphg_92 31:d346f9244b4a 412 emg2lowfilter = filterlow2.step(emg2abs); // Final Left Biceps values to be sent
ralphg_92 32:a779b1131977 413 LB0 = emg2lowfilter;
ralphg_92 32:a779b1131977 414 // same story as with the right biceps
ralphg_92 32:a779b1131977 415 MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
ralphg_92 32:a779b1131977 416 +LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
ralphg_92 32:a779b1131977 417 *AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
ralphg_92 32:a779b1131977 418 *AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
ralphg_92 32:a779b1131977 419 +LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
ralphg_92 32:a779b1131977 420 +LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
ralphg_92 32:a779b1131977 421 +LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
ralphg_92 32:a779b1131977 422 +LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
ralphg_92 32:a779b1131977 423 +LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
ralphg_92 32:a779b1131977 424 +LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
ralphg_92 32:a779b1131977 425 LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
ralphg_92 32:a779b1131977 426 LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
ralphg_92 32:a779b1131977 427 LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69;
ralphg_92 32:a779b1131977 428 LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63;
ralphg_92 32:a779b1131977 429 LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57;
ralphg_92 32:a779b1131977 430 LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51;
ralphg_92 32:a779b1131977 431 LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45;
ralphg_92 32:a779b1131977 432 LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39;
ralphg_92 32:a779b1131977 433 LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33;
ralphg_92 32:a779b1131977 434 LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27;
ralphg_92 32:a779b1131977 435 LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21;
ralphg_92 32:a779b1131977 436 LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15;
ralphg_92 32:a779b1131977 437 LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9;
ralphg_92 32:a779b1131977 438 LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2;
ralphg_92 32:a779b1131977 439 LB2 = LB1; LB1 = LB0;
relvorelvo 23:e5db011bd410 440 // Left Lower Arm OR Triceps
relvorelvo 23:e5db011bd410 441 emg3 = emg3_in.read();
relvorelvo 23:e5db011bd410 442 emg3highfilter = filterhigh3.step(emg3);
relvorelvo 23:e5db011bd410 443 emg3notchfilter = filternotch3.step(emg3highfilter);
relvorelvo 23:e5db011bd410 444 emg3abs = fabs(emg3notchfilter);
ralphg_92 31:d346f9244b4a 445 emg3lowfilter = filterlow3.step(emg3abs); // Final Lower Arm values to be sent
ralphg_92 32:a779b1131977 446 LL0 = emg3lowfilter;
ralphg_92 32:a779b1131977 447 // same story as with the right biceps
ralphg_92 32:a779b1131977 448 MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
ralphg_92 32:a779b1131977 449 +LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
ralphg_92 32:a779b1131977 450 *AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
ralphg_92 32:a779b1131977 451 *AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
ralphg_92 32:a779b1131977 452 +LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
ralphg_92 32:a779b1131977 453 +LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
ralphg_92 32:a779b1131977 454 +LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
ralphg_92 32:a779b1131977 455 +LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
ralphg_92 32:a779b1131977 456 +LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
ralphg_92 32:a779b1131977 457 +LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
ralphg_92 32:a779b1131977 458 LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
ralphg_92 32:a779b1131977 459 LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
ralphg_92 32:a779b1131977 460 LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69;
ralphg_92 32:a779b1131977 461 LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63;
ralphg_92 32:a779b1131977 462 LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57;
ralphg_92 32:a779b1131977 463 LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51;
ralphg_92 32:a779b1131977 464 LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45;
ralphg_92 32:a779b1131977 465 LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39;
ralphg_92 32:a779b1131977 466 LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33;
ralphg_92 32:a779b1131977 467 LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27;
ralphg_92 32:a779b1131977 468 LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21;
ralphg_92 32:a779b1131977 469 LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15;
ralphg_92 32:a779b1131977 470 LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9;
ralphg_92 32:a779b1131977 471 LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2;
ralphg_92 32:a779b1131977 472 LL2 = LL1; LL1 = LL0;
ralphg_92 27:ca07f895f999 473 // Right Lower Arm OR Triceps
ralphg_92 27:ca07f895f999 474 emg4 = emg4_in.read();
ralphg_92 27:ca07f895f999 475 emg4highfilter = filterhigh4.step(emg4);
ralphg_92 27:ca07f895f999 476 emg4notchfilter = filternotch4.step(emg4highfilter);
ralphg_92 27:ca07f895f999 477 emg4abs = fabs(emg4notchfilter);
ralphg_92 32:a779b1131977 478 emg4lowfilter = filterlow4.step(emg4abs); // Final Lower Arm values to be sent
ralphg_92 32:a779b1131977 479 RL0 = emg4lowfilter;
ralphg_92 32:a779b1131977 480 // same story as with the right biceps
ralphg_92 32:a779b1131977 481 MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
ralphg_92 32:a779b1131977 482 +RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
ralphg_92 32:a779b1131977 483 *AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
ralphg_92 32:a779b1131977 484 *AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
ralphg_92 32:a779b1131977 485 +RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
ralphg_92 32:a779b1131977 486 +RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
ralphg_92 32:a779b1131977 487 +RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
ralphg_92 32:a779b1131977 488 +RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
ralphg_92 32:a779b1131977 489 +RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
ralphg_92 32:a779b1131977 490 +RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
ralphg_92 32:a779b1131977 491 RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
ralphg_92 32:a779b1131977 492 RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
ralphg_92 32:a779b1131977 493 RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69;
ralphg_92 32:a779b1131977 494 RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63;
ralphg_92 32:a779b1131977 495 RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57;
ralphg_92 32:a779b1131977 496 RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51;
ralphg_92 32:a779b1131977 497 RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45;
ralphg_92 32:a779b1131977 498 RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39;
ralphg_92 32:a779b1131977 499 RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33;
ralphg_92 32:a779b1131977 500 RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27;
ralphg_92 32:a779b1131977 501 RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21;
ralphg_92 32:a779b1131977 502 RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15;
ralphg_92 32:a779b1131977 503 RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9;
ralphg_92 32:a779b1131977 504 RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2;
ralphg_92 32:a779b1131977 505 RL2 = RL1; RL1 = RL0;
ralphg_92 29:09c1567d6148 506
relvorelvo 23:e5db011bd410 507
ralphg_92 31:d346f9244b4a 508 // Compare measurement to the calibrated value to decide actions
ralphg_92 31:d346f9244b4a 509 // more options could be added if the callibration is well defined enough
ralphg_92 31:d346f9244b4a 510 // This part checks for right biceps contractions only
ralphg_92 33:97e69c32a768 511 /* if (maxpart1<MOVAVG_RB || maxpart2<MOVAVG_LB || maxpart3<MOVAVG_LL || maxpart4<MOVAVG_RL){
ralphg_92 33:97e69c32a768 512 wait(0.15f);*/
ralphg_92 33:97e69c32a768 513 if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 29:09c1567d6148 514 red = 1;
ralphg_92 29:09c1567d6148 515 blue = 1;
ralphg_92 29:09c1567d6148 516 green = 0;
ralphg_92 33:97e69c32a768 517 //MV1 = 0.5;
ralphg_92 33:97e69c32a768 518 //MV2 = 0;
ralphg_92 35:82b432f5aef7 519 x = x + 0.1;
ralphg_92 34:6e74f6629a0e 520 double alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
ralphg_92 34:6e74f6629a0e 521 double beta = inversekinematics2(x,y);
ralphg_92 33:97e69c32a768 522 setpoint1 = alpha;
ralphg_92 33:97e69c32a768 523 setpoint2 = beta;
ralphg_92 35:82b432f5aef7 524 wait(0.1f);
ralphg_92 33:97e69c32a768 525 }
ralphg_92 33:97e69c32a768 526 // This part checks for left biceps contractions only
ralphg_92 33:97e69c32a768 527 else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 29:09c1567d6148 528 red = 0;
ralphg_92 29:09c1567d6148 529 blue = 1;
ralphg_92 29:09c1567d6148 530 green = 1;
ralphg_92 33:97e69c32a768 531 //MV1 = -0.5;
ralphg_92 33:97e69c32a768 532 //MV2 = 0;
ralphg_92 35:82b432f5aef7 533 x = x - 0.1;
ralphg_92 34:6e74f6629a0e 534 double alpha = inversekinematics1(x,y);
ralphg_92 34:6e74f6629a0e 535 double beta = inversekinematics2(x,y);
ralphg_92 33:97e69c32a768 536 setpoint1 = alpha;
ralphg_92 33:97e69c32a768 537 setpoint2 = beta;
ralphg_92 35:82b432f5aef7 538 wait(0.1f);
ralphg_92 33:97e69c32a768 539 }
ralphg_92 33:97e69c32a768 540 // This part checks for left lower arm contractions only
ralphg_92 33:97e69c32a768 541 else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
relvorelvo 23:e5db011bd410 542 red = 1;
relvorelvo 23:e5db011bd410 543 blue = 0;
relvorelvo 23:e5db011bd410 544 green = 1;
ralphg_92 33:97e69c32a768 545 //MV1 = 0;
ralphg_92 33:97e69c32a768 546 //MV2 = 0.5;
ralphg_92 35:82b432f5aef7 547 y = y + 0.1;
ralphg_92 34:6e74f6629a0e 548 double alpha = inversekinematics1(x,y);
ralphg_92 34:6e74f6629a0e 549 double beta = inversekinematics2(x,y);
ralphg_92 33:97e69c32a768 550 setpoint1 = alpha;
ralphg_92 33:97e69c32a768 551 setpoint2 = beta;
ralphg_92 35:82b432f5aef7 552 wait(0.1f);
ralphg_92 33:97e69c32a768 553 }
ralphg_92 33:97e69c32a768 554 // This part checks for right lower arm contractions only
ralphg_92 33:97e69c32a768 555 else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
ralphg_92 29:09c1567d6148 556 red = 0;
relvorelvo 23:e5db011bd410 557 blue = 1;
relvorelvo 23:e5db011bd410 558 green = 0;
ralphg_92 33:97e69c32a768 559 //MV1 = 0;
ralphg_92 33:97e69c32a768 560 //MV2 = -0.5;
ralphg_92 35:82b432f5aef7 561 y = y - 0.1;
ralphg_92 34:6e74f6629a0e 562 double alpha = inversekinematics1(x,y);
ralphg_92 34:6e74f6629a0e 563 double beta = inversekinematics2(x,y);
ralphg_92 33:97e69c32a768 564 setpoint1 = alpha;
ralphg_92 33:97e69c32a768 565 setpoint2 = beta;
ralphg_92 35:82b432f5aef7 566 wait(0.2f);
ralphg_92 33:97e69c32a768 567 }
ralphg_92 33:97e69c32a768 568 /* // This part checks for both lower arm contractions only
ralphg_92 33:97e69c32a768 569 else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
ralphg_92 31:d346f9244b4a 570 red = 0;
ralphg_92 31:d346f9244b4a 571 blue = 0;
ralphg_92 31:d346f9244b4a 572 green = 0;
ralphg_92 33:97e69c32a768 573 //MV1 = -0.5;
ralphg_92 33:97e69c32a768 574 //MV2 = -0.5;
ralphg_92 33:97e69c32a768 575 }
ralphg_92 33:97e69c32a768 576 // This part checks for both biceps contractions only
ralphg_92 33:97e69c32a768 577 else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 31:d346f9244b4a 578 red = 0;
ralphg_92 31:d346f9244b4a 579 blue = 0;
ralphg_92 31:d346f9244b4a 580 green = 0;
ralphg_92 33:97e69c32a768 581 //MV1 = 0.5;
ralphg_92 33:97e69c32a768 582 //MV2 = 0.5;
ralphg_92 33:97e69c32a768 583 }
ralphg_92 33:97e69c32a768 584 // This part checks for right lower arm & left biceps contractions only
ralphg_92 33:97e69c32a768 585 else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
ralphg_92 30:2c67abcdb892 586 red = 0;
ralphg_92 30:2c67abcdb892 587 blue = 0;
ralphg_92 30:2c67abcdb892 588 green = 0;
ralphg_92 33:97e69c32a768 589 //MV1 = 0.5;
ralphg_92 33:97e69c32a768 590 //MV2 = -0.5;
ralphg_92 33:97e69c32a768 591 }
ralphg_92 33:97e69c32a768 592 // This part checks for left lower arm & right biceps contractions only
ralphg_92 33:97e69c32a768 593 else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
ralphg_92 30:2c67abcdb892 594 red = 0;
ralphg_92 30:2c67abcdb892 595 blue = 0;
ralphg_92 30:2c67abcdb892 596 green = 0;
ralphg_92 33:97e69c32a768 597 //MV1 = -0.5;
ralphg_92 33:97e69c32a768 598 //MV2 = 0.5;
ralphg_92 33:97e69c32a768 599 }*/
ralphg_92 33:97e69c32a768 600 //}
ralphg_92 33:97e69c32a768 601 else {
ralphg_92 33:97e69c32a768 602 red = 1; // Shut down all led colors if no movement is registered
ralphg_92 33:97e69c32a768 603 blue = 1;
ralphg_92 33:97e69c32a768 604 green = 1;
ralphg_92 33:97e69c32a768 605 //MV1 = 0;
ralphg_92 33:97e69c32a768 606 //MV2 = 0;
ralphg_92 33:97e69c32a768 607 }
ralphg_92 33:97e69c32a768 608
ralphg_92 31:d346f9244b4a 609 // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
ralphg_92 32:a779b1131977 610 scope.set(0, MOVAVG_RB ); // plot Right biceps voltage
ralphg_92 32:a779b1131977 611 scope.set(1, MOVAVG_LB ); // Plot Left biceps voltage
ralphg_92 32:a779b1131977 612 scope.set(2, MOVAVG_LL ); // Plot Lower Left Arm voltage
ralphg_92 32:a779b1131977 613 scope.set(3, MOVAVG_RL ); // Plot Lower Right Arm Voltage
ralphg_92 31:d346f9244b4a 614 scope.send(); // send everything to the HID scope
ralphg_92 31:d346f9244b4a 615
relvorelvo 23:e5db011bd410 616 }
ralphg_92 34:6e74f6629a0e 617 /*
ralphg_92 33:97e69c32a768 618 // PID calculations
ralphg_92 33:97e69c32a768 619 //
ralphg_92 33:97e69c32a768 620 //
ralphg_92 34:6e74f6629a0e 621 void PIDcalculation1() {
ralphg_92 33:97e69c32a768 622 //PID calculation for motor 1
ralphg_92 34:6e74f6629a0e 623 filter();
ralphg_92 33:97e69c32a768 624 count1 = Encoder1.getPulses();
ralphg_92 34:6e74f6629a0e 625 angle1 += (0.000748 * count1);
ralphg_92 34:6e74f6629a0e 626 new_error1 = setpoint1 - angle1;
ralphg_92 33:97e69c32a768 627
ralphg_92 33:97e69c32a768 628 change_error1 = new_error1 - last_error1;
ralphg_92 33:97e69c32a768 629 total_error1 += new_error1;
ralphg_92 33:97e69c32a768 630 pid_term1 = (Kp * new_error1) + (Ki * total_error1) + (Kd * change_error1);
ralphg_92 33:97e69c32a768 631 if (pid_term1<-255) {
ralphg_92 33:97e69c32a768 632 pid_term1 = -255;
ralphg_92 33:97e69c32a768 633 }
ralphg_92 33:97e69c32a768 634 if (pid_term1>255) {
ralphg_92 33:97e69c32a768 635 pid_term1 = 255;
ralphg_92 33:97e69c32a768 636 }
ralphg_92 33:97e69c32a768 637 pid_term_scaled1 = abs(pid_term1);
ralphg_92 33:97e69c32a768 638
ralphg_92 33:97e69c32a768 639 last_error1 = new_error1;
ralphg_92 34:6e74f6629a0e 640 }
ralphg_92 34:6e74f6629a0e 641 void PIDcalculation2() {
ralphg_92 33:97e69c32a768 642 //PID calculation for motor 2
ralphg_92 34:6e74f6629a0e 643 filter();
ralphg_92 33:97e69c32a768 644 count2 = Encoder2.getPulses();
ralphg_92 34:6e74f6629a0e 645 angle2 += (0.000748 * count2);
ralphg_92 34:6e74f6629a0e 646 new_error2 = setpoint2 - angle2;
ralphg_92 33:97e69c32a768 647
ralphg_92 33:97e69c32a768 648 change_error2 = new_error2 - last_error2;
ralphg_92 33:97e69c32a768 649 total_error2 += new_error2;
ralphg_92 33:97e69c32a768 650 pid_term2 = (Kp * new_error2) + (Ki * total_error2) + (Kd * change_error2);
ralphg_92 33:97e69c32a768 651 if (pid_term2<-255) {
ralphg_92 33:97e69c32a768 652 pid_term2 = -255;
ralphg_92 33:97e69c32a768 653 }
ralphg_92 33:97e69c32a768 654 if (pid_term2>255) {
ralphg_92 33:97e69c32a768 655 pid_term2 = 255;
ralphg_92 33:97e69c32a768 656 }
ralphg_92 33:97e69c32a768 657 pid_term_scaled2 = abs(pid_term2);
ralphg_92 33:97e69c32a768 658
ralphg_92 33:97e69c32a768 659 last_error2 = new_error2;
ralphg_92 33:97e69c32a768 660 }
ralphg_92 34:6e74f6629a0e 661 */
ralphg_92 33:97e69c32a768 662 // Motor control
ralphg_92 33:97e69c32a768 663 //
ralphg_92 33:97e69c32a768 664 //
ralphg_92 33:97e69c32a768 665 // check to see if motor1 needs to be activated
ralphg_92 34:6e74f6629a0e 666 void SetMotor1() {
ralphg_92 34:6e74f6629a0e 667 //PIDcalculation1();
ralphg_92 34:6e74f6629a0e 668 filter();
ralphg_92 34:6e74f6629a0e 669 angle1 += (0.0981 * count1);
ralphg_92 33:97e69c32a768 670 if (angle1<setpoint1){
ralphg_92 33:97e69c32a768 671 motor1direction = 0; // counterclockwise rotation
ralphg_92 31:d346f9244b4a 672 }
ralphg_92 31:d346f9244b4a 673 else {
ralphg_92 33:97e69c32a768 674 motor1direction = 1; // clockwise rotation
ralphg_92 33:97e69c32a768 675 }
ralphg_92 35:82b432f5aef7 676 while (angle1<setpoint1 || angle1>setpoint1) {
ralphg_92 35:82b432f5aef7 677 if (angle2<setpoint2 || angle2>setpoint2) {
ralphg_92 35:82b432f5aef7 678 motor1pwm = 0.5;
ralphg_92 35:82b432f5aef7 679 }
ralphg_92 35:82b432f5aef7 680 else if (angle1 == setpoint1){
ralphg_92 35:82b432f5aef7 681 motor1pwm = 0;
ralphg_92 35:82b432f5aef7 682 }
ralphg_92 31:d346f9244b4a 683 }
ralphg_92 31:d346f9244b4a 684 }
ralphg_92 33:97e69c32a768 685 // check if motor1 needs to be activated
ralphg_92 34:6e74f6629a0e 686 void SetMotor2() {
ralphg_92 34:6e74f6629a0e 687 filter();
ralphg_92 34:6e74f6629a0e 688 //PIDcalculation2();
ralphg_92 34:6e74f6629a0e 689 angle2 += (0.0981 * count2);
ralphg_92 33:97e69c32a768 690 if (angle2<setpoint2){
ralphg_92 33:97e69c32a768 691 motor2direction = 0; // counterclockwise rotation
ralphg_92 33:97e69c32a768 692 }
ralphg_92 33:97e69c32a768 693 else {
ralphg_92 31:d346f9244b4a 694 motor2direction = 1; // clockwise rotation
ralphg_92 31:d346f9244b4a 695 }
ralphg_92 35:82b432f5aef7 696 while (angle2<setpoint2 || angle2>setpoint2) {
ralphg_92 35:82b432f5aef7 697 if (angle2<setpoint2 || angle2>setpoint2) {
ralphg_92 35:82b432f5aef7 698 motor2pwm = 0.5;
ralphg_92 35:82b432f5aef7 699 }
ralphg_92 35:82b432f5aef7 700 else if (angle2 == setpoint2){
ralphg_92 35:82b432f5aef7 701 motor2pwm = 0;
ralphg_92 35:82b432f5aef7 702 }
ralphg_92 31:d346f9244b4a 703 }
ralphg_92 31:d346f9244b4a 704 }
ralphg_92 31:d346f9244b4a 705
ralphg_92 33:97e69c32a768 706 void MeasureAndControl(void) {
ralphg_92 34:6e74f6629a0e 707 SetMotor1();
ralphg_92 34:6e74f6629a0e 708 SetMotor2();
ralphg_92 31:d346f9244b4a 709 }
ralphg_92 31:d346f9244b4a 710
ralphg_92 31:d346f9244b4a 711
relvorelvo 23:e5db011bd410 712 int main(){
ralphg_92 33:97e69c32a768 713 pc.baud(115200);
ralphg_92 31:d346f9244b4a 714 main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
ralphg_92 31:d346f9244b4a 715 max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
relvorelvo 23:e5db011bd410 716 max_read3.attach(&get_max3, 2);
ralphg_92 32:a779b1131977 717 Motorcontrol.attach(&MeasureAndControl,0.5);
ralphg_92 34:6e74f6629a0e 718 //PIDtimer.attach(&PIDcalculation1, 0.005);
ralphg_92 34:6e74f6629a0e 719 //PIDtimer.attach(&PIDcalculation2, 0.005);
tomlankhorst 15:0da764eea774 720 while(1) {}
vsluiter 0:32bb76391d89 721 }