Diff: main.cpp

Revision:

6:14a9c4f30c86

Parent:

5:4b25551aeb6e

Child:

7:9a1007e35bac

--- a/main.cpp	Mon Mar 18 12:54:58 2019 +0000
+++ b/main.cpp	Tue Mar 19 16:12:42 2019 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "QEI.h"
+#include "BiQuad.h"
 
 // Algemeen
 DigitalIn button2(SW2);
@@ -23,21 +24,23 @@
 DigitalIn EncoderA(D13);
 DigitalIn EncoderB(D12);
 QEI encoder2 (D13, D12, NC, 8400, QEI::X4_ENCODING);
-float Pulses2;
-float Degrees2;
+double Pulses2;
+double motor_position2;
 
 //Pot meter
 AnalogIn pot(A1);
+AnalogIn pot0(A0);
 float Pot2;
+float Pot1;
 
 //Ticker
 Ticker Pwm;
 Ticker PotRead;
-Ticker Kin;
+
 
 //Kinematica
-float stap;
-float KPot;
+double stap;
+double KPot;
 float KPotabs;
 float ElbowReference;
 float Ellebooghoek1;
@@ -50,115 +53,174 @@
 float lowerlim = 0;
 float upperlim = 748.8;   //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
 
-   
+// VARIABLES PID CONTROLLER
+double Kp = Pot1*10; //
+double Ki = 5;   //
+double Kd = 5; //
+double Ts = 0.1; // Sample time in seconds
+
+
+
+}
 float Kinematics(float KPot)
-    {
-            
-    if (KPot > 0.45f){
-    stap = KPot*15;         // 144 graden van de arm in 5 seconden
-    Hoeknieuw = ElbowReference + stap;
-    return Hoeknieuw;
-    }
-    
-    else if (KPot < -0.45f){
-    stap = KPot*15;
-    Hoeknieuw = ElbowReference + stap;
-    return Hoeknieuw;
-    }
-    
-    else{
-    return ElbowReference;    
-    }  
+{
+
+    if (KPot > 0.45f) {
+        stap = KPot*150*Ts;         // 144 graden van de arm in 5 seconden
+        Hoeknieuw = ElbowReference + stap;
+        return Hoeknieuw;
     }
 
-float Limits(float Ellebooghoek2){
-    
+    else if (KPot < -0.45f) {
+        stap = KPot*150*Ts;
+        Hoeknieuw = ElbowReference + stap;
+        return Hoeknieuw;
+    }
+
+    else {
+        return ElbowReference;
+    }
+}
+
+float Limits(float Ellebooghoek2)
+{
+
     if (Ellebooghoek2 <= upperlim && Ellebooghoek2 >= lowerlim) {       //Binnen de limieten
         Ellebooghoek3 = Ellebooghoek2;
-    } 
-    
+    }
+
     else {
-            if (Ellebooghoek2 >= upperlim) {                            //Boven de limiet
+        if (Ellebooghoek2 >= upperlim) {                            //Boven de limiet
             Ellebooghoek3 = upperlim;
-        } 
-            else {                                                      //Onder de limiet
+        } else {                                                    //Onder de limiet
             Ellebooghoek3 = lowerlim;
         }
     }
-    
+
     return Ellebooghoek3;
-    }
+}
+
+
 
 
-void Period(void)
-    {
-    pwmpin2.period_us(60);
+// ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~
+
+double PID_controller(double error)
+{
+    static double error_integral = 0;
+    static double error_prev = error; // initialization with this value only done once!
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
+
+    // Proportional part:
+    double u_k = Kp * error;
+
+    // Integral part
+    error_integral = error_integral + error * Ts;
+    double u_i = Ki * error_integral;
+
+    // Derivative part
+    double error_derivative = (error - error_prev)/Ts;
+    double filtered_error_derivative = LowPassFilter.step(error_derivative);
+    double u_d = Kd * filtered_error_derivative;
+    error_prev = error;
+
+    // Sum all parts and return it
+    return u_k + u_i + u_d;
+}
+
+void moter2_control(double u)
+{
+    direction2= u < 0.0f; //positief = CW
+    if (fabs(u)> 0.7f) {
+        u = 0.7f;
+    } else {
+        u= u;
     }
-    
+    pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
+}
+
 void PwmMotor(void)
-    {     
+{
     float Ellebooghoek1 = Kinematics(pwm2);
     float Ellebooghoek4 = Limits(Ellebooghoek1);
     ElbowReference = Ellebooghoek4;
-    
-    pc.printf("ElbowReference:%f\n", ElbowReference);
-    pc.printf("pwm2=%f\r\n",pwm2);
-    
-    Degrees2 = (Pulses2/8400)*360;
-    pc.printf("Degrees is:%f\n", Degrees2);
-    direction2 = pwm2 < 0.0f;    //positief = CW, negatief = CCW
-    pwmpin2 = fabs(pwm2);
+
+    //pc.printf("ElbowReference:%f\n", ElbowReference);
+    //pc.printf("pwm2=%f\r\n",pwm2);
+
+    Pulses2 = encoder2.getPulses();
+    motor_position2 = -(Pulses2/8400)*360;
     
+    double error = reference_rotation - motor_position2;
+    //double error = ElbowReference - motor_position2;
+    //pc.printf("Degrees is:%f, error = %f\n", motor_position2, error);
 
-    }  
+    //pwmpin2 = fabs(pwm2);                 //zonder PID
+
+    double u = PID_controller(error);
+    moter2_control(u);
+
+}
+
 void MotorOn(void)
-    {
+{
     pwmpin2 = 0;
-    Pwm.attach (PwmMotor, 0.1);
-    
-    }
+    Pwm.attach (PwmMotor, Ts);
+
+}
 void MotorOff(void)
-    {
-    Pwm.detach ();    
+{
+    Pwm.detach ();
     pwmpin2 = 0;
-    }
+}
 
-void ContinuousReader(void){
+void ContinuousReader(void)
+{
     Pot2 = pot.read();
+    Pot1 = pot0.read();
     pwm2 =(Pot2*2)-1;            //scaling
-    Pulses2 = encoder2.getPulses();
+
     //pc.printf("%f\r\n",Pot2);
-    }
-    
+}
+
+
 
-       
-int main() {
-    Period();
-    PotRead.attach(ContinuousReader,0.1);
+int main()
+{
+    Timer t;
+    t.start();
+    int counter = 0;
+    pwmpin2.period_us(60);
+    PotRead.attach(ContinuousReader,Ts);
     pc.baud(115200);
-    pc.printf("start\r\n");
+    //pc.printf("start\r\n");
     led = 1;
     led2 =1;
     led3 =1;
-    
-    while (true){
-    led3 = 0;
-    if (!button2)
-    {
-        led3 = 1;
+
+    while (true) {
+        led3 = 0;
+        if (!button2) {
+            led3 = 1;
+            led = 0;
+            //pc.printf("MotorOn\r\n");
+            MotorOn();
+        }
+        if (!button3) {
+            //pc.printf("MotorOff\r\n");
+            PotRead.detach();
+            MotorOff();
+        }
         led = 0;
-        pc.printf("MotorOn\r\n");
-        MotorOn();   
+        if(counter==10) {
+            float tmp = t.read();
+            printf("%f,%f,%f,%f\n\r",tmp,motor_position2,reference_rotation,Pot1);
+            counter = 0;
+        }
+        counter++;
+        wait(0.001);
     }
-    if (!button3)
-    {
-        pc.printf("MotorOff\r\n");
-        PotRead.detach();
-        MotorOff();
-    }
-    led = 0;
-        }
-    }
-    
+}
 
 
+