Revision 7:f54ff293f31a, committed 2017-10-13

Comitter:

vera1

Date:

Fri Oct 13 10:20:35 2017 +0000

Parent:

6:b89ca8add6ee

Commit message:

smth with pid

Changed in this revision

--- a/main.cpp	Mon Oct 09 15:05:54 2017 +0000
+++ b/main.cpp	Fri Oct 13 10:20:35 2017 +0000
@@ -17,13 +17,13 @@
 
 
 float PwmPeriod = 0.0001f;
-
+/*
 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,27 +32,28 @@
     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 PID(volatile double e, const double Kp, const double Ki, const double Kd, double Ts){
+  e = potvalue - position;
   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;    //PID controller
+  return    Kp*e+Ki*e_int; //PID controller
   }
 
-
+*/
 
 volatile double potvalue = 0.0;
 volatile float position = 0.0;
 void readpot(){
-    potvalue = pot.read()*6.28;
+    potvalue = pot.read();
     position = motor1.getPosition()/4200.00*6.28;
     pc.printf("pos: %f, speed %f reference velocity = %.2f\r\n",position, motor1.getSpeed(), potvalue);
-    //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;
+    motorspeed = potvalue;
+    //float motorPID = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS);
+    //motorspeed = motorPID;
     }
  // output PID controller is not yet tested.
 
@@ -60,27 +61,20 @@
 {  
     
     pc.baud(9600);
-    potmeterreadout.attach(readpot,0.2f);
+    potmeterreadout.attach(readpot,0.01f);
     motorspeed.period(PwmPeriod);
     //float motorspeed = 0.0f;
-    while (true) {
-        
-            
+    while (true) {      
             //pc.printf("reference velocity = %.2f\r\n", potvalue);     
-        
-
             if ((button2 == 1)&&(button1 == 0)) {
-
                 motorposition = 0;         // motor turns anticlockwise
-                led2 = 0;
-                
+                led2 = 0;             
             }
-            if ((button2 ==0)&&(button1 ==1)){
-            
+            if ((button2 ==0)&&(button1 ==1)){           
                 motorposition = 1;         // motor turns anticlockwise
                 led2 = 1;
-                }
-        //}
+            }
+        
 
     }
 }