Diff: main.cpp

Revision:

24:bdd74b91abbb

Parent:

23:fdde3e4b9e69

--- a/main.cpp	Tue Oct 25 10:47:41 2016 +0000
+++ b/main.cpp	Thu Oct 27 08:12:25 2016 +0000
@@ -2,10 +2,22 @@
 #include "HIDScope.h"
 #include "MODSERIAL.h"
 #include "QEI.h"
+#include "PID.h"
+#include "BiQuad.h"
 
 DigitalOut led_g(LED_GREEN);
 DigitalOut led_b(LED_BLUE);
 DigitalOut led_r(LED_RED);
+/*
+#define RATEM2 0.001f // transmission during translation
+#define RATEM3 1 // transmission during rotation
+#define Kc 0.30
+#define Ti 0.00
+#define Td 0.00
+
+PID M3_Rotation_Controller(KC, Ti, Td, RATEM3);
+PID M2_Translation_Controller(KC, Ti, Td, RATEM2);
+*/
 
 DigitalOut M1_Rotate(D2); // voltage only base rotation
 PwmOut M1_Speed(D3);      // voltage only base rotation
@@ -37,7 +49,7 @@
 bool draailinks;
 bool turn = 0;
 float waiter = 0.1;
-float afstand = 200;
+float afstand = -200;
 float translation = 0;
 float degrees3 = 0;
 
@@ -78,6 +90,23 @@
 double highpassFilterRight = 0;
 double lowpassFilterRight = 0;
 
+/*/ copied from slides
+const double Ts = 0.01;
+const double Kp = 1.0, Ki = 0.5, Kd = 0.1;
+const double N = 25; // N = 1/Tf
+
+BiQuad pidf;
+// AnalogIn reference( A0 );
+Ticker controllerTicker;
+
+void controller()
+{
+    double ctrlOutputTranslation = pidf.step(GetTranslationM2());
+    double ctrlOutputRotation = pidf.step( GetRotationM3());
+    //double ctrlOutput = pidf.step( reference.read() );
+}
+ end copy*/
+
 double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
                const double b1, const double b2)
 {
@@ -87,16 +116,7 @@
     v1 = v;
     return y;
 }
-/*double biquad2(double u, double&v1, double&v2, const double c1, const double c2, const double d0,
-               const double d1, const double d2)
-{
-    double v = u - c1*v1 - c2*v2;
-    double y = d0*v + d1*v1 + d2*v2;
-    v2 = v1;
-    v1 = v;
-    return y;
-}
-*/
+
 /** Sample function
  * this function samples the emg and sends it to HIDScope
  **/
@@ -117,9 +137,10 @@
     scope.send();
     //pc.printf("%f \n \r ", lowpassFilter);
 }
+
 void sample()
 {
-    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
+    // 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)
@@ -131,10 +152,11 @@
     x_prev = x; // Prepare for next round
 
     scope.send();
-    /* To indicate that the function is working, the LED is toggled */
+    // To indicate that the function is working, the LED is toggled
     led = !led;
     pc.printf("%f, %f \n \r ", x, b);
 }
+
 void GetDirections()
 {
     pc.baud(115200);
@@ -217,14 +239,13 @@
     M3_Speed = 0;
 
     turn = 0;
-
+}
 
-}
 void Burgerflip()
 {
     if (GetPositionM2() > afstand) {
         M3_Speed = 0.2;
-        M3_Rotate = 0;
+        M3_Rotate = 1;
         M2_Speed = 0;
     } else if (GetPositionM2() < afstand) {
         M2_Speed = 1;
@@ -237,10 +258,16 @@
 }
 int main()
 {
+    //Leds
     led_g = 1;
     led_b = 1;
     led_r = 1;
 
+    /*
+        //PID
+        pidf.PIDF( Kp, Ki, Kd, N, Ts );
+        controllerTicker.attach( &controller, Ts );
+    */
     /**Attach the 'sample' function to the timer 'sample_timer'.
     * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
     */
@@ -254,7 +281,7 @@
             calibrate = true;
             threshold_Left = lowpassFilterLeft*0.7;
             threshold_Right = lowpassFilterRight*0.7;
-            
+
         }
         if (calibrate == true) {
             pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);