Diff: main.cpp

Revision:

26:c640851fa1e7

Parent:

25:6dcf79346c40

Child:

27:16327d1337cf

--- a/main.cpp	Thu Oct 27 09:10:32 2016 +0000
+++ b/main.cpp	Fri Oct 28 08:29:18 2016 +0000
@@ -2,22 +2,10 @@
 #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
@@ -49,7 +37,6 @@
 bool draailinks;
 bool turn = 0;
 float waiter = 0.1;
-float afstand = -200;
 float translation = 0;
 float degrees3 = 0;
 
@@ -90,22 +77,35 @@
 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
+//setpoints
+const double setpoint_Translation = -200;
+const double setpoint_Back = 0;
+const double setpoint_Rotation = 135;
+double M3_ControlSpeed = 0;
+double M2_ControlSpeed = 0;
+double setpointError_Translation;
+double setpointError_Rotation;
 
-BiQuad pidf;
-// AnalogIn reference( A0 );
-Ticker controllerTicker;
+//copied from slides
+//Arm PID
+const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
+const double Translation_Kp = 6.9240, Translation_Ki = 0.8160, Translation_Kd = 0.4080;
+double Translation_error = 0;
+double Translation_e_prev = 0;
 
-void controller()
+//Spatel PID
+const double Rotation_Kp = , Rotation_Ki =  , Rotation_Kd = ;
+double Rotation_error = 0;
+double Rotation_e_prev = 0;
+
+double pid_control(double error, const double kp, const double ki, const double kd, double &e_int, double &e_prev)
 {
-    double ctrlOutputTranslation = pidf.step(GetTranslationM2());
-    double ctrlOutputRotation = pidf.step( GetRotationM3());
-    //double ctrlOutput = pidf.step( reference.read() );
+    double e_der = (error - e_prev) / Ts;
+    e_prev = error;
+    e_int = e_int + (Ts * error);
+
+    return kp*error + ki + e_int + kd + e_der;
 }
- 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)
@@ -120,7 +120,24 @@
 /** Sample function
  * this function samples the emg and sends it to HIDScope
  **/
+void motorRotation(double setpoint)
+{
+    theta = GetRotationM3();
+    setpointError_Rotation =  setpoint - theta;
 
+    //set direction
+    if (setpointError_Rotation >0) {
+        M3_Rotate = 0;
+    } else {
+        M3_Rotate = 1;
+    }
+    M3_ControlSpeed = (Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
+    if (M3_ControlSpeed < 0.02) {
+       M3_ControlSpeed = 0;
+    }
+    M3_Speed = M3_ControlSpeed;
+
+}
 void filterSampleLeft()
 {
     highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
@@ -253,7 +270,7 @@
         M2_Rotate = 0;
 
     }
-    if (GetRotationM3() > 150) {
+    if (GetRotationM3() > setpoint_Rotation) {
         GoBack();
     }
 }
@@ -264,17 +281,12 @@
     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
     */
     //sample_timer.attach(&sample, 0.001953125);
-    sample_timer2.attach(&filterSampleLeft, 0.001953125);        //512 Hz
-    sample_timer.attach(&filterSampleRight, 0.001953125);
+    sample_timer2.attach(&filterSampleLeft, Ts);        //512 Hz
+    sample_timer.attach(&filterSampleRight, Ts);
     pc.baud(115200);
     pc.printf("please push the button to calibrate \n \r");
     while (1) {