Diff: main.cpp

Revision:

44:de6b4eac5cb7

Parent:

43:e8f2193822b7

--- a/main.cpp	Wed Oct 31 16:32:36 2018 +0000
+++ b/main.cpp	Wed Oct 31 17:32:25 2018 +0000
@@ -43,17 +43,17 @@
 Ticker      encoders;
 
 // Global variables
-const float     pi = 3.14159265358979f;
-float           u3 = 0.0f;         // Normalised variable for the movement of motor 3
-const float     fCountsRad = 4200.0f;
-const float     dt = 0.001f;
+const double     pi = 3.14159265358979;
+double           u3 = 0.0;         // Normalised variable for the movement of motor 3
+const double     fCountsRad = 4200.0;
+const double     dt = 0.001;
 
-float   currentanglel;
-float   errorl;
-float   currentangler;
-float   errorr;
-float   currentanglef;
-float   errorf;
+double   currentanglel;
+double   errorl;
+double   currentangler;
+double   errorr;
+double   currentanglef;
+double   errorf;
 
 // Functions
 void Emergency()        // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
@@ -83,160 +83,160 @@
         return  countsf;
     }
 
-    float   CurrentAngle(float counts)      // Calculates the current angle of the motor (between -2*pi to 2*pi) based on the counts from the encoder
-    {   float   angle = ((float)counts*2.0f*pi)/fCountsRad;
-        while   (angle > 2.0f*pi)
-        {   angle = angle-2.0f*pi;
+    double   CurrentAngle(double counts)      // Calculates the current angle of the motor (between -2*pi to 2*pi) based on the counts from the encoder
+    {   double   angle = ((double)counts*2.0*pi)/fCountsRad;
+        while   (angle > 2.0*pi)
+        {   angle = angle-2.0*pi;
         }
-        while   (angle < -2.0f*pi)
-        {   angle = angle+2.0f*pi;
+        while   (angle < -2.0*pi)
+        {   angle = angle+2.0*pi;
         }
         return angle;
     }
 
-    float  ErrorCalc(float refvalue,float CurAngle)     // Calculates the error of the system, based on the current angle and the reference value
-    {   float   error = refvalue - CurAngle;
+    double  ErrorCalc(double refvalue,double CurAngle)     // Calculates the error of the system, based on the current angle and the reference value
+    {   double   error = refvalue - CurAngle;
         return  error;
     }
         
-        float Kpcontr()     // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
-        {   float   Kp = 20.0f*pot2;
+        double Kpcontr()     // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
+        {   double   Kp = 20.0*pot2;
             return  Kp;
         }
         
-        float Kdcontr()     // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
-        {   float   Kd = 0.25f*pot1;
+        double Kdcontr()     // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
+        {   double   Kd = 0.25*pot1;
             return  Kd;
         }               
                    
-    float PIDcontrollerl(float refvalue,float CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
-    {   //float   Kp = Kpcontr();
-        float   Kp = 10.42f; 
-        float   Ki = 1.02f;
-        float   Kd = 0.0493f;
-        //float   Kd = Kdcontr();
-        float   error = ErrorCalc(refvalue,CurAngle);
-        static float    error_integrall = 0.0;
-        static float    error_prevl = error; // initialization with this value only done once!
+    double PIDcontrollerl(double refvalue,double CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
+    {   //double   Kp = Kpcontr();
+        double   Kp = 10.42; 
+        double   Ki = 1.02;
+        double   Kd = 0.0493;
+        //double   Kd = Kdcontr();
+        double   error = ErrorCalc(refvalue,CurAngle);
+        static double    error_integrall = 0.0;
+        static double    error_prevl = error; // initialization with this value only done once!
         static  BiQuad  PIDfilterl(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
         // Proportional part:
-        float   u_k = Kp * error;
+        double   u_k = Kp * error;
         // Integral part
         error_integrall = error_integrall + error * dt;
-        float   u_i = Ki * error_integrall;
+        double   u_i = Ki * error_integrall;
         // Derivative part
-        float   error_derivative = (error - error_prevl)/dt;
-        float   filtered_error_derivative = PIDfilterl.step(error_derivative);
-        float   u_d = Kd * filtered_error_derivative;
+        double   error_derivative = (error - error_prevl)/dt;
+        double   filtered_error_derivative = PIDfilterl.step(error_derivative);
+        double   u_d = Kd * filtered_error_derivative;
         error_prevl = error;
         // Sum all parts and return it
         return  u_k + u_i + u_d;
     }
 
-    float DesiredAnglel()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-    {   float   angle = (pot1*2.0f*pi)-pi;
+    double DesiredAnglel()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+    {   double   angle = (pot1*2.0*pi)-pi;
         return  angle;
     }    
         
 void    turnl()    // main function for movement of motor 1, all above functions with an extra tab are called
 {   
-    //float   refvaluel = pi/4.0f;
-    float   refvaluel = DesiredAnglel();                             // different minus sign per motor
+    //double   refvaluel = pi/4.0f;
+    double   refvaluel = DesiredAnglel();                             // different minus sign per motor
     int     countsl = Countslinput();                               // different encoder pins per motor
     currentanglel = CurrentAngle(countsl);                  // different minus sign per motor
-    float   PIDcontroll = PIDcontrollerl(refvaluel,currentanglel);   // same for every motor
+    double   PIDcontroll = PIDcontrollerl(refvaluel,currentanglel);   // same for every motor
     errorl = ErrorCalc(refvaluel,currentanglel);            // same for every motor
     
     pin6 = fabs(PIDcontroll);                                       // different pins for every motor
-    pin7 = PIDcontroll > 0.0f;                                      // different pins for every motor
+    pin7 = PIDcontroll > 0.0;                                      // different pins for every motor
 }
 
-    float PIDcontrollerr(float refvalue,float CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
-    {   //float   Kp = Kpcontr();
-        float   Kp = 10.42f; 
-        float   Ki = 1.02f;
-        float   Kd = 0.0493f;
-        //float   Kd = Kdcontr();
-        float   error = ErrorCalc(refvalue,CurAngle);
-        static float    error_integralr = 0.0;
-        static float    error_prevr = error; // initialization with this value only done once!
+    double PIDcontrollerr(double refvalue,double CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
+    {   //double   Kp = Kpcontr();
+        double   Kp = 10.42; 
+        double   Ki = 1.02;
+        double   Kd = 0.0493;
+        //double   Kd = Kdcontr();
+        double   error = ErrorCalc(refvalue,CurAngle);
+        static double    error_integralr = 0.0;
+        static double    error_prevr = error; // initialization with this value only done once!
         static  BiQuad  PIDfilterr(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
         // Proportional part:
-        float   u_k = Kp * error;
+        double   u_k = Kp * error;
         // Integral part
         error_integralr = error_integralr + error * dt;
-        float   u_i = Ki * error_integralr;
+        double   u_i = Ki * error_integralr;
         // Derivative part
-        float   error_derivative = (error - error_prevr)/dt;
-        float   filtered_error_derivative = PIDfilterr.step(error_derivative);
-        float   u_d = Kd * filtered_error_derivative;
+        double   error_derivative = (error - error_prevr)/dt;
+        double   filtered_error_derivative = PIDfilterr.step(error_derivative);
+        double   u_d = Kd * filtered_error_derivative;
         error_prevr = error;
         // Sum all parts and return it
         return  u_k + u_i + u_d;
     }
     
-    float DesiredAngler()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-    {   float   angle = (pot2*2.0f*pi)-pi;
+    double DesiredAngler()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+    {   double   angle = (pot2*2.0*pi)-pi;
         return  angle;
     }  
 
 void    turnr()    // main function for movement of motor 1, all above functions with an extra tab are called
 {   
-    float   refvaluer = pi/4.0f;
-    //float   refvaluer = DesiredAngler();                             // different minus sign per motor
+    double   refvaluer = pi/4.0;
+    //double   refvaluer = DesiredAngler();                             // different minus sign per motor
     int     countsr = Countsrinput();                               // different encoder pins per motor
     currentangler = CurrentAngle(countsr);                  // different minus sign per motor
-    float   PIDcontrolr = PIDcontrollerr(refvaluer,currentangler);   // same for every motor
+    double   PIDcontrolr = PIDcontrollerr(refvaluer,currentangler);   // same for every motor
     errorr = ErrorCalc(refvaluer,currentangler);            // same for every motor
     
     pin5 = fabs(PIDcontrolr);                                       // different pins for every motor
-    pin4 = PIDcontrolr > 0.0f;                                      // different pins for every motor
+    pin4 = PIDcontrolr > 0.0;                                      // different pins for every motor
 }
 
-    float PIDcontrollerf(float refvalue,float CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
-    {   //float   Kp = Kpcontr();
-        float   Kp = 10.42f; 
-        float   Ki = 1.02f;
-        float   Kd = 0.0493f;
-        //float   Kd = Kdcontr();
-        float   error = ErrorCalc(refvalue,CurAngle);
-        static float    error_integralf = 0.0;
-        static float    error_prevf = error; // initialization with this value only done once!
+    double PIDcontrollerf(double refvalue,double CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
+    {   //double   Kp = Kpcontr();
+        double   Kp = 10.42; 
+        double   Ki = 1.02;
+        double   Kd = 0.0493;
+        //double   Kd = Kdcontr();
+        double   error = ErrorCalc(refvalue,CurAngle);
+        static double    error_integralf = 0.0;
+        static double    error_prevf = error; // initialization with this value only done once!
         static  BiQuad  PIDfilterf(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
         // Proportional part:
-        float   u_k = Kp * error;
+        double   u_k = Kp * error;
         // Integral part
         error_integralf = error_integralf + error * dt;
-        float   u_i = Ki * error_integralf;
+        double   u_i = Ki * error_integralf;
         // Derivative part
-        float   error_derivative = (error - error_prevf)/dt;
-        float   filtered_error_derivative = PIDfilterf.step(error_derivative);
-        float   u_d = Kd * filtered_error_derivative;
+        double   error_derivative = (error - error_prevf)/dt;
+        double   filtered_error_derivative = PIDfilterf.step(error_derivative);
+        double   u_d = Kd * filtered_error_derivative;
         error_prevf = error;
         // Sum all parts and return it
         return  u_k + u_i + u_d;
     }
     
-    float DesiredAnglef()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-    {   float   angle = (pot2*2.0f*pi)-pi;
+    double DesiredAnglef()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+    {   double   angle = (pot2*2.0*pi)-pi;
         return  angle;
     }  
 
 void    turnf()    // main function for movement of motor 1, all above functions with an extra tab are called
 {   
-    //float   refvaluef = pi/4.0f;
-    float   refvaluef = DesiredAnglef();                             // different minus sign per motor
+    //double   refvaluef = pi/4.0f;
+    double   refvaluef = DesiredAnglef();                             // different minus sign per motor
     int     countsf = Countsfinput();                               // different encoder pins per motor
     currentanglef = CurrentAngle(countsf);                  // different minus sign per motor
-    float   PIDcontrolf = PIDcontrollerf(refvaluef,currentanglef);   // same for every motor
+    double   PIDcontrolf = PIDcontrollerf(refvaluef,currentanglef);   // same for every motor
     errorf = ErrorCalc(refvaluef,currentanglef);            // same for every motor
     
     pin3 = fabs(PIDcontrolf);                                       // different pins for every motor
-    pin2 = PIDcontrolf > 0.0f;                                      // different pins for every motor
+    pin2 = PIDcontrolf > 0.0;                                      // different pins for every motor
 }
 
-float ActualPosition(int counts, int offsetcounts)      // After calibration, this function is used to return the counts (and thus the angle of the system) to 0
-{   float MotorPosition = - (counts - offsetcounts) / fCountsRad;
+double ActualPosition(int counts, int offsetcounts)      // After calibration, this function is used to return the counts (and thus the angle of the system) to 0
+{   double MotorPosition = - (counts - offsetcounts) / fCountsRad;
     // minus sign to correct for direction convention
     return MotorPosition;
 }
@@ -255,7 +255,7 @@
                 
     while   (true)
     {   
-        pc.printf("angle l/r/f: \t %f \t\t %f \t\t %f \t\t error l/r/f: \t %f \t\t %f \t\t %f\r\n",currentanglel,currentangler,currentanglef,errorl,errorr,errorf);
-        wait(0.1f);   
+        pc.printf("angle l/r/d: \t %d \t\t %d \t\t %d \t\t error l/r/f: \t %d \t\t %d \t\t %d\r\n",currentanglel,currentangler,currentanglef,errorl,errorr,errorf);
+        wait(0.1);   
     }
 }