Ralph Gerlings
try this for a change
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
EMG_5
by 
Ralph Gerlings
main.cpp
- Committer:
- ralphg_92
- Date:
- 2017-11-03
- Revision:
- 41:08d41bb622bb
- Parent:
- 40:c40bd1a84e7c
- Child:
- 42:3aa03b5cefb0
File content as of revision 41:08d41bb622bb:
#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "QEI.h"
//Define Objects
AnalogIn emg1_in( A0 ); // Read Out The Signal
AnalogIn emg2_in( A1 );
AnalogIn emg3_in( A2 );
AnalogIn emg4_in( A3 );
DigitalIn max_reader12( SW2 ); // Define Button Press
DigitalIn max_reader34( SW3 );
DigitalOut motor1direction( D4 );
PwmOut motor1pwm( D5);
PwmOut motor2pwm( D6 );
DigitalOut motor2direction( D7 );
QEI Encoder1(D10, D11, NC, 64); // Encoder
QEI Encoder2(D12, D13, NC, 64);
Ticker main_timer;
Ticker max_read1;
Ticker max_read3;
Ticker Motorcontrol;
Ticker tencoder;
HIDScope scope( 4 );
DigitalOut red(LED_RED);
DigitalOut blue(LED_BLUE);
DigitalOut green(LED_GREEN);
Serial pc(USBTX, USBRX);
// EMG Variables & Constants
//Right Biceps
double emg1;
double emg1highfilter;
double emg1notchfilter;
double emg1abs;
double emg1lowfilter;
double max1;
double maxpart1;
// Left Biceps
double emg2;
double emg2highfilter;
double emg2notchfilter;
double emg2abs;
double emg2lowfilter;
double max2;
double maxpart2;
// Left Lower Arm
double emg3;
double emg3highfilter;
double emg3notchfilter;
double emg3abs;
double emg3lowfilter;
double max3;
double maxpart3;
// Right Lower Arm
double emg4;
double emg4highfilter;
double emg4notchfilter;
double emg4abs;
double emg4lowfilter;
double max4;
double maxpart4;
// Moving Average Floats
// Right Biceps Movavg
float RB0, RB1, RB2, RB3, RB4, RB5, RB6, RB7, RB8, RB9, RB10, RB11, RB12,
RB13, RB14, RB15, RB16, RB17, RB18, RB19, RB20, RB21, RB22, RB23, RB24,
RB25, RB26, RB27, RB28, RB29, RB30, RB31, RB32, RB33, RB34, RB35, RB36,
RB37, RB38, RB39, RB40, RB41, RB42, RB43, RB44, RB45, RB46, RB47, RB48,
RB49, RB50, RB51, RB52, RB53, RB54, RB55, RB56, RB57, RB58, RB59, RB60,
RB61, RB62, RB63, RB64, RB65, RB66, RB67, RB68, RB69, RB70, RB71, RB72,
RB73, RB74, RB75, RB76, RB77, RB78, RB79, RB80, RB81, RB82, RB83, RB84,
RB85, RB86, RB87, MOVAVG_RB;
// Left Biceps Movavg
float LB0, LB1, LB2, LB3, LB4, LB5, LB6, LB7, LB8, LB9, LB10, LB11, LB12,
LB13, LB14, LB15, LB16, LB17, LB18, LB19, LB20, LB21, LB22, LB23, LB24,
LB25, LB26, LB27, LB28, LB29, LB30, LB31, LB32, LB33, LB34, LB35, LB36,
LB37, LB38, LB39, LB40, LB41, LB42, LB43, LB44, LB45, LB46, LB47, LB48,
LB49, LB50, LB51, LB52, LB53, LB54, LB55, LB56, LB57, LB58, LB59, LB60,
LB61, LB62, LB63, LB64, LB65, LB66, LB67, LB68, LB69, LB70, LB71, LB72,
LB73, LB74, LB75, LB76, LB77, LB78, LB79, LB80, LB81, LB82, LB83, LB84,
LB85, LB86, LB87, MOVAVG_LB;
// Left Lower Arm Movavg
float LL0, LL1, LL2, LL3, LL4, LL5, LL6, LL7, LL8, LL9, LL10, LL11, LL12,
LL13, LL14, LL15, LL16, LL17, LL18, LL19, LL20, LL21, LL22, LL23, LL24,
LL25, LL26, LL27, LL28, LL29, LL30, LL31, LL32, LL33, LL34, LL35, LL36,
LL37, LL38, LL39, LL40, LL41, LL42, LL43, LL44, LL45, LL46, LL47, LL48,
LL49, LL50, LL51, LL52, LL53, LL54, LL55, LL56, LL57, LL58, LL59, LL60,
LL61, LL62, LL63, LL64, LL65, LL66, LL67, LL68, LL69, LL70, LL71, LL72,
LL73, LL74, LL75, LL76, LL77, LL78, LL79, LL80, LL81, LL82, LL83, LL84,
LL85, LL86, LL87, MOVAVG_LL;
// Right Lower Arm Movavg
float RL0, RL1, RL2, RL3, RL4, RL5, RL6, RL7, RL8, RL9, RL10, RL11, RL12,
RL13, RL14, RL15, RL16, RL17, RL18, RL19, RL20, RL21, RL22, RL23, RL24,
RL25, RL26, RL27, RL28, RL29, RL30, RL31, RL32, RL33, RL34, RL35, RL36,
RL37, RL38, RL39, RL40, RL41, RL42, RL43, RL44, RL45, RL46, RL47, RL48,
RL49, RL50, RL51, RL52, RL53, RL54, RL55, RL56, RL57, RL58, RL59, RL60,
RL61, RL62, RL63, RL64, RL65, RL66, RL67, RL68, RL69, RL70, RL71, RL72,
RL73, RL74, RL75, RL76, RL77, RL78, RL79, RL80, RL81, RL82, RL83, RL84,
RL85, RL86, RL87, MOVAVG_RL;
float AV = 0.02; //multiplication value for adding each movavg value
// This value also adds a very slight gain to every value
// RKI Variables & Constants
double setpoint1 = 2.35;
double setpoint2 = 0.785;
double x = 10.77;
double y = 15.73;
float pi = 3.14159265359;
float a = 22.25; //originally 22.25, makes for xinitial=10.766874 // length of arm 1
float b = 26.5; //originally 16.48 makes for yinitial=15.733 // length of arm 2
double alpha_old = 2.35; //INITIAL ANGLES IN RADIANS
double beta_old = 0.785;
float delta1;
float delta2;
float diffmotor_a;
float diffmotor_b;
// PID Variables & Constants
double Kp = 0.5;// you can set these constants however you like depending on trial & error
double Ki = 0.1;
double Kd = 0.1;
float last_error1 = 0;
float new_error1 = 0;
float change_error1 = 0;
float total_error1 = 0;
float pid_term1 = 0;
float pid_term_scaled1 = 0;
float last_error2 = 0;
float new_error2 = 0;
float change_error2 = 0;
float total_error2 = 0;
float pid_term2 = 0;
float pid_term_scaled2 = 0;
// Motor Variables & Constants
//float MV1 = 0;
//float MV2 = 0;
double count1 = 0;
double count2 = 0;
double angle1 = 2.35;
double angle2 = 0.785;
// BiQuad Filter Settings
// Right Biceps
BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); // Filter at 10 Hz
BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); // Filter at 50 Hz
BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); // Filter at 15 Hz
// Left Biceps
BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
// Left Lower Arm OR Triceps
BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
// Right Lower Arm OR Triceps
BiQuad filterhigh4(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
BiQuad filternotch4(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
//
// RKI Calculations
//
//
float inversekinematics1(float x, float y){
// cosine law:
float c = sqrtf(x*x + y*y); // here comes the relevant math, see my inverse kinematics explaination using cosine law
float alpha1 = acosf((a*a + c*c - b*b)/(2*a*c));
// remaining parts of alpha:
float alpha2 = atan2f(y,x);
float alpha = alpha1 + alpha2;
return alpha;
}
float inversekinematics2(float x, float y){
// cosine law:
float c = sqrtf(x*x + y*y); // here comes the relevant math, see my inverse kinematics explaination using cosine law
float beta = acosf((a*a + b*b - c*c)/(2*a*b));
return beta;
}
// Finding max values for correct motor switch if the button is pressed
//
//
// calibration of both biceps
void get_max1(){
if (max_reader12==0){
green = !green;
red = 1;
blue = 1;
for(int n=0;n<2000;n++){ // measure 2000 samples and filter it
emg1 = emg1_in.read(); // read out emg
emg1highfilter = filterhigh1.step(emg1); // high pass filtered
emg1notchfilter = filternotch1.step(emg1highfilter); // notch filtered
emg1abs = fabs(emg1notchfilter); // take the absolute value
emg1lowfilter = filterlow1.step(emg1abs); // low pass filtered
RB0 = emg1lowfilter;
// Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
+RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
*AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
*AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
+RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
+RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
+RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
+RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
+RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
+RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;
RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69;
RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63;
RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57;
RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51;
RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45;
RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39;
RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33;
RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27;
RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21;
RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15;
RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9;
RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2;
RB2 = RB1; RB1 = RB0;
emg2 = emg2_in.read(); // read out emg
emg2highfilter = filterhigh2.step(emg2); // high pass filtered
emg2notchfilter = filternotch2.step(emg2highfilter); // notch filtered
emg2abs = fabs(emg2notchfilter); // take the absolute value
emg2lowfilter = filterlow2.step(emg2abs); // low pass filtered
LB0 = emg2lowfilter;
// same story as with the right biceps
MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
+LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
*AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
*AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
+LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
+LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
+LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
+LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
+LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
+LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69;
LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63;
LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57;
LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51;
LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45;
LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39;
LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33;
LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27;
LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21;
LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15;
LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9;
LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2;
LB2 = LB1; LB1 = LB0;
if (max1<MOVAVG_RB){
max1 = MOVAVG_RB; // set the max value at the highest measured value
}
if (max2<MOVAVG_LB){
max2 = MOVAVG_LB; // set the max value at the highest measured value
}
wait(0.001f); // measure at 1000Hz
}
wait(0.2f);
green = 1;
}
maxpart1 = 0.15*max1; // set cut off voltage at 13% of max for right biceps
maxpart2 = 0.15*max2; // set cut off voltage at 13% of max for left biceps
}
// calibration of both lower arms
void get_max3(){
if (max_reader34==0){
green = 1;
blue = 1;
red = !red;
for(int n=0;n<2000;n++){
emg3 = emg3_in.read();
emg3highfilter = filterhigh3.step(emg3);
emg3notchfilter = filternotch3.step(emg3highfilter);
emg3abs = fabs(emg3notchfilter);
emg3lowfilter = filterlow3.step(emg3abs);
LL0 = emg3lowfilter;
// same story as with the right biceps
MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
+LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
*AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
*AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
+LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
+LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
+LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
+LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
+LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
+LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69;
LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63;
LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57;
LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51;
LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45;
LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39;
LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33;
LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27;
LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21;
LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15;
LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9;
LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2;
LL2 = LL1; LL1 = LL0;
emg4 = emg4_in.read();
emg4highfilter = filterhigh4.step(emg4);
emg4notchfilter = filternotch4.step(emg4highfilter);
emg4abs = fabs(emg4notchfilter);
emg4lowfilter = filterlow4.step(emg4abs);
RL0 = emg4lowfilter;
// same story as with the right biceps
MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
+RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
*AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
*AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
+RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
+RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
+RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
+RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
+RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
+RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69;
RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63;
RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57;
RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51;
RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45;
RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39;
RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33;
RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27;
RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21;
RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15;
RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9;
RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2;
RL2 = RL1; RL1 = RL0;
if (max3<MOVAVG_LL){
max3 = MOVAVG_LL; // set the max value at the highest measured value
}
if (max4<MOVAVG_RL){
max4 = MOVAVG_RL; // set the max value at the highest measured value
}
wait(0.001f);
}
wait(0.2f);
red = 1;
}
maxpart3 = 0.17*max3; // set cut off voltage at 15% of max for left lower arm
maxpart4 = 0.17*max4; // set cut off voltage at 15% of max for right lower arm
}
// EMG Filtering & Scope
//
//
void filter(/*double setpoint1, double setpoint2*/) {
// Right Biceps
emg1 = emg1_in.read();
emg1highfilter = filterhigh1.step(emg1);
emg1notchfilter = filternotch1.step(emg1highfilter);
emg1abs = fabs(emg1notchfilter);
emg1lowfilter = filterlow1.step(emg1abs); // Final Right Biceps values to be sent
RB0 = emg1lowfilter;
// Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
+RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
*AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
*AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
+RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
+RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
+RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
+RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
+RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
+RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;
RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69;
RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63;
RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57;
RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51;
RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45;
RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39;
RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33;
RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27;
RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21;
RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15;
RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9;
RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2;
RB2 = RB1; RB1 = RB0;
// Left Biceps
emg2 = emg2_in.read();
emg2highfilter = filterhigh2.step(emg2);
emg2notchfilter = filternotch2.step(emg2highfilter);
emg2abs = fabs(emg2notchfilter);
emg2lowfilter = filterlow2.step(emg2abs); // Final Left Biceps values to be sent
LB0 = emg2lowfilter;
// same story as with the right biceps
MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
+LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
*AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
*AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
+LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
+LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
+LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
+LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
+LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
+LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69;
LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63;
LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57;
LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51;
LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45;
LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39;
LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33;
LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27;
LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21;
LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15;
LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9;
LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2;
LB2 = LB1; LB1 = LB0;
// Left Lower Arm OR Triceps
emg3 = emg3_in.read();
emg3highfilter = filterhigh3.step(emg3);
emg3notchfilter = filternotch3.step(emg3highfilter);
emg3abs = fabs(emg3notchfilter);
emg3lowfilter = filterlow3.step(emg3abs); // Final Lower Arm values to be sent
LL0 = emg3lowfilter;
// same story as with the right biceps
MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
+LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
*AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
*AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
+LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
+LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
+LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
+LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
+LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
+LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69;
LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63;
LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57;
LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51;
LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45;
LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39;
LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33;
LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27;
LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21;
LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15;
LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9;
LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2;
LL2 = LL1; LL1 = LL0;
// Right Lower Arm OR Triceps
emg4 = emg4_in.read();
emg4highfilter = filterhigh4.step(emg4);
emg4notchfilter = filternotch4.step(emg4highfilter);
emg4abs = fabs(emg4notchfilter);
emg4lowfilter = filterlow4.step(emg4abs); // Final Lower Arm values to be sent
RL0 = emg4lowfilter;
// same story as with the right biceps
MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
+RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
*AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
*AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
+RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
+RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
+RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
+RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
+RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
+RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69;
RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63;
RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57;
RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51;
RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45;
RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39;
RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33;
RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27;
RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21;
RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15;
RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9;
RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2;
RL2 = RL1; RL1 = RL0;
// Compare measurement to the calibrated value to decide actions
// more options could be added if the callibration is well defined enough
// This part checks for right biceps contractions only
/* if (maxpart1<MOVAVG_RB || maxpart2<MOVAVG_LB || maxpart3<MOVAVG_LL || maxpart4<MOVAVG_RL){
wait(0.15f);*/
if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 1;
blue = 1;
green = 0;
//MV1 = 0.5;
//MV2 = 0;
x = x + 0.1;
if (x > 16) {
x = 15;
}
double alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
double beta = inversekinematics2(x,y);
setpoint1 = alpha;
setpoint2 = beta;
wait(0.1f);
}
// This part checks for left biceps contractions only
else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 0;
blue = 1;
green = 1;
//MV1 = -0.5;
//MV2 = 0;
x = x - 0.1;
if (x < 10.77){
x = 10.77;
}
double alpha = inversekinematics1(x,y);
double beta = inversekinematics2(x,y);
setpoint1 = alpha;
setpoint2 = beta;
wait(0.1f);
}
// This part checks for left lower arm contractions only
else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 1;
blue = 0;
green = 1;
//MV1 = 0;
//MV2 = 0.5;
y = y + 0.1;
if (y > 19.5) {
y = 19.5;
}
double alpha = inversekinematics1(x,y);
double beta = inversekinematics2(x,y);
setpoint1 = alpha;
setpoint2 = beta;
wait(0.1f);
}
// This part checks for right lower arm contractions only
else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
red = 0;
blue = 1;
green = 0;
//MV1 = 0;
//MV2 = -0.5;
y = y - 0.1;
if (y < 15.73) {
y = 15.73;
}
double alpha = inversekinematics1(x,y);
double beta = inversekinematics2(x,y);
setpoint1 = alpha;
setpoint2 = beta;
wait(0.1f);
}
/* // This part checks for both lower arm contractions only
else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
//MV1 = -0.5;
//MV2 = -0.5;
}
// This part checks for both biceps contractions only
else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
//MV1 = 0.5;
//MV2 = 0.5;
}
// This part checks for right lower arm & left biceps contractions only
else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
//MV1 = 0.5;
//MV2 = -0.5;
}
// This part checks for left lower arm & right biceps contractions only
else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
//MV1 = -0.5;
//MV2 = 0.5;
}*/
//}
else {
red = 1; // Shut down all led colors if no movement is registered
blue = 1;
green = 1;
//MV1 = 0;
//MV2 = 0;
}
// Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
scope.set(0, MOVAVG_RB ); // plot Right biceps voltage
scope.set(1, MOVAVG_LB ); // Plot Left biceps voltage
scope.set(2, MOVAVG_LL ); // Plot Lower Left Arm voltage
scope.set(3, MOVAVG_RL ); // Plot Lower Right Arm Voltage
scope.send(); // send everything to the HID scope
}
void encoders(){
count1 = Encoder1.getPulses();
count2 = Encoder2.getPulses();
}
/*
// PID calculations
//
//
void PIDcalculation1() {
//PID calculation for motor 1
filter();
count1 = Encoder1.getPulses();
angle1 += (0.000748 * count1);
new_error1 = setpoint1 - angle1;
change_error1 = new_error1 - last_error1;
total_error1 += new_error1;
pid_term1 = (Kp * new_error1) + (Ki * total_error1) + (Kd * change_error1);
if (pid_term1<-255) {
pid_term1 = -255;
}
if (pid_term1>255) {
pid_term1 = 255;
}
pid_term_scaled1 = abs(pid_term1);
last_error1 = new_error1;
}
void PIDcalculation2() {
//PID calculation for motor 2
filter();
count2 = Encoder2.getPulses();
angle2 += (0.000748 * count2);
new_error2 = setpoint2 - angle2;
change_error2 = new_error2 - last_error2;
total_error2 += new_error2;
pid_term2 = (Kp * new_error2) + (Ki * total_error2) + (Kd * change_error2);
if (pid_term2<-255) {
pid_term2 = -255;
}
if (pid_term2>255) {
pid_term2 = 255;
}
pid_term_scaled2 = abs(pid_term2);
last_error2 = new_error2;
}
*/
// Motor control
//
//
// check to see if motor1 needs to be activated
void SetMotor1() {
//PIDcalculation1();
//filter();
while (angle1<setpoint1 || angle1>setpoint1 || angle2<setpoint2 || angle2>setpoint2) {
encoders();
double count1;
double count2;
angle1 += (0.0981 * count1);
angle2 += (0.0981 * count2);
if (angle1<setpoint1 && angle2<setpoint2) {
motor1direction = 1; // counterclockwise rotation
motor2direction = 1;
}
else if (angle1>setpoint1 && angle2<setpoint2) {
motor1direction = 0; // clockwise rotation
motor2direction = 1;
}
else if (angle1<setpoint1 && angle2>setpoint2) {
motor1direction = 1;
motor2direction = 0;
}
else if (angle1>setpoint1 && angle2>setpoint2) {
motor1direction = 0;
motor2direction = 0;
}
if ((angle1<setpoint1 || angle1>setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
motor1pwm = 0.2;
motor2pwm = 0.2;
}
else if ((angle1<setpoint1 || angle1>setpoint1) && (angle2 == setpoint2)) {
motor1pwm = 0.2;
motor2pwm = 0;
}
else if ((angle1 == setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
motor1pwm = 0;
motor2pwm = 0.2;
}
else if ((angle1 == setpoint1) && (angle2 == setpoint2)){
motor1pwm = 0;
motor2pwm = 0;
}
}
}
/*
// check if motor1 needs to be activated
void SetMotor2() {
filter();
//PIDcalculation2();
while (angle2<setpoint2 || angle2>setpoint2) {
count2 = Encoder2.getPulses();
angle2 += (0.0981 * count2);
if (angle2<setpoint2){
motor1direction = 0; // counterclockwise rotation
}
else if (angle2>setpoint2){
motor1direction = 1; // clockwise rotation
}
if (angle2<setpoint2 || angle2>setpoint2) {
motor2pwm = 0.5;
}
else if (angle2 == setpoint2){
motor2pwm = 0;
}
}
}
void MeasureAndControl(void) {
SetMotor1();
//SetMotor2();
}
*/
int main(){
pc.baud(115200);
main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
max_read3.attach(&get_max3, 2);
tencoder.attach(&encoders, 0.001);
Motorcontrol.attach(&SetMotor1,0.1);
//PIDtimer.attach(&PIDcalculation1, 0.005);
//PIDtimer.attach(&PIDcalculation2, 0.005);
while(1) {}
}