Ramon Waninge / Mbed 2 deprecated Milestone1

Dependencies:   FastPWM mbed QEI biquadFilter HIDScope MODSERIAL

Files at this revision

API Documentation at this revision

Comitter:
efvanmarrewijk
Date:
Thu Nov 01 08:46:58 2018 +0000
Parent:
49:48363ca21a15
Commit message:
Latest version just before merge, without calibration code and with code for movement of 3 motors, 3rd motor is TURNED OFF (after calibration it can be turned on again)

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Wed Oct 31 20:55:06 2018 +0000
+++ b/main.cpp	Thu Nov 01 08:46:58 2018 +0000
@@ -43,7 +43,7 @@
 Ticker      encoders;
 
 // Global variables
-const float     pi = 3.14159265358979f;
+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.002f;
@@ -85,13 +85,13 @@
         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;
+    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;
         }
-        while   (angle < -2.0f*pi)
-        {   angle = angle+2.0f*pi;
+        while   (angle < -2.0f*PI)
+        {   angle = angle+2.0f*PI;
         }
         return angle;
     }
@@ -112,11 +112,11 @@
         }               
                    
     float PIDcontrollerl(float refvalue,float CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
-    {   Kp = Kpcontr();
-        //float   Kp = 10.42f; 
+    {   //Kp = Kpcontr();
+        float   Kp = 19.24f; 
         float Ki = 1.02f;
-        //float   Kd = 0.0493f;
-        Kd = Kdcontr();
+        float   Kd = 0.827f;
+        //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!
@@ -136,29 +136,29 @@
     }
 
     float DesiredAnglel()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-    {   float   angle = (pot1*2.0f*pi)-pi;
+    {   float   angle = (pot1*2.0f*PI)-PI;
         return  angle;
     }    
         
 void    turnl()    // main function for movement of motor 1, all above functions with an extra tab are called
 {   
-    float   refvaluel = 0.5f*pi;
-    //float   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
-    errorl = ErrorCalc(refvaluel,currentanglel);            // same for every motor
+    //float   refvaluel = 0.5f*PI;
+    float   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
+    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
+    pin6 = fabs(PIDcontroll);                                           // different pins for every motor
+    pin7 = PIDcontroll > 0.0f;                                          // 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();
+    {   //Kp = Kpcontr();
+        float   Kp = 19.24f; 
+        float Ki = 1.02f;
+        float   Kd = 0.827f;
+        //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!
@@ -178,29 +178,29 @@
     }
     
     float DesiredAngler()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-    {   float   angle = (pot2*2.0f*pi)-pi;
+    {   float   angle = (pot2*2.0f*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
-    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
-    errorr = ErrorCalc(refvaluer,currentangler);            // same for every motor
+    //float   refvaluer = PI/4.0f;
+    float   refvaluer = -DesiredAngler();                               // DONT CHANGE THIS MINUS SIGN: 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
+    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
+    pin5 = fabs(PIDcontrolr);                                           // different pins for every motor
+    pin4 = PIDcontrolr > 0.0f;                                          // 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();
+    {   //Kp = Kpcontr();
+        float   Kp = 19.24f; 
+        float Ki = 1.02f;
+        float   Kd = 0.827f;
+        //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!
@@ -220,27 +220,21 @@
     }
     
     float DesiredAnglef()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-    {   float   angle = (pot2*2.0f*pi)-pi;
+    {   float   angle = (pot2*2.0f*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
-    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
-    errorf = ErrorCalc(refvaluef,currentanglef);            // same for every motor
+    float   refvaluef = 0.0f;
+    //float   refvaluef = -DesiredAnglef();                               // DONT CHANGE THIS MINUS SIGN: 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
+    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
-}
-
-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;
-    // minus sign to correct for direction convention
-    return MotorPosition;
+    pin3 = fabs(PIDcontrolf);                                           // different pins for every motor
+    pin2 = PIDcontrolf > 0.0f;                                          // different pins for every motor
 }
 
 // Main program
@@ -250,16 +244,16 @@
     pin3.period_us(15);     // If you give a period on one pin, c++ gives all pins this period
         
     motorl.attach(turnl,dt);
-    //motorr.attach(turnr,dt);
-    //motorf.attach(turnf,dt);
+    motorr.attach(turnr,dt);
+    //motorf.attach(turnf,dt);      // DONT TURN THIS ON, UNLESS THE CALIBRATION MODE WORKS
     
     emergencybutton.rise(Emergency);              //If the button is pressed, stop program
                 
     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);
+        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);
         
-        pc.printf("Kp: %f \t\t Kd: %f \t\t angle: %f \t\t error: %f\r\n",Kp,Kd,currentanglel,errorl);
+        //pc.printf("Kp: %f \t\t Kd: %f \t\t angle: %f \t\t error: %f\r\n",Kp,Kd,currentanglel,errorl);
         wait(0.1f);   
     }
 }