main.cpp

00001 #include "mbed.h"
00002 //#include "HIDScope.h"
00003 #include "MODSERIAL.h"
00004 #include "BiQuad.h"
00005 #include "QEI.h"
00006 
00007 //Define Objects
00008     AnalogIn    emg1_in( A0 ); // Read Out The Signal
00009     AnalogIn    emg2_in( A1 );
00010     AnalogIn    emg3_in( A2 );
00011     AnalogIn    emg4_in( A3 );
00012     DigitalIn   max_reader12( SW2 ); // Define Button Press
00013     DigitalIn   max_reader34( SW3 );
00014     
00015     DigitalOut motor1direction( D4 );
00016     PwmOut motor1pwm( D5);
00017     PwmOut motor2pwm( D6 );
00018     DigitalOut motor2direction( D7 );
00019     QEI Encoder1(D10, D11, NC, 64); // Encoder
00020     QEI Encoder2(D12, D13, NC, 64); 
00021 
00022     Ticker      main_timer;
00023     Ticker      max_read1;
00024     Ticker      max_read3;
00025     Ticker      Motorcontrol;
00026     Ticker      tencoder;
00027     //HIDScope    scope( 4 );
00028     DigitalOut  red(LED_RED);
00029     DigitalOut  blue(LED_BLUE);
00030     DigitalOut  green(LED_GREEN);
00031     Serial      pc(USBTX, USBRX);
00032 
00033 
00034 double original_angle1 =  2.35;
00035 double original_angle2 = 0.785;
00036 
00037 // EMG Variables & Constants
00038     //Right Biceps
00039     double emg1;
00040     double emg1highfilter;
00041     double emg1notchfilter;
00042     double emg1abs;
00043     double emg1lowfilter;
00044     double max1;
00045     double maxpart1;
00046     // Left Biceps
00047     double emg2;
00048     double emg2highfilter;
00049     double emg2notchfilter;
00050     double emg2abs;
00051     double emg2lowfilter;
00052     double max2;
00053     double maxpart2;
00054     // Left Lower Arm
00055     double emg3;
00056     double emg3highfilter;
00057     double emg3notchfilter;
00058     double emg3abs;
00059     double emg3lowfilter;
00060     double max3;
00061     double maxpart3;
00062     // Right Lower Arm
00063     double emg4;
00064     double emg4highfilter;
00065     double emg4notchfilter;
00066     double emg4abs;
00067     double emg4lowfilter;
00068     double max4;
00069     double maxpart4;
00070     // Moving Average Floats
00071     // Right Biceps Movavg
00072     float RB0, RB1, RB2, RB3, RB4, RB5, RB6, RB7, RB8, RB9, RB10, RB11, RB12, 
00073      RB13, RB14, RB15, RB16, RB17, RB18, RB19, RB20, RB21, RB22, RB23, RB24, 
00074      RB25, RB26, RB27, RB28, RB29, RB30, RB31, RB32, RB33, RB34, RB35, RB36,
00075      RB37, RB38, RB39, RB40, RB41, RB42, RB43, RB44, RB45, RB46, RB47, RB48,
00076      RB49, RB50, RB51, RB52, RB53, RB54, RB55, RB56, RB57, RB58, RB59, RB60,
00077      RB61, RB62, RB63, RB64, RB65, RB66, RB67, RB68, RB69, RB70, RB71, RB72,
00078      RB73, RB74, RB75, RB76, RB77, RB78, RB79, RB80, RB81, RB82, RB83, RB84,
00079      RB85, RB86, RB87, MOVAVG_RB;
00080     // Left Biceps Movavg
00081     float LB0, LB1, LB2, LB3, LB4, LB5, LB6, LB7, LB8, LB9, LB10, LB11, LB12, 
00082      LB13, LB14, LB15, LB16, LB17, LB18, LB19, LB20, LB21, LB22, LB23, LB24, 
00083      LB25, LB26, LB27, LB28, LB29, LB30, LB31, LB32, LB33, LB34, LB35, LB36,
00084      LB37, LB38, LB39, LB40, LB41, LB42, LB43, LB44, LB45, LB46, LB47, LB48,
00085      LB49, LB50, LB51, LB52, LB53, LB54, LB55, LB56, LB57, LB58, LB59, LB60,
00086      LB61, LB62, LB63, LB64, LB65, LB66, LB67, LB68, LB69, LB70, LB71, LB72,
00087      LB73, LB74, LB75, LB76, LB77, LB78, LB79, LB80, LB81, LB82, LB83, LB84,
00088      LB85, LB86, LB87, MOVAVG_LB;
00089     // Left Lower Arm Movavg 
00090     float LL0, LL1, LL2, LL3, LL4, LL5, LL6, LL7, LL8, LL9, LL10, LL11, LL12, 
00091      LL13, LL14, LL15, LL16, LL17, LL18, LL19, LL20, LL21, LL22, LL23, LL24, 
00092      LL25, LL26, LL27, LL28, LL29, LL30, LL31, LL32, LL33, LL34, LL35, LL36,
00093      LL37, LL38, LL39, LL40, LL41, LL42, LL43, LL44, LL45, LL46, LL47, LL48,
00094      LL49, LL50, LL51, LL52, LL53, LL54, LL55, LL56, LL57, LL58, LL59, LL60,
00095      LL61, LL62, LL63, LL64, LL65, LL66, LL67, LL68, LL69, LL70, LL71, LL72,
00096      LL73, LL74, LL75, LL76, LL77, LL78, LL79, LL80, LL81, LL82, LL83, LL84,
00097      LL85, LL86, LL87, MOVAVG_LL;    
00098     // Right Lower Arm Movavg
00099     float RL0, RL1, RL2, RL3, RL4, RL5, RL6, RL7, RL8, RL9, RL10, RL11, RL12, 
00100      RL13, RL14, RL15, RL16, RL17, RL18, RL19, RL20, RL21, RL22, RL23, RL24, 
00101      RL25, RL26, RL27, RL28, RL29, RL30, RL31, RL32, RL33, RL34, RL35, RL36,
00102      RL37, RL38, RL39, RL40, RL41, RL42, RL43, RL44, RL45, RL46, RL47, RL48,
00103      RL49, RL50, RL51, RL52, RL53, RL54, RL55, RL56, RL57, RL58, RL59, RL60,
00104      RL61, RL62, RL63, RL64, RL65, RL66, RL67, RL68, RL69, RL70, RL71, RL72,
00105      RL73, RL74, RL75, RL76, RL77, RL78, RL79, RL80, RL81, RL82, RL83, RL84,
00106      RL85, RL86, RL87, MOVAVG_RL;    
00107     float AV = 0.02; //multiplication value for adding each movavg value
00108     // This value also adds a very slight gain to every value
00109 
00110 // RKI Variables & Constants
00111     double setpoint1 = 2.35;
00112     double setpoint2 = 0.785;
00113     double x = 10.77;
00114     double y = 15.73;
00115     float pi = 3.14159265359;
00116     float a = 22.25;             //originally 22.25, makes for xinitial=10.766874   // length of arm 1
00117     float b = 26.5;             //originally 16.48   makes for yinitial=15.733      // length of arm 2
00118     double alpha_old = 2.35;     //INITIAL ANGLES IN RADIANS
00119     double beta_old = 0.785;
00120     float delta1;
00121     float delta2;
00122     float diffmotor_a;
00123     float diffmotor_b;
00124 
00125 // PID Variables & Constants
00126     double Kp = 0.5;// you can set these constants however you like depending on trial & error
00127     double Ki = 0.1;
00128     double Kd = 0.1;
00129 
00130     float last_error1 = 0;
00131     float new_error1 = 0;
00132     float change_error1 = 0;
00133     float total_error1 = 0;
00134     float pid_term1 = 0;
00135     float pid_term_scaled1 = 0;
00136     
00137     float last_error2 = 0;
00138     float new_error2 = 0;
00139     float change_error2 = 0;
00140     float total_error2 = 0;
00141     float pid_term2 = 0;
00142     float pid_term_scaled2 = 0;
00143     
00144 // Motor Variables & Constants
00145     //float MV1 = 0;
00146     //float MV2 = 0;
00147     double count1 = 0;
00148     double count2 = 0;
00149     double angle1;
00150     double angle2;
00151    
00152 // BiQuad Filter Settings
00153     // Right Biceps
00154     BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); // Filter at 10 Hz
00155     BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); // Filter at 50 Hz
00156     BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); // Filter at 15 Hz
00157     // Left Biceps
00158     BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
00159     BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
00160     BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
00161     // Left Lower Arm OR Triceps
00162     BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
00163     BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
00164     BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
00165     // Right Lower Arm OR Triceps
00166     BiQuad filterhigh4(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
00167     BiQuad filternotch4(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
00168     BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
00169     //
00170 
00171 // RKI Calculations
00172 //
00173 //
00174 
00175 float inversekinematics1(float x, float y){
00176     // cosine law:
00177     float c = sqrtf(x*x + y*y);                             // here comes the relevant math, see my inverse kinematics explaination using cosine law
00178     float alpha1 = acosf((a*a + c*c - b*b)/(2*a*c));
00179     
00180     // remaining parts of alpha:
00181     float alpha2 = atan2f(y,x);
00182     float alpha = alpha1 + alpha2;
00183     return alpha;
00184 }
00185 
00186 float inversekinematics2(float x, float y){
00187     // cosine law:
00188     float c = sqrtf(x*x + y*y);                             // here comes the relevant math, see my inverse kinematics explaination using cosine law
00189     float beta = acosf((a*a + b*b - c*c)/(2*a*b));
00190     return beta;
00191 }
00192 
00193 // Finding max values for correct motor switch if the button is pressed
00194 //
00195 //
00196 // calibration of both biceps
00197 void get_max1(){
00198     if (max_reader12==0){
00199             green = !green;
00200             red = 1;
00201             blue = 1;
00202             for(int n=0;n<2000;n++){ // measure 2000 samples and filter it
00203 
00204             emg1 = emg1_in.read();      // read out emg
00205             emg1highfilter = filterhigh1.step(emg1); // high pass filtered
00206             emg1notchfilter = filternotch1.step(emg1highfilter); // notch filtered
00207             emg1abs = fabs(emg1notchfilter); // take the absolute value
00208             emg1lowfilter = filterlow1.step(emg1abs); // low pass filtered
00209             RB0 = emg1lowfilter;
00210             // Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
00211             MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
00212             +RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
00213             *AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
00214             *AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
00215             +RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
00216             +RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
00217             +RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
00218             +RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
00219             +RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
00220             +RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
00221             RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
00222             RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;            
00223             RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69; 
00224             RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63; 
00225             RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57; 
00226             RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51; 
00227             RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45; 
00228             RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39; 
00229             RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33; 
00230             RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27; 
00231             RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21; 
00232             RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15; 
00233             RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9; 
00234             RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2; 
00235             RB2 = RB1; RB1 = RB0;
00236             
00237             emg2 = emg2_in.read();      // read out emg
00238             emg2highfilter = filterhigh2.step(emg2); // high pass filtered
00239             emg2notchfilter = filternotch2.step(emg2highfilter); // notch filtered
00240             emg2abs = fabs(emg2notchfilter); // take the absolute value
00241             emg2lowfilter = filterlow2.step(emg2abs); // low pass filtered
00242             LB0 = emg2lowfilter;
00243             // same story as with the right biceps
00244             MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
00245             +LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
00246              *AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
00247             *AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
00248             +LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
00249             +LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
00250             +LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
00251             +LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
00252             +LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
00253             +LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
00254             LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
00255             LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
00256             LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69; 
00257             LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63; 
00258             LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57; 
00259             LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51; 
00260             LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45; 
00261             LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39; 
00262             LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33; 
00263             LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27; 
00264             LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21; 
00265             LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15; 
00266             LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9; 
00267             LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2; 
00268             LB2 = LB1; LB1 = LB0;
00269 
00270             if (max1<MOVAVG_RB){
00271                 max1 = MOVAVG_RB; // set the max value at the highest measured value
00272             }
00273             if (max2<MOVAVG_LB){
00274                 max2 = MOVAVG_LB; // set the max value at the highest measured value                
00275             }
00276             wait(0.001f); // measure at 1000Hz   
00277             }
00278             wait(0.2f);
00279             green = 1;
00280     }
00281     maxpart1 = 0.15*max1; // set cut off voltage at 13% of max for right biceps
00282     maxpart2 = 0.15*max2; // set cut off voltage at 13% of max for left biceps
00283 }
00284 
00285 // calibration of both lower arms
00286 void get_max3(){
00287     if (max_reader34==0){
00288             green = 1;
00289             blue = 1;
00290             red = !red;
00291             for(int n=0;n<2000;n++){
00292                         
00293             emg3 = emg3_in.read();
00294             emg3highfilter = filterhigh3.step(emg3);
00295             emg3notchfilter = filternotch3.step(emg3highfilter);
00296             emg3abs = fabs(emg3notchfilter);
00297             emg3lowfilter = filterlow3.step(emg3abs);
00298             LL0 = emg3lowfilter;
00299             // same story as with the right biceps
00300             MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
00301             +LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
00302             *AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
00303             *AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
00304             +LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
00305             +LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
00306             +LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
00307             +LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
00308             +LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
00309             +LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
00310             LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
00311             LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
00312             LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69; 
00313             LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63; 
00314             LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57; 
00315             LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51; 
00316             LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45; 
00317             LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39; 
00318             LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33; 
00319             LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27; 
00320             LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21; 
00321             LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15; 
00322             LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9; 
00323             LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2; 
00324             LL2 = LL1; LL1 = LL0;
00325             
00326             emg4 = emg4_in.read();
00327             emg4highfilter = filterhigh4.step(emg4);
00328             emg4notchfilter = filternotch4.step(emg4highfilter);
00329             emg4abs = fabs(emg4notchfilter);
00330             emg4lowfilter = filterlow4.step(emg4abs);
00331             RL0 = emg4lowfilter;
00332             // same story as with the right biceps
00333             MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
00334             +RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
00335             *AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
00336             *AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
00337             +RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
00338             +RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
00339             +RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
00340             +RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
00341             +RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
00342             +RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
00343             RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
00344             RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
00345             RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69; 
00346             RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63; 
00347             RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57; 
00348             RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51; 
00349             RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45; 
00350             RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39; 
00351             RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33; 
00352             RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27; 
00353             RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21; 
00354             RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15; 
00355             RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9; 
00356             RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2; 
00357             RL2 = RL1; RL1 = RL0; 
00358             
00359             if (max3<MOVAVG_LL){
00360                 max3 = MOVAVG_LL; // set the max value at the highest measured value
00361             }
00362             if (max4<MOVAVG_RL){
00363                 max4 = MOVAVG_RL; // set the max value at the highest measured value
00364             }
00365             wait(0.001f);    
00366             }
00367             wait(0.2f);
00368             red = 1;
00369     }
00370     maxpart3 = 0.17*max3; // set cut off voltage at 15% of max for left lower arm
00371     maxpart4 = 0.17*max4; // set cut off voltage at 15% of max for right lower arm
00372 }
00373 
00374 // EMG Filtering & Scope
00375 //
00376 //
00377 void filter(/*double setpoint1, double setpoint2*/) {
00378     // Right Biceps
00379     emg1 = emg1_in.read();
00380     emg1highfilter = filterhigh1.step(emg1);
00381     emg1notchfilter = filternotch1.step(emg1highfilter);
00382     emg1abs = fabs(emg1notchfilter);
00383     emg1lowfilter = filterlow1.step(emg1abs); // Final Right Biceps values to be sent
00384     RB0 = emg1lowfilter;
00385     // Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
00386     MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
00387     +RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
00388     *AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
00389     *AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
00390     +RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
00391     +RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
00392     +RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
00393     +RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
00394     +RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
00395     +RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
00396     RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
00397     RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;
00398     RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69; 
00399     RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63; 
00400     RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57; 
00401     RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51; 
00402     RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45; 
00403     RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39; 
00404     RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33; 
00405     RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27; 
00406     RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21; 
00407     RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15; 
00408     RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9; 
00409     RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2; 
00410     RB2 = RB1; RB1 = RB0;
00411     // Left Biceps
00412     emg2 = emg2_in.read();
00413     emg2highfilter = filterhigh2.step(emg2);
00414     emg2notchfilter = filternotch2.step(emg2highfilter);
00415     emg2abs = fabs(emg2notchfilter);
00416     emg2lowfilter = filterlow2.step(emg2abs); // Final Left Biceps values to be sent
00417     LB0 = emg2lowfilter;
00418     // same story as with the right biceps
00419     MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
00420     +LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
00421     *AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
00422     *AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
00423     +LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
00424     +LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
00425     +LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
00426     +LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
00427     +LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
00428     +LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
00429     LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
00430     LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
00431     LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69; 
00432     LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63; 
00433     LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57; 
00434     LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51; 
00435     LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45; 
00436     LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39; 
00437     LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33; 
00438     LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27; 
00439     LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21; 
00440     LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15; 
00441     LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9; 
00442     LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2; 
00443     LB2 = LB1; LB1 = LB0;
00444     // Left Lower Arm OR Triceps
00445     emg3 = emg3_in.read();
00446     emg3highfilter = filterhigh3.step(emg3);
00447     emg3notchfilter = filternotch3.step(emg3highfilter);
00448     emg3abs = fabs(emg3notchfilter);
00449     emg3lowfilter = filterlow3.step(emg3abs); // Final Lower Arm values to be sent
00450     LL0 = emg3lowfilter;
00451     // same story as with the right biceps
00452     MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
00453     +LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
00454     *AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
00455     *AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
00456     +LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
00457     +LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
00458     +LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
00459     +LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
00460     +LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
00461     +LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
00462     LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
00463     LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
00464     LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69; 
00465     LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63; 
00466     LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57; 
00467     LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51; 
00468     LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45; 
00469     LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39; 
00470     LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33; 
00471     LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27; 
00472     LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21; 
00473     LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15; 
00474     LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9; 
00475     LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2; 
00476     LL2 = LL1; LL1 = LL0;
00477     // Right Lower Arm OR Triceps
00478     emg4 = emg4_in.read();
00479     emg4highfilter = filterhigh4.step(emg4);
00480     emg4notchfilter = filternotch4.step(emg4highfilter);
00481     emg4abs = fabs(emg4notchfilter);
00482     emg4lowfilter = filterlow4.step(emg4abs); // Final Lower Arm values to be sent
00483     RL0 = emg4lowfilter;
00484     // same story as with the right biceps
00485     MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
00486     +RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
00487     *AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
00488     *AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
00489     +RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
00490     +RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
00491     +RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
00492     +RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
00493     +RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
00494     +RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
00495     RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
00496     RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
00497     RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69; 
00498     RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63; 
00499     RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57; 
00500     RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51; 
00501     RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45; 
00502     RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39; 
00503     RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33; 
00504     RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27; 
00505     RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21; 
00506     RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15; 
00507     RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9; 
00508     RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2; 
00509     RL2 = RL1; RL1 = RL0; 
00510 
00511     
00512  // Compare measurement to the calibrated value to decide actions
00513  // more options could be added if the callibration is well defined enough
00514     // This part checks for right biceps contractions only
00515 /*    if (maxpart1<MOVAVG_RB || maxpart2<MOVAVG_LB || maxpart3<MOVAVG_LL || maxpart4<MOVAVG_RL){
00516     wait(0.15f);*/
00517         if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
00518             red = 1;
00519             blue = 1;
00520             green = 0;
00521             //MV1 = 0.5;
00522             //MV2 = 0;
00523             x = x + 0.1;
00524             if (x > 16) {
00525             x = 16;
00526             }
00527             double alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
00528             double beta = inversekinematics2(x,y);
00529             setpoint1 = alpha;
00530             setpoint2 = beta;
00531             wait(0.1f);
00532             
00533         }            
00534         // This part checks for left biceps contractions only  
00535         else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
00536             red = 0;
00537             blue = 1;
00538             green = 1;
00539             //MV1 = -0.5;
00540             //MV2 = 0;
00541             x = x - 0.1;
00542             if (x < 10.77){
00543             x = 10.87;
00544             }
00545             double alpha = inversekinematics1(x,y);
00546             double beta = inversekinematics2(x,y);
00547             setpoint1 = alpha;
00548             setpoint2 = beta;
00549             wait(0.1f);
00550         }
00551         // This part checks for left lower arm contractions only  
00552         else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
00553             red = 1;
00554             blue = 0;
00555             green = 1;
00556             //MV1 = 0;
00557             //MV2 = 0.5;
00558             y = y + 0.1;
00559             if (y > 19.5) {
00560             y = 19.5;
00561             }
00562             double alpha = inversekinematics1(x,y);
00563             double beta = inversekinematics2(x,y);
00564             setpoint1 = alpha;
00565             setpoint2 = beta;
00566             wait(0.1f);
00567         }
00568         // This part checks for right lower arm contractions only
00569         else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
00570             red = 0;
00571             blue = 1;
00572             green = 0;
00573             //MV1 = 0;
00574             //MV2 = -0.5;
00575             y = y - 0.1;
00576             if (y < 15.73) {
00577             y = 15.83;
00578             }
00579             double alpha = inversekinematics1(x,y);
00580             double beta = inversekinematics2(x,y);
00581             setpoint1 = alpha;
00582             setpoint2 = beta;
00583             wait(0.1f);
00584         }           
00585 /*        // This part checks for both lower arm contractions only 
00586         else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
00587             red = 0;
00588             blue = 0;
00589             green = 0;
00590             //MV1 = -0.5;
00591             //MV2 = -0.5;
00592         }
00593         // This part checks for both biceps contractions only 
00594         else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
00595             red = 0;
00596             blue = 0;
00597             green = 0;
00598             //MV1 = 0.5;
00599             //MV2 = 0.5;
00600         }
00601         // This part checks for right lower arm & left biceps contractions only
00602         else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
00603             red = 0;
00604             blue = 0;
00605             green = 0;
00606             //MV1 = 0.5;
00607             //MV2 = -0.5;
00608         }  
00609         // This part checks for left lower arm & right biceps contractions only
00610         else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
00611             red = 0;
00612             blue = 0;
00613             green = 0;
00614             //MV1 = -0.5;
00615             //MV2 = 0.5;
00616         }*/
00617     //}                                 
00618         else {
00619             red = 1; // Shut down all led colors if no movement is registered
00620             blue = 1;
00621             green = 1;
00622             //MV1 = 0;
00623             //MV2 = 0;
00624         }
00625     
00626     // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
00627     //scope.set(0, MOVAVG_RB ); // plot Right biceps voltage 
00628     //scope.set(1, MOVAVG_LB ); // Plot Left biceps voltage 
00629     //scope.set(2, MOVAVG_LL ); // Plot Lower Left Arm voltage 
00630     //scope.set(3, MOVAVG_RL ); // Plot Lower Right Arm Voltage 
00631     //scope.send(); // send everything to the HID scope 
00632     
00633 }
00634 
00635 void encoders(){
00636         count1 = Encoder1.getPulses();
00637         count2 = Encoder2.getPulses();
00638 }
00639 
00640 /*
00641 
00642 // PID calculations
00643 //
00644 //
00645 void PIDcalculation1() {
00646 //PID calculation for motor 1
00647     filter();
00648     count1 = Encoder1.getPulses();
00649     angle1 += (0.000748 * count1);
00650     new_error1 = setpoint1 - angle1;
00651     
00652     change_error1 = new_error1 - last_error1;
00653     total_error1 += new_error1;
00654     pid_term1 = (Kp * new_error1) + (Ki * total_error1) + (Kd * change_error1);
00655     if (pid_term1<-255) {
00656     pid_term1 = -255;
00657     }
00658     if (pid_term1>255) {
00659     pid_term1 = 255;
00660     }
00661     pid_term_scaled1 = abs(pid_term1);
00662     
00663     last_error1 = new_error1;
00664 }
00665 void PIDcalculation2() {    
00666 //PID calculation for motor 2
00667     filter();
00668     count2 = Encoder2.getPulses();
00669     angle2 += (0.000748 * count2);
00670     new_error2 = setpoint2 - angle2;
00671     
00672     change_error2 = new_error2 - last_error2;
00673     total_error2 += new_error2;
00674     pid_term2 = (Kp * new_error2) + (Ki * total_error2) + (Kd * change_error2);
00675     if (pid_term2<-255) {
00676     pid_term2 = -255;
00677     }
00678     if (pid_term2>255) {
00679     pid_term2 = 255;
00680     }
00681     pid_term_scaled2 = abs(pid_term2);
00682     
00683     last_error2 = new_error2;
00684 }
00685  */ 
00686 // Motor control
00687 //
00688 //
00689 // check to see if motor1 needs to be activated
00690 void SetMotor1() {
00691     //PIDcalculation1();
00692     //filter();
00693     pc.printf("\n\rBegin SetMotor1\r");
00694     while (angle1<setpoint1 || angle1>setpoint1 || angle2<setpoint2 || angle2>setpoint2) {
00695         pc.printf("\n\rlaten we dit dan 0 noemen\r");
00696         encoders();
00697         double count1;
00698         double count2;
00699         pc.printf("\n\rcount1=%i count2=%i\r",count1,count2);
00700         angle1 = original_angle1 + (0.000747998251 * count1);
00701         angle2 = original_angle2 + (0.000747998251 * count2);
00702         if (angle1<setpoint1 && angle2<setpoint2) {
00703         pc.printf("\n\rangle1<setpoint1 && angle2<setpoint2\r");
00704         motor1direction = 1; // counterclockwise rotation
00705         motor2direction = 1;
00706         }
00707         else if (angle1>setpoint1 && angle2<setpoint2) {
00708         pc.printf("\n\rangle1>setpoint1 && angle2<setpoint2\r");
00709         motor1direction = 0; // clockwise rotation
00710         motor2direction = 1;
00711         }
00712         else if (angle1<setpoint1 && angle2>setpoint2) {
00713         pc.printf("\n\rangle1<setpoint1 && angle2>setpoint2\r");
00714         motor1direction = 1;
00715         motor2direction = 0;
00716         }
00717         else if (angle1>setpoint1 && angle2>setpoint2) {
00718         pc.printf("\n\rangle1>setpoint1 && angle2>setpoint2\r");
00719         motor1direction = 0;
00720         motor2direction = 0;
00721         }
00722         if ((angle1<setpoint1 || angle1>setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {     //1
00723         pc.printf("\n\rdit noem ik maar 1\r");
00724         motor1pwm = 0.2;
00725         motor2pwm = 0.2;
00726         pc.printf("\n\rangle1=%d angle2=%d setpoint1=%d setpoint2=%d\r",angle1,angle2,setpoint1,setpoint2);
00727         }
00728         else if ((angle1<setpoint1 || angle1>setpoint1) && (angle2 == setpoint2)) {
00729         pc.printf("\n\rdit noem ik maar 2\r");
00730         motor1pwm = 0.2;
00731         motor2pwm = 0;
00732         }
00733         else if ((angle1 == setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
00734         pc.printf("\n\rdit noem ik maar 3\r");
00735         motor1pwm = 0;
00736         motor2pwm = 0.2;
00737         }
00738         else if ((angle1 == setpoint1) && (angle2 == setpoint2)){
00739         pc.printf("\n\rdit noem ik maar 4\r");
00740         motor1pwm = 0;
00741         motor2pwm = 0;
00742         }
00743     pc.printf("\n\rend SetMotor1\r");
00744     }
00745 }
00746 /*
00747 // check if motor1 needs to be activated
00748 void SetMotor2() {
00749     filter();
00750     //PIDcalculation2();
00751     while (angle2<setpoint2 || angle2>setpoint2) {
00752         count2 = Encoder2.getPulses();
00753         angle2 += (0.0981 * count2);
00754         if (angle2<setpoint2){
00755         motor1direction = 0; // counterclockwise rotation
00756         }
00757         else  if (angle2>setpoint2){
00758         motor1direction = 1; // clockwise rotation
00759         }
00760         if (angle2<setpoint2 || angle2>setpoint2) {
00761         motor2pwm = 0.5;
00762         }
00763         else if (angle2 == setpoint2){
00764         motor2pwm = 0;
00765         }
00766     }
00767 }
00768 
00769 void MeasureAndControl(void) {
00770     SetMotor1();
00771     //SetMotor2();
00772 }
00773 */
00774     
00775 int main(){   
00776  // pc.baud(115200);
00777     pc.printf("\n\rMAIN START\r");
00778     main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
00779     max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
00780     max_read3.attach(&get_max3, 2);
00781     tencoder.attach(&encoders, 0.001);
00782     Motorcontrol.attach(&SetMotor1,0.1);
00783     //PIDtimer.attach(&PIDcalculation1, 0.005);
00784     //PIDtimer.attach(&PIDcalculation2, 0.005);
00785     pc.printf("\n\rMAIN END!!!");
00786     while(1) {}
00787 }