Ralph Gerlings / Mbed 2 deprecated backto1motor

even versimpelen

Dependencies:   HIDScope MODSERIAL PID QEI biquadFilter mbed

Fork of cpfromralph1 by Remi van Veen

main.cpp@33:97e69c32a768, 2017-11-03 (annotated)

Committer:
ralphg_92
Date:
Fri Nov 03 00:25:57 2017 +0000
Revision:
33:97e69c32a768
Parent:
32:a779b1131977
Child:
34:6e74f6629a0e
it runs the filter loop but the motor goes haywire

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