Diff: main.cpp

Revision:

41:68b170829965

Parent:

40:9ecaab27acde

Child:

42:a080925eabf8

Child:

43:2b2e0bff0b39

--- a/main.cpp	Thu Nov 03 16:12:23 2016 +0000
+++ b/main.cpp	Thu Nov 03 18:56:09 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);
+HIDScope    scope(6);
 
 //initial values
 float dx;
@@ -65,7 +67,7 @@
 float TowerHeight = 0.232;    //height of motor axes above table surface!
 float StampHeight = 0.056;    // height of end effector
 float y_stampup = 0.1;      //height stamp while not stamping: 10cm above table surface
-float y_stampdown = 0.0;    //height stamp while stamping: at table surface
+float y_stampdown = -0.04;    //height stamp while stamping: at table surface
 
 
 //set initial conditions
@@ -102,21 +104,26 @@
 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.1;   //left arm EMG threshold
+double threshold_r = 0.08; //right arm EMG threshold
 float EMGgain = 1.0;
+
 float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
 float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
 float Motor1ExtraGain = 1.0;
-float MotorMaxSpeed = 0.5;  //define a maximum PWM speed for the motor
+float MotorMaxSpeed = 0.1;  //define a maximum PWM speed for the motor
 float t_sample = 0.01; //seconds
-const float maxStampDistance = 0.65;       //0.66;
-const float minStampDistance = 0.39;
-float Kp = 3.0;//potMeter2.read();
-float Ki = 0.1;  //0.01*Kp; //potMeter2.read();
-float Kd = 0.04;     //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.
+const float maxStampDistance = 0.7;       //0.66;
+const float minStampDistance = 0.35;
+float Kp = 4.0;//potMeter2.read();
+float Ki = 0.02;  //0.01*Kp; //potMeter2.read();
+float Kd = 0.02;     //0.1;(unstable!) //0.05; //0.02;        //0.04*Kp; //potMeter2.read();
+float N = 25; //N=1/Tf, Higher N is faster derivative action but more sensitive to noise.
 
 float q1_refOutNew = 0;
 float q1_refOutMin = 0;             //Physical min angle 0.00 radians + 0.1 rad
@@ -135,6 +142,25 @@
 
 BiQuad pidf;
 
+BiQuadChain bcq1R;
+BiQuadChain bcq2R;
+// Notch filter wo=50; bw=wo/35
+BiQuad bq1R(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);
+// High pass Butterworth filter 2nd order, Fc=10;
+BiQuad bq2R(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);
+// Low pass Butterworth filter 2nd order, Fc = 8;
+BiQuad bq3R(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);
+
+BiQuadChain bcq1L;
+BiQuadChain bcq2L;
+// Notch filter wo=50; bw=wo/35
+BiQuad bq1L(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);
+// High pass Butterworth filter 2nd order, Fc=10;
+BiQuad bq2L(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);
+// Low pass Butterworth filter 2nd order, Fc = 8;
+BiQuad bq3L(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-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
@@ -151,6 +177,43 @@
 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 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
+        Tout = -2;
+        }
+    else if (biceps_l > threshold_l && biceps_r <= threshold_r){
+        //arm 1 activated, move left
+        Tout  = -1;
+        }
+    else if (biceps_l <= threshold_l && biceps_r > threshold_r){
+        //arm 1 activated, move right
+        Tout = 1;
+        }
+    else{
+        //wait(0.2);
+        Tout = 0;
+        }
+
+//    scope.set(0, inLout);
+//    scope.set(1, inRout);  
+    
+}
+
 void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut){
     
     //get joint positions q feedback from encoder
@@ -185,19 +248,19 @@
     //get velocity vector v = (Pe*- Pe) = [0; dx; dy] 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){
         //both arms activated: stamp moves down
         //led1 = 1;
         //led2 = 1;
         ReferencePosition_xnew = ReferencePosition_x;
-        ReferencePosition_ynew = y_stampdown; //ReferencePosition_y - dy_stampdown;   //into stamping vertical position ~the stamp down action
+        ReferencePosition_ynew = ReferencePosition_y - 0.015; //ReferencePosition_y - dy_stampdown;   //into stamping vertical position ~the stamp down action
         
         }
-    else if (biceps_l > 0 && biceps_r <= 0){
+    else if (T==-1){
         //arm 1 activated, move left
         //led1 = 1;
         //led2 = 0;
-        ReferencePosition_xnew = ReferencePosition_x - 0.02; //biceps_l;
+        ReferencePosition_xnew = ReferencePosition_x - 0.0009; //biceps_l;
         ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
         /*
         PositionError_x = ReferencePosition_x - Position_x;        //Position error in dx,dy
@@ -209,11 +272,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){
         //arm 1 activated, move right
         //led1 = 0;
         //led2 = 1;
-        ReferencePosition_xnew = ReferencePosition_x + 0.02; //biceps_r;
+        ReferencePosition_xnew = ReferencePosition_x + 0.0009; //biceps_r;
         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
@@ -224,7 +287,7 @@
         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{
+    else{       //T==0
         //led1 = 0;
         //led2 = 0;
         //ReferencePosition_xnew = ReferencePosition_x;
@@ -243,6 +306,11 @@
         ReferencePosition_y = y_stampup;
         pc.printf("Target too close! \r\n");
         }
+    else if (ReferencePosition_ynew < y_stampdown){
+        ReferencePosition_x = ReferencePosition_xnew; // + 0.1;
+        ReferencePosition_y = y_stampdown;
+        pc.printf("Target too close! \r\n");
+        }
     else {
         ReferencePosition_x = ReferencePosition_xnew;        
         ReferencePosition_y = ReferencePosition_ynew;
@@ -297,7 +365,13 @@
     pc.printf("q2ref: %f    ", q2_refOut);
     pc.printf("q1deg: %f   ", q1deg);
     pc.printf("q2deg: %f   \r\n", q2deg);
-        
+    
+    scope.set(0, ReferencePosition_xnew);
+    scope.set(1, ReferencePosition_ynew);
+    scope.set(2, envelopeL);
+    scope.set(3, envelopeR);
+    scope.set(4, T);
+    scope.send();
     /*
     pc.printf("dx:          %f \r\n", dx);
     pc.printf("dy:          %f \r\n", dy);
@@ -449,6 +523,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);
@@ -479,25 +554,35 @@
  //int led2val = led2.read();
  pc.baud(115200);
  pc.printf("Test putty IK");
+     ledRed=1;
+    ledBlue=1; 
+    ledRed=0; //red
+
+    bcq1R.add(&bq1R).add(&bq2R);        //set BiQuad chains
+    bcq2R.add(&bq3R);
+    
+    bcq1L.add(&bq1L).add(&bq2L);
+    bcq2L.add(&bq3L);
+
             counts1 = Encoder1.getPulses();           // gives position of encoder 
             counts2 = Encoder2.getPulses();           // gives position of encoder 
- wait(10.0);
- MeasureTicker.attach(&MeasureTicker_act, 0.1f); 
- bqc.add(&bq1).add(&bq2);
+ wait(20.0);
+ MeasureTicker.attach(&MeasureTicker_act, 0.002); //0.1f); 
+ //bqc.add(&bq1).add(&bq2);                   //set BiQuad chain
  pidf.PIDF( Kp, Ki, Kd, N, t_sample );    //set PID filter
  while(1)
     {
         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){