Diff: main.cpp

Revision:

6:b89ca8add6ee

Parent:

5:05d09e921b5d

Child:

7:986a0c5fba0e

--- a/main.cpp	Mon Oct 09 14:52:21 2017 +0000
+++ b/main.cpp	Mon Oct 09 15:05:54 2017 +0000
@@ -18,12 +18,12 @@
 
 float PwmPeriod = 0.0001f;
 
-/*const double M1_TS = 0.01f;                                                             // timestep
-const double M1_KP = 2.5, M1_KI = 1.0, M1_KD = 0.5;                                     // controller gains for motor 1
+const double M1_TS = 0.01f;                                                             // timestep
+const double M1_KP = 0.216, M1_KI = 1.8, M1_KD = 0.0;                                   // controller gains for motor 1
 double m1_err_int = 0, m1_prev_err = 0;                                                 // initiallize errors
 const double M1_F_A1 = 1.0, M1_F_A2 = 2.0, M1_F_B0 = 1.0, M1_F_B1 = 3.0, M1_F_B2 = 4.0; // derivative filter coefficients
 double m1_f_v1 = 0.0, m1_f_v2 = 0.0;                                                    // filter variables
-
+/*
 // biquad filter for emg signals
 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;
@@ -32,26 +32,27 @@
     v1 = v;
     return y;
     }
-
+*/
 // PID controller function
 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double&e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
   double e_der = (e - e_prev)/Ts;       // derivative
-  e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
+  //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
   e_prev = e;
   e_int = e_int + Ts*e;                 // integral   
-  return Kp*e + Ki*e_int + Kd*e_der;    //PID controller
+  return Kp*e + Ki*e_int;    //PID controller
   }
 
-*/
+
 
-volatile float potvalue = 0.0;
+volatile double potvalue = 0.0;
 volatile float position = 0.0;
 void readpot(){
-    potvalue = pot.read();
+    potvalue = pot.read()*6.28;
     position = motor1.getPosition()/4200.00*6.28;
     pc.printf("pos: %f, speed %f reference velocity = %.2f\r\n",position, motor1.getSpeed(), potvalue);
-    motorspeed = potvalue;
-    //motorpid = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
+    //motorspeed = potvalue;
+    float motorPID = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
+    motorspeed = motorPID;
     }
  // output PID controller is not yet tested.