Diff: main.cpp

Revision:

31:91ad5b188bd9

Parent:

30:c4a3e868ef04

Child:

32:a5b411833d1e

--- a/main.cpp	Tue Oct 30 14:17:13 2018 +0000
+++ b/main.cpp	Tue Oct 30 15:39:39 2018 +0000
@@ -1,19 +1,17 @@
 // Inclusion of libraries
-#include "mbed.h"
-#include "FastPWM.h"    
-#include "QEI.h"        // Includes library for encoder
-#include "MODSERIAL.h"
-#include "HIDScope.h"
-#include "BiQuad.h"
+#include    "mbed.h"
+#include    "FastPWM.h"    
+#include    "QEI.h"        // Includes library for encoder
+#include    "MODSERIAL.h"
+#include    "HIDScope.h"
+#include    "BiQuad.h"
 
 // Input
 AnalogIn    pot1(A1);
 AnalogIn    pot2(A2);
 InterruptIn button1(D0);
 InterruptIn button2(D1);
-InterruptIn emergencybutton(SW2);   
-//The button SW2 on the K64F is the emergency button: if you press this, everything will abort as soon as possible
-
+InterruptIn emergencybutton(SW2);   //The button SW2 on the K64F is the emergency button: if you press this, everything will abort as soon as possible
 
 DigitalIn   pin8(D8);     // Encoder 1 B
 DigitalIn   pin9(D9);     // Encoder 1 A
@@ -47,10 +45,6 @@
 float           u3 = 0.0;         // Normalised variable for the movement of motor 3
 const float     fCountsRad = 4200.0;
 const float     dt = 0.001;
-//const float    Kp = 18.0;    // given value is 17.5
-//const float    Ki = 2.02;  // given value is 1.02
-//const float    Kd = 23.0;      // given value is 23.2
-//const float    Ts = 0.0025; // Sample time in seconds
 
 // Functions
 void Emergency()
@@ -58,13 +52,13 @@
     greenled = 1;
     blueled = 1;
     redled = 0;
-    pin3 = 0;
+    pin3 = 0;                       // All motor forces to zero
     pin5 = 0;
     pin6 = 0;
-    exit (0);                       //Abort mission!!
+    exit (0);                       // Abort mission!!
 }
 
-    //Subfunctions
+    // Subfunctions
     int Counts1input()
     {   int     counts1;
         counts1 = Encoder1.getPulses();
@@ -98,36 +92,33 @@
     }
         
         float Kpcontr()
-        {   float   Kp = 50*pot2;
+        {   float   Kp = 20*pot2;
             return  Kp;
         }
         
         float Kdcontr()
-        {   float   Kd = 0.5*pot1;
+        {   float   Kd = 0.25*pot1;
             return  Kd;
         }               
-        
+               
     float PIDcontroller(float yref,float CurAngle)
     {   //float   Kp = Kpcontr();
-        float   Kp = 10.09;
+        float   Kp = 10.42; 
         float   Ki = 1.02;
-        float   Kd = 0.05;
+        float   Kd = 0.0493;
         //float   Kd = Kdcontr();
         float   error = ErrorCalc(yref,CurAngle);
         static float    error_integral = 0.0;
         static float    error_prev = error; // initialization with this value only done once!
-        static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-        
+        static  BiQuad  PIDfilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
         // Proportional part:
         float   u_k = Kp * error;
-        
         // Integral part
         error_integral = error_integral + error * dt;
         float   u_i = Ki * error_integral;
-        
         // Derivative part
         float   error_derivative = (error - error_prev)/dt;
-        float   filtered_error_derivative = LowPassFilter.step(error_derivative);
+        float   filtered_error_derivative = PIDfilter.step(error_derivative);
         float   u_d = Kd * filtered_error_derivative;
         error_prev = error;
         
@@ -143,31 +134,17 @@
 
 void    turn1()    // main function, all below functions with an extra tab are called
 {   
-    //float   refvalue1 = pi/2.0;
+    //float   refvalue1 = pi/4.0;
     float   refvalue1 = DesiredAngle();
     int     counts1 = Counts1input();
     float   angle1 = CurrentAngle(counts1);
     float   PIDcontr = PIDcontroller(refvalue1,angle1);
     float   error = ErrorCalc(refvalue1,angle1);
     
-    float tmp = fabs(PIDcontr);
-    /*if (tmp < 0.1f) { 
-        tmp = 0.0f; 
-    }
-    */
+    pin6 = fabs(PIDcontr);
     pin7 = PIDcontr > 0.0f;
-    pin6 = tmp;
-    //pin6 = 0.4+0.6*tmp;     //geschaald
-    /*pin6 = fabs(PIDcontr)/pi;
-    if  (PIDcontr > 0.1)
-    {   pin7 = true;
-    }
-    else if (PIDcontr < -0.1)
-    {   pin7 = false;    
-    }
-    else
-    {   pin6 = 0;      
-    }*/
+    //pin6 = 0.4+0.6*fabs(PIDcontr);     //geschaald
+    //pin6 = fabs(PIDcontr)/pi;
     pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error,refvalue1,angle1);
 }
 
@@ -182,10 +159,8 @@
 int main()
 {   
     pc.baud(115200);  
-    
-    pin3.period_us(15);
-    //pin5.period(0.05);
-    //pin6.period(0.05);
+    pin3.period_us(15);     // Period on one pin, gives period on all pins
+        
     //motor.attach(turn1,dt);
 
     emergencybutton.rise(Emergency);              //If the button is pressed, stop program