Diff: main.cpp

Revision:

36:a700f64ba747

Parent:

35:a21c1cab8086

Child:

37:4d75c2432d71

diff -r a21c1cab8086 -r a700f64ba747 main.cpp
--- a/main.cpp	Tue Nov 01 15:47:40 2016 +0000
+++ b/main.cpp	Wed Nov 02 13:51:25 2016 +0000
@@ -37,15 +37,17 @@
 PwmOut motor2MagnitudePin(D5);
 DigitalIn button1(D3);
 DigitalIn button2(D9);
+AnalogIn    emg0( A0 );
+AnalogIn    emg1( A1 );
 
-DigitalOut ledgrn(LED_GREEN);
-DigitalOut ledred(LED_RED);
-DigitalOut ledblue(LED_BLUE);
+DigitalOut ledGrn(LED_GREEN);
+DigitalOut ledRed(LED_RED);
+DigitalOut ledBlue(LED_BLUE);
 
 //library settings
 Serial pc(USBTX,USBRX);
-Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
-//HIDScope    scope(6);
+Ticker filter_timer, MeasureTicker; //, BiQuadTicker; //, TimeTracker; // sampleT;
+HIDScope    scope(6);
 
 //initial values
 float dx;
@@ -98,15 +100,20 @@
 int counts2 = 0;
 int counts1Prev = 0;
 int counts2Prev = 0;
-
+double envelopeL = 0;
+double envelopeR = 0;
 
 //set constant or variable values (VALUES HAVE TO BE EDITED)
+int T=0;        //EMG 'switch' variable
+double threshold_l=0.025;   //left arm EMG threshold
+double threshold_r = 0.025; //right arm EMG threshold
 float EMGgain = 1.0;
 float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
+float x_movement = 0.01;    //movement in x direction when applying EMG
 float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
 float t_sample = 0.1; //seconds
 const float maxStampDistance = 2.0;
-float Kp = 3; //potMeter2.read();
+float Kp = 3.0;//potMeter2.read();
 float Ki = 0.2*Kp;  //0.01*Kp; //potMeter2.read();
 float Kd = 0.05;     //0.1;(unstable!) //0.05; //0.02;        //0.04*Kp; //potMeter2.read();
 float N = 100; //N=1/Tf, Higher N is faster derivative action but more sensitive to noise.
@@ -127,12 +134,33 @@
 BiQuad bq2(1.0000,   -1.5704,    1.2756,   -0.4844,    0.0762);
 BiQuad pidf;        //for lowpass filtering of PID controller
 
+BiQuadChain bcq1R;
+BiQuadChain bcq2R;
+// Notch filter wo=50; bw=wo/35
+BiQuad bq1R(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
+// High pass Butterworth filter 2nd order, Fc=10;
+BiQuad bq2R(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
+// Low pass Butterworth filter 2nd order, Fc = 8;
+BiQuad bq3R(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);
+
+BiQuadChain bcq1L;
+BiQuadChain bcq2L;
+// Notch filter wo=50; bw=wo/35
+BiQuad bq1L(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
+// High pass Butterworth filter 2nd order, Fc=10;
+BiQuad bq2L(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
+// Low pass Butterworth filter 2nd order, Fc = 8;
+BiQuad bq3L(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);
+
+// In the following: R is used for right arm, L is used for left arm!
+
 //set go-Ticker settings
-volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
-void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
-void BiQuadTicker_act(){BiQuadTicker_go=true;};
-void FeedbackTicker_act(){FeedbackTicker_go=true;};
-void TimeTracker_act(){TimeTracker_go=true;};
+volatile bool filter_timer_go=false, MeasureTicker_go=false; //BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
+void filter_timer_act(){filter_timer_go=true;}; // Activates go-flags
+void MeasureTicker_act(){MeasureTicker_go=true;}; 
+//void BiQuadTicker_act(){BiQuadTicker_go=true;};
+//void FeedbackTicker_act(){FeedbackTicker_go=true;};
+//void TimeTracker_act(){TimeTracker_go=true;};
 //void sampleT_act(){sampleT_go=true;};
 
 //define encoder counts and degrees
@@ -143,8 +171,68 @@
 const int inverse_gear_ratio = 131;
 const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio);  //87567.0496892 counts per radian, encoder axis
 
-void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut){
+void FilteredSample(int &Tout, double &envelopeLout, double &envelopeRout)
+{   
+    double inLout = emg0.read();
+    double inRout = emg1.read();
+    
+    double outRfilter1 = bcq1R.step(inRout);
+    double outRrect= fabs(outRfilter1);
+    envelopeRout = bcq2R.step(outRrect);
+    
+    double outLfilter1 = bcq1L.step(inLout);
+    double outLrect = fabs(outLfilter1);
+    envelopeLout = bcq2L.step(outLrect);
     
+    double biceps_l = (double) envelopeLout * EMGgain; //emg0.read();     //velocity or reference position change, EMG with a gain
+    double biceps_r = (double) envelopeRout * EMGgain; //emg1.read();
+    if (biceps_l > threshold_l && biceps_r > threshold_r){
+        //both arms activated: stamp moves down
+        //pc.printf("Stamp down   ");
+        //pc.printf("right: %f    ",biceps_r);
+        //pc.printf("left:  %f\n\r",biceps_l);
+        //wait(0.5);
+        Tout = -2;
+        //pc.printf("T=%d\n\r",T);
+        ledRed=!ledRed;//blink purple
+        ledBlue=!ledBlue;
+        }
+    else if (biceps_l > threshold_l && biceps_r <= threshold_r){
+        //arm 1 activated, move left
+        //pc.printf("Move left    ");
+        //pc.printf("right: %f    ",biceps_r);
+        //pc.printf("left:  %f\n\r",biceps_l);
+        //wait(0.5);
+        Tout  = -1;
+        //pc.printf("T=%d\n\r",T);
+        ledBlue=1;//off
+        ledRed=0;//on    red
+        }
+    else if (biceps_l <= threshold_l && biceps_r > threshold_r){
+        //arm 1 activated, move right
+        //pc.printf("Move right   "); 
+        //pc.printf("right: %f    ",biceps_r);
+        //pc.printf("left:  %f\n\r",biceps_l);
+        //wait(0.5);
+        Tout = 1;
+        //pc.printf("T=%d\n\r",T);
+        ledBlue=0;//on    blue
+        ledRed=1;//off
+        }
+    else{
+        //wait(0.2);
+        ledRed = 1;
+        ledBlue = 1;  //off
+        //pc.printf("Nothing...   ");
+        //wait(0.5);
+        Tout = 0;
+        //pc.printf("right: %f    ",biceps_r);
+        //pc.printf("left:  %f\n\r",biceps_l);
+        }
+}
+
+void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut)
+{    
     //get joint positions q feedback from encoder
     float Encoder1Position = counts1/resolution;         //angular position in radians, encoder axis
     float Encoder2Position = counts2/resolution;
@@ -169,10 +257,10 @@
       }
     else{      
     */
-    //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
+    //get new reference position from EMG  
     biceps_l = !button1.read() * EMGgain; //emg0.read();     //velocity or reference position change, EMG with a gain
     biceps_r = !button2.read() * EMGgain; //emg1.read();
-    if (biceps_l > 0 && biceps_r > 0){
+    if (T==-2){          //(biceps_l > 0 && biceps_r > 0){
         //both arms activated: stamp moves down
         //led1 = 1;
         //led2 = 1;
@@ -180,11 +268,11 @@
         ReferencePosition_ynew = y_stampdown; //ReferencePosition_y - dy_stampdown;   //into stamping vertical position ~the stamp down action
         
         }
-    else if (biceps_l > 0 && biceps_r <= 0){
+    else if (T==-1){     //(biceps_l > 0 && biceps_r <= 0){
         //arm 1 activated, move left
         //led1 = 1;
         //led2 = 0;
-        ReferencePosition_xnew = ReferencePosition_x - 0.2; //biceps_l;
+        ReferencePosition_xnew = ReferencePosition_x - x_movement; //biceps_l; //0.2;
         ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
         /*
         PositionError_x = ReferencePosition_x - Position_x;        //Position error in dx,dy
@@ -196,11 +284,11 @@
         q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
         */
         }
-    else if (biceps_l <= 0 && biceps_r > 0){
+    else if (T==1){      //(biceps_l <= 0 && biceps_r > 0){
         //arm 1 activated, move right
         //led1 = 0;
         //led2 = 1;
-        ReferencePosition_xnew = ReferencePosition_x + 0.2; //biceps_r;
+        ReferencePosition_xnew = ReferencePosition_x + x_movement; //biceps_r; //0.2;
         ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
         /*PositionError_x = ReferencePosition_x - Position_x;        //Position error in dx,dy
         PositionError_y = ReferencePosition_y - Position_y;        //Position error in dx,dy
@@ -257,7 +345,7 @@
     //update joint angles
     //q1Out = q1Out + q1_dotOut;  //in radians
     //q2Out = q2Out + q2_dotOut;
-    
+    /*
     pc.baud(115200);
     pc.printf("refX: %f ",ReferencePosition_xnew);
     pc.printf("refY: %f ",ReferencePosition_ynew);
@@ -265,7 +353,7 @@
     pc.printf("q1ref: %f    ", q1_refOut);
     pc.printf("q2: %f   ", q2Out);
     pc.printf("q2ref: %f    ", q2_refOut);
-    
+    */
     /*
     pc.printf("dx:          %f \r\n", dx);
     pc.printf("dy:          %f \r\n", dy);
@@ -282,6 +370,13 @@
     pc.printf("Motor2:      %f \r\n", q2Out);
     */
     
+    scope.set(0, envelopeR);
+    scope.set(1, envelopeL);
+    scope.set(2, T);
+    scope.set(3, ReferencePosition_xnew);
+    scope.set(4, ReferencePosition_ynew);
+    scope.set(5, q1_refOut);
+    scope.send();
     }
     
 
@@ -337,11 +432,12 @@
     pc.printf("IE2: %f  ", q2IntError);
     pc.printf("DE2: %f  ", q2DerivativeError);
     */
+    /*
     pc.printf("TE1: %f  ", TotalError1);
     pc.printf("TE2: %f  ", TotalError2);
     pc.printf("M1: %f   ", motorValue1Out);
     pc.printf("M2: %f   \r\n", motorValue2Out);
-      
+    */
     q1_error_prev = q1_error;
     q2_error_prev = q2_error;
     TotalError1_prev = TotalError1;
@@ -400,6 +496,7 @@
     // This function measures the EMG of both arms, calculates via IK what
     // the joint positions should be, and controls the motor with 
     // a Feedback controller. This is called from a Ticker.
+    FilteredSample(T, envelopeL, envelopeR);
     GetReferenceKinematics1(q1, q2, q1_ref, q2_ref);
     FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
     SetMotor1(motorValue1, motorValue2);
@@ -428,25 +525,40 @@
  //Initialize
  //int led1val = led1.read();
  //int led2val = led2.read();
- pc.baud(115200);
- pc.printf("Test putty IK");
+ //pc.baud(115200);
+ //pc.printf("Test putty IK");
+    ledRed=1;
+    ledBlue=1; 
+    ledRed=0; //red
+
+    bcq1R.add(&bq1R).add(&bq2R);
+    bcq2R.add(&bq3R);
+    
+    bcq1L.add(&bq1L).add(&bq2L);
+    bcq2L.add(&bq3L);
+      
+    filter_timer.attach(&filter_timer_act, 0.0004); //2500Hz (same as with filter coefficients on matlab!!! Thus adjust!)
  MeasureTicker.attach(&MeasureTicker_act, 0.1f); 
  bqc.add(&bq1).add(&bq2);           //set BiQuad chain
  pidf.PIDF( Kp, Ki, Kd, N, t_sample );    //set PID filter
  while(1)
     {
+//        if (filter_timer_go){
+    //              filter_timer_go=false;
+    //              FilteredSample(T);}
         if (MeasureTicker_go){
             MeasureTicker_go=false;
-            ledgrn = 1;
-            ledblue = 0;
+            ledGrn = 1;
+            ledBlue = 0;
             MeasureAndControl();
             counts1 = Encoder1.getPulses();           // gives position of encoder 
             counts2 = Encoder2.getPulses();           // gives position of encoder 
             //pc.printf("counts1: %i  ", counts1);
             //pc.printf("counts2: %i  \r\n", counts2);
-            ledblue = 1;
-            ledgrn = 0;
+            ledBlue = 1;
+            ledGrn = 0;
             }
+
 /*
         if (BiQuadTicker_go){
             BiQuadTicker_go=false;