Revision 37:4d75c2432d71, committed 2016-11-02

Comitter:

GerhardBerman

Date:

Wed Nov 02 16:12:00 2016 +0000

Parent:

36:a700f64ba747

Commit message:

Sample Frequency changed to 500 Hz, right arm EMG is correct, left arm EMG is too small

Changed in this revision

--- a/main.cpp	Wed Nov 02 13:51:25 2016 +0000
+++ b/main.cpp	Wed Nov 02 16:12:00 2016 +0000
@@ -111,7 +111,7 @@
 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
+float t_sample = 0.002; //seconds
 const float maxStampDistance = 2.0;
 float Kp = 3.0;//potMeter2.read();
 float Ki = 0.2*Kp;  //0.01*Kp; //potMeter2.read();
@@ -137,21 +137,20 @@
 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);
+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.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
+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(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);
+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.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
+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.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
+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(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);
-
+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
@@ -183,6 +182,12 @@
     double outLfilter1 = bcq1L.step(inLout);
     double outLrect = fabs(outLfilter1);
     envelopeLout = bcq2L.step(outLrect);
+  
+    scope.set(0, inLout);
+    scope.set(1, inRout);  
+    scope.set(2, envelopeL);
+    scope.set(3, envelopeR);
+    scope.send();
     
     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();
@@ -194,8 +199,8 @@
         //wait(0.5);
         Tout = -2;
         //pc.printf("T=%d\n\r",T);
-        ledRed=!ledRed;//blink purple
-        ledBlue=!ledBlue;
+        //ledRed=!ledRed;//blink purple
+        //ledBlue=!ledBlue;
         }
     else if (biceps_l > threshold_l && biceps_r <= threshold_r){
         //arm 1 activated, move left
@@ -205,8 +210,8 @@
         //wait(0.5);
         Tout  = -1;
         //pc.printf("T=%d\n\r",T);
-        ledBlue=1;//off
-        ledRed=0;//on    red
+        //ledBlue=1;//off
+        //ledRed=0;//on    red
         }
     else if (biceps_l <= threshold_l && biceps_r > threshold_r){
         //arm 1 activated, move right
@@ -216,13 +221,13 @@
         //wait(0.5);
         Tout = 1;
         //pc.printf("T=%d\n\r",T);
-        ledBlue=0;//on    blue
-        ledRed=1;//off
+        //ledBlue=0;//on    blue
+        //ledRed=1;//off
         }
     else{
         //wait(0.2);
-        ledRed = 1;
-        ledBlue = 1;  //off
+        //ledRed = 1;
+        //ledBlue = 1;  //off
         //pc.printf("Nothing...   ");
         //wait(0.5);
         Tout = 0;
@@ -370,14 +375,14 @@
     pc.printf("Motor2:      %f \r\n", q2Out);
     */
     
-    scope.set(0, envelopeR);
-    scope.set(1, envelopeL);
+/*    scope.set(0, envelopeL);
+    scope.set(1, envelopeR);
     scope.set(2, T);
     scope.set(3, ReferencePosition_xnew);
     scope.set(4, ReferencePosition_ynew);
     scope.set(5, q1_refOut);
     scope.send();
-    }
+*/    }
     
 
 void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){
@@ -499,9 +504,9 @@
     FilteredSample(T, envelopeL, envelopeR);
     GetReferenceKinematics1(q1, q2, q1_ref, q2_ref);
     FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
-    SetMotor1(motorValue1, motorValue2);
+    //SetMotor1(motorValue1, motorValue2);
 }
-
+/*
 void TimeTrackerF(){
      //wait(1);   
      //float Potmeter1 = potMeter1.read();
@@ -512,7 +517,6 @@
      //pc.printf("TTCounts: %i \r\n", counts1);
 }
 
-/*
 void BiQuadFilter(){            //this function creates a BiQuad filter for the DerivativeCounts
     //double in=DerivativeCounts();
     bqcDerivativeCounts=bqc.step(DerivativeCounts);
@@ -537,15 +541,16 @@
     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); 
+ //filter_timer.attach(&filter_timer_act, 0.002); //500Hz (same as with filter coefficients on matlab!!! Thus adjust!)
+ MeasureTicker.attach(&MeasureTicker_act, 0.002); //0.0004);
  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 (filter_timer_go){
+            filter_timer_go=false;
+            FilteredSample(T, envelopeL, envelopeR);
+            }
         if (MeasureTicker_go){
             MeasureTicker_go=false;
             ledGrn = 1;