Diff: main.cpp

Revision:

30:492595db0fc3

Parent:

29:b6d7bcb26f47

Child:

31:21a112643dc9

--- a/main.cpp	Tue Nov 01 14:05:14 2016 +0000
+++ b/main.cpp	Wed Nov 02 13:30:57 2016 +0000
@@ -22,21 +22,22 @@
 DigitalOut M3_Rotate(D7);   // encoder side pot 1 spatel rotation
 PwmOut M3_Speed(D6);        // encoder side pot 1 spatel rotation
 
-DigitalIn links(SW3);
-DigitalIn rechts(SW2);
+bool links;
+bool rechts;
 
 AnalogIn pot1(A4); // pot 1 motor 1
 AnalogIn pot2(A3); // pot 2 motor 3
 
 //Define objects
+HIDScope    scope( 2 );
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
-DigitalIn   buttonCalibrate(D9);
+DigitalIn   buttonCalibrate(SW3);
 
 bool draairechts;
 bool draailinks;
 bool turn = 0;
-float waiter = 0.1;
+float waiter = 0.2;
 float translation = 0;
 float degrees3 = 0;
 
@@ -55,11 +56,9 @@
 double threshold_Left = 0;
 double threshold_Right= 0;
 Ticker      sample_timer;
-Ticker      sample_timer2;
 Ticker      printinfo;
 Ticker      checkSetpointTranslation;
 Ticker      checkSetpointRotation;
-HIDScope    scope( 2 );
 DigitalOut  led(LED1);
 const double a1 = -1.6475;
 const double a2 = 0.7009;
@@ -71,10 +70,14 @@
 const double d0 = 0.0001551;
 const double d1 = 0.0003103;
 const double d2 = 0.0001551;
-double v1_high = 0;
-double v2_high = 0;
-double v1_low = 0;
-double v2_low = 0;
+double v1_HR = 0;
+double v2_HR = 0;
+double v1_LR = 0;
+double v2_LR = 0;
+double v1_HL = 0;
+double v2_HL = 0;
+double v1_LL = 0;
+double v2_LL = 0;
 double highpassFilterLeft = 0;
 double lowpassFilterLeft = 0;
 double highpassFilterRight = 0;
@@ -116,8 +119,8 @@
     return kp*error + ki + e_int + kd + e_der;
 }
 
-double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
-               const double b1, const double b2)
+double biquad(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
+              const double b1, const double b2)
 {
     double v = u - a1*v1 - a2*v2;
     double y = b0*v + b1*v1 + b2*v2;
@@ -130,31 +133,27 @@
  * this function samples the emg and sends it to HIDScope
  **/
 
-void filterSampleLeft()
+void filterSample()
 {
-    highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
-    lowpassFilterLeft = biquad1(highpassFilterLeft, v1_low, v2_low, c1, c2, d0, d1, d2);
+    highpassFilterLeft = fabs(biquad(emg0.read(), v1_HL, v2_HL, a1, a2, b0, b1, b2));
+    lowpassFilterLeft = biquad(highpassFilterLeft, v1_LL, v2_LL, c1, c2, d0, d1, d2);
+    //pc.printf("%f \n \r ", lowpassFilter);
+    highpassFilterRight = fabs(biquad(emg1.read(), v1_HR, v2_HR, a1, a2, b0, b1, b2));
+    lowpassFilterRight = biquad(highpassFilterRight, v1_LR, v2_LR, c1, c2, d0, d1, d2);
     scope.set(0, lowpassFilterLeft );
-    scope.send();
-    //pc.printf("%f \n \r ", lowpassFilter);
-}
-void filterSampleRight()
-{
-    highpassFilterRight = fabs(biquad1(emg1.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
-    lowpassFilterRight = biquad1(highpassFilterRight, v1_low, v2_low, c1, c2, d0, d1, d2);
     scope.set(1, lowpassFilterRight );
     scope.send();
     //pc.printf("%f \n \r ", lowpassFilter);
 }
 
-void sample()
+/*void sample()
 {
     // 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, emg0.read() );
     scope.set(1, emg1.read() );
     /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
     *  Ensure that enough channels are available (HIDScope scope( 2 ))
-    *  Finally, send all channels to the PC at once */
+    *  Finally, send all channels to the PC at once
 
     x = emg0;   // Capture data        scope.set(0, x);   // store data in first element of scope memory
     b = (x_prev + x)/2.0;   // averaging filter
@@ -164,25 +163,34 @@
     // To indicate that the function is working, the LED is toggled
     led = !led;
     pc.printf("%f, %f \n \r ", x, b);
-}
+} \*/
 
 void GetDirections()
 {
+    if (lowpassFilterRight < threshold_Right)
+        rechts = 0;
+    if (lowpassFilterRight > threshold_Right)
+        rechts = 1;
+    if (lowpassFilterLeft < threshold_Left)
+        links = 0;
+    if (lowpassFilterLeft > threshold_Left)
+        links = 1;
+
     pc.baud(115200);
-    if ((rechts == 0) && (links == 0) && (turn == 0)) {
+    if ((rechts == 1) && (links == 1) && (turn == 0)) {
         draailinks = 0;
         draairechts = 0;
         turn = 1;
         pc.printf("begin de actie \n \r ");
         wait(waiter);
 
-    } else if ((rechts == 0) && (links == 0) && (turn == 1)) {
+    } else if ((rechts == 1) && (links == 1) && (turn == 1)) {
         draailinks = 0;
         draairechts = 0;
         turn = 0;
         pc.printf("breek de actie af \n \r ");
         wait(waiter);
-    } else if ((rechts == 1) && (links == 1)&& (turn == 0)) {
+    } else if ((rechts == 0) && (links == 0)&& (turn == 0)) {
 
     } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
         /* if the right button is pressed and the motor isn't rotating to the left,
@@ -225,17 +233,19 @@
 
     return radians3;
 }
-void CheckErrorRotation(){
+void CheckErrorRotation()
+{
     theta_rotation = GetRotationM3();
     SetpointError_Rotation =  setpointRotation -theta_rotation;
 }
-void CheckErrorTranslation(){
+void CheckErrorTranslation()
+{
     theta_translation = GetPositionM2();
     SetpointError_Translation =  setpointTranslation -theta_translation;
 }
 void motorRotation()
 {
-        printf("setpoint = %f \n\r", setpointRotation);
+    printf("setpoint = %f \n\r", setpointRotation);
     //set direction
     if (SetpointError_Rotation > 0) {
         M3_Rotate = 0;
@@ -251,7 +261,7 @@
         boolrotate = true;
     if ((theta_rotation < (Setpoint_Rotation*0.07) ) && (M3_Speed == 0))
         boolrotate = false;
-        M3_Speed = M3_ControlSpeed;
+    M3_Speed = M3_ControlSpeed;
 }
 void motorTranslation()
 {
@@ -267,14 +277,14 @@
     M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
     if (fabs(SetpointError_Translation) < fabs(Setpoint_Translation*0.05)) {
         M2_ControlSpeed = 0;
-        
+
     }
     if ((theta_translation < Setpoint_Translation*0.95) && (M2_ControlSpeed == 0))
         booltranslate = true;
     if ((theta_translation > Setpoint_Translation*0.05) && (M2_ControlSpeed == 0))
         booltranslate = false;
     M2_Speed = M2_ControlSpeed;
-  
+
 }
 void GoBack()
 {
@@ -324,8 +334,7 @@
     * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
     */
     //sample_timer.attach(&sample, 0.001953125);
-    sample_timer2.attach(&filterSampleLeft, Ts);        //512 Hz
-    sample_timer.attach(&filterSampleRight, Ts);
+    sample_timer.attach(&filterSample, Ts);        //512 Hz
     checkSetpointTranslation.attach(&CheckErrorTranslation,Ts);
     checkSetpointRotation.attach(&CheckErrorRotation,Ts);
     //printinfo.attach(&print, Ts);
@@ -334,12 +343,12 @@
     while (1) {
         if (buttonCalibrate == 0) {
             calibrate = true;
-            threshold_Left = lowpassFilterLeft*0.7;
-            threshold_Right = lowpassFilterRight*0.7;
+            threshold_Left = lowpassFilterLeft*0.9;
+            threshold_Right = lowpassFilterRight*0.9;
         }
         if (calibrate == true) {
-            
-            pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);
+
+            pc.printf("calibration complete, calL = %f, L=%f  CalR = %f, R = %f, boolL=%b boolR=%b  \n \r", threshold_Left, lowpassFilterLeft, threshold_Right, lowpassFilterRight, links, rechts);
             //pc.printf("rotation is %f, setpoint %f, error = %f en translation = %f en de error %f \n \r", GetRotationM3(), Setpoint_Back, SetpointError_Rotation, GetPositionM2(), SetpointError_Translation);
             GetDirections();
             if (draairechts == true) {
@@ -356,7 +365,9 @@
             }
             if ((draailinks == false) && (draairechts == false)) {
                 M1_Speed = 0;
+
             }
+
         }
     }
 }
\ No newline at end of file