Diff: main.cpp

Revision:

3:4520d0ca4e56

Parent:

2:80177e90c1e6

Child:

4:abdc2d24d0bc

--- a/main.cpp	Fri Oct 06 08:26:55 2017 +0000
+++ b/main.cpp	Fri Oct 06 12:14:32 2017 +0000
@@ -16,31 +16,50 @@
 Encoder motor1(PTD0,PTC4); // motor 1 is from encoder, otherwise it would be a pain in the ass to change all that code to motorencoder
 
 
-float PwmPeriod = 1.0/5000.0;
-/*
-void readoutencoder()
-{
-    pc.printf("pos: %d, speed %f \r\n",motor1.getPosition(), motor1.getSpeed());
-}
+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
+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;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    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_prev = e;
+  e_int = e_int + Ts*e;                 // integral   
+  return Kp*e + Ki*e_int + Kd*e_der;    //PID controller
+  }
+
 */
+
 volatile float potvalue = 0.0;
 volatile float position = 0.0;
 void readpot(){
     potvalue = pot.read();
     //position = motor1.getPosition();
     pc.printf("pos: %d, speed %f reference velocity = %.2f\r\n",motor1.getPosition(), motor1.getSpeed(), potvalue);
-    led1 = 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);
     }
-void writevalues(){
- 
- }   
+ // output PID controller is not yet tested.
+
 int main()
 {  
     
     pc.baud(9600);
     potmeterreadout.attach(readpot,0.2f);
-    motorspeed.period(0.0001f);
+    motorspeed.period(PwmPeriod);
     //float motorspeed = 0.0f;
     while (true) {