Diff: main.cpp

Revision:

8:aff2a7d5861a

Parent:

5:05d09e921b5d

--- a/main.cpp	Mon Oct 09 14:52:21 2017 +0000
+++ b/main.cpp	Fri Oct 13 12:27:41 2017 +0000
@@ -4,11 +4,14 @@
 // this program can turn the motor clockwise or counterclockwise depending on the value p or n typed into the terminal. with 's' you can stop the motor
 
 MODSERIAL pc(USBTX,USBRX);
-PwmOut motorspeed(D5);
-DigitalOut motorposition(D4);
+PwmOut motorspeed(D6);
+PwmOut motorspeed2(D5);
+DigitalOut motorposition2(D4);
+DigitalOut motorposition(D7);
 DigitalOut led1(D8);
 DigitalOut led2(D11);
 AnalogIn pot(A1);
+AnalogIn pot2(A2);
 DigitalIn button1(D13);
 DigitalIn button2(D12);
 Ticker potmeterreadout;
@@ -18,69 +21,42 @@
 
 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;
-    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;
+double PI(double e, const double Kp, const double Ki, const double Kd, double Ts, double&e_int){
+  //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
+  return Kp*e + Ki*e_int;    //PI controller
   }
 
-*/
 
-volatile float potvalue = 0.0;
 volatile float position = 0.0;
 void readpot(){
-    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;
-    //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);
+    float motorpi = PI(reference - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int);
+    pc.printf("PI output = %f, reference = %i\n\r", motorpi, reference);
+    //motorspeed = motorpi;
     }
- // output PID controller is not yet tested.
+
 
 int main()
 {  
-    
+    float reference = 0.0;
     pc.baud(9600);
     potmeterreadout.attach(readpot,0.2f);
-    motorspeed.period(PwmPeriod);
+    motorspeed2.period(PwmPeriod);
     //float motorspeed = 0.0f;
     while (true) {
-        
-            
-            //pc.printf("reference velocity = %.2f\r\n", potvalue);     
-        
-
-            if ((button2 == 1)&&(button1 == 0)) {
-
-                motorposition = 0;         // motor turns anticlockwise
-                led2 = 0;
-                
-            }
-            if ((button2 ==0)&&(button1 ==1)){
-            
-                motorposition = 1;         // motor turns anticlockwise
-                led2 = 1;
-                }
-        //}
-
+        reference = 0.0;
+        wait(2.0f);
+        reference = 1.0;
+        wait(5.0f);
+       
     }
 }