Diff: main.cpp

Revision:

15:a52be6368cd5

Parent:

14:e21cb701ccb8

Child:

16:720365110953

diff -r e21cb701ccb8 -r a52be6368cd5 main.cpp
--- a/main.cpp	Mon Oct 15 12:56:25 2018 +0000
+++ b/main.cpp	Mon Oct 15 14:40:50 2018 +0000
@@ -29,11 +29,11 @@
 FastPWM pin5(D5);       // Motor 2 pwm
 FastPWM pin6(D6);       // Motor 1 pwm
 DigitalOut pin7(D7);    // Motor 1 direction
-//float u1  = pot1;
+//double u1  = pot1;
 
 MODSERIAL pc(USBTX, USBRX);
 Ticker motor;
-float u3 = 0.0;         // Normalised variable for the movement of motor 3
+double u3 = 0.0;         // Normalised variable for the movement of motor 3
 
 void draaibuttons()         
 {   /*  Pressing button 2 concludes in a change of speed. While button 1 is pressed,
@@ -54,7 +54,6 @@
             {   u3 = 1.0f;
             }
         }
-    
 }
 
 void draai()    
@@ -63,7 +62,7 @@
     shield for the moving direction and speed of motor 3.
 */
 {      
-    float u1 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 1
+    double u1 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 1
     if (u1>0)
     {   pin4 = true;
     }
@@ -72,7 +71,7 @@
     }
     pin5 = fabs(u1);
     
-    float u2 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 2
+    double u2 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 2
     if (u2<0)
     {   pin7 = true;
     }
@@ -81,7 +80,7 @@
     }
     pin6 = fabs(u2);  
     
-    if (u3>0)
+    if (u3>0)           //misschien weg?
     {   pin2 = true;
     }
     else if(u3<0)
@@ -92,23 +91,49 @@
     }
     pin3 = fabs(u3);   
 }
+/*
+double Kp = 17.5;
+double Ki = 1.02;
+double Kd = 23.2;
+double Ts = 0.01;       // Sample time in seconds
+
+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);      
+    
+    // Proportional part:
+    double u_k = Kp * e; 
+    // 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;   
+}
+*/
 
 int main()
 {
     pc.baud(115200);
     
     pin5.period(1.0/10000);
+    pin3.period_us(50);   
+    pin5.period_us(50);    
+    pin6.period_us(50);
+    
+    //Ticker
+    motor.attach(draai, 0.001);
+    
+    //Interrupts
     button1.rise(&draaibuttons);       
     button2.rise(&draaibuttons);
-    
-    pin3.period_us(50);
-    motor.attach(draai, 0.001);
-   
-    pin5.period_us(50);
-    motor.attach(draai, 0.001);
-    
-    pin6.period_us(50);
-    motor.attach(draai, 0.001);
     while(true)
     {
     }