Diff: main.cpp

Revision:

32:a5b411833d1e

Parent:

31:91ad5b188bd9

Child:

33:ec07e11676ec

Child:

34:30100c1901d4

--- a/main.cpp	Tue Oct 30 15:39:39 2018 +0000
+++ b/main.cpp	Tue Oct 30 18:28:26 2018 +0000
@@ -13,20 +13,20 @@
 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
 
-DigitalIn   pin8(D8);     // Encoder 1 B
-DigitalIn   pin9(D9);     // Encoder 1 A
-DigitalIn   pin10(D10);   // Encoder 2 B
-DigitalIn   pin11(D11);   // Encoder 2 A
-DigitalIn   pin12(D12);   // Encoder 3 B
-DigitalIn   pin13(D13);   // Encoder 3 A
+DigitalIn   pin8(D8);       // Encoder 1 B
+DigitalIn   pin9(D9);       // Encoder 1 A
+DigitalIn   pin10(D10);     // Encoder 2 B
+DigitalIn   pin11(D11);     // Encoder 2 A
+DigitalIn   pin12(D12);     // Encoder 3 B
+DigitalIn   pin13(D13);     // Encoder 3 A
 
 // Output
-DigitalOut  pin2(D2);    // Motor 3 direction
+DigitalOut  pin2(D2);       // Motor 3 direction
 FastPWM     pin3(D3);       // Motor 3 pwm
-DigitalOut  pin4(D4);    // Motor 2 direction
+DigitalOut  pin4(D4);       // Motor 2 direction
 FastPWM     pin5(D5);       // Motor 2 pwm
 FastPWM     pin6(D6);       // Motor 1 pwm
-DigitalOut  pin7(D7);    // Motor 1 direction
+DigitalOut  pin7(D7);       // Motor 1 direction
 
 DigitalOut  greenled(LED_GREEN,1);
 DigitalOut  redled(LED_RED,1);
@@ -41,73 +41,72 @@
 Ticker      encoders;
 
 // Global variables
-const float     pi = 3.14159265358979;
-float           u3 = 0.0;         // Normalised variable for the movement of motor 3
-const float     fCountsRad = 4200.0;
-const float     dt = 0.001;
+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;
 
 // Functions
-void Emergency()
-{   // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
-    greenled = 1;
+void Emergency()        // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
+{   greenled = 1;       // Red light on
     blueled = 1;
     redled = 0;
-    pin3 = 0;                       // All motor forces to zero
+    pin3 = 0;           // All motor forces to zero
     pin5 = 0;
     pin6 = 0;
-    exit (0);                       // Abort mission!!
+    exit (0);           // Abort mission!!
 }
 
     // Subfunctions
-    int Counts1input()
+    int Counts1input()      // Gets the counts from encoder 1
     {   int     counts1;
         counts1 = Encoder1.getPulses();
         return  counts1;
     }
-    int Counts2input()
+    int Counts2input()      // Gets the counts from encoder 2
     {   int     counts2;
         counts2 = Encoder2.getPulses();
         return  counts2;
     }
-    int Counts3input()
+    int Counts3input()      // Gets the counts from encoder3
     {   int     counts3;
         counts3 = Encoder3.getPulses();
         return  counts3;
     }
 
-    float   CurrentAngle(float counts)
-    {   float   angle = ((float)counts*2.0*pi)/fCountsRad;
-        while   (angle > 2.0*pi)
-        {   angle = angle-2.0*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.0*pi)
-        {   angle = angle+2.0*pi;
+        while   (angle < -2.0f*pi)
+        {   angle = angle+2.0f*pi;
         }
         return angle;
     }
 
-    float  ErrorCalc(float yref,float CurAngle)
-    {   float   error = yref - CurAngle;
+    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;
         return  error;
     }
         
-        float Kpcontr()
-        {   float   Kp = 20*pot2;
+        float Kpcontr()     // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
+        {   float   Kp = 20.0f*pot2;
             return  Kp;
         }
         
-        float Kdcontr()
-        {   float   Kd = 0.25*pot1;
+        float Kdcontr()     // Sets the Kd value for the controller dependent on the scaled angle of potmeter 1
+        {   float   Kd = 0.25f*pot1;
             return  Kd;
         }               
                
-    float PIDcontroller(float yref,float CurAngle)
+    float PIDcontroller(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.42; 
-        float   Ki = 1.02;
-        float   Kd = 0.0493;
+        float   Kp = 10.42f; 
+        float   Ki = 1.02f;
+        float   Kd = 0.0493f;
         //float   Kd = Kdcontr();
-        float   error = ErrorCalc(yref,CurAngle);
+        float   error = ErrorCalc(refvalue,CurAngle);
         static float    error_integral = 0.0;
         static float    error_prev = error; // initialization with this value only done once!
         static  BiQuad  PIDfilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
@@ -121,45 +120,43 @@
         float   filtered_error_derivative = PIDfilter.step(error_derivative);
         float   u_d = Kd * filtered_error_derivative;
         error_prev = error;
-        
         // Sum all parts and return it
         pc.printf ("Kp = %f     Kd = %f",Kp,Kd);
         return  u_k + u_i + u_d;
     }
 
-        float DesiredAngle()
-        {   float   angle = (pot1*2.0*pi)-pi;
+        float DesiredAngle()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
+        {   float   angle = (pot1*2.0f*pi)-pi;
             return  angle;
         }
 
-void    turn1()    // main function, all below functions with an extra tab are called
+void    turn1()    // main function for movement of motor 1, all above functions with an extra tab are called
 {   
     //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   currentangle1 = CurrentAngle(counts1);
+    float   PIDcontrol1 = PIDcontroller(refvalue1,currentangle1);
+    float   error1 = ErrorCalc(refvalue1,currentangle1);
     
-    pin6 = fabs(PIDcontr);
-    pin7 = PIDcontr > 0.0f;
+    pin6 = fabs(PIDcontrol1);
+    pin7 = PIDcontrol1 > 0.0f;
     //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);
+    pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error1,refvalue1,currentangle1);
 }
 
-float ActualPosition(int counts, int offsetcounts)
-{   // After calibration, the counts are returned to 0;
-    float MotorPos = - (counts - offsetcounts) / fCountsRad;
+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 MotorPos;
+    return MotorPosition;
 }
 
 // Main program
 int main()
 {   
     pc.baud(115200);  
-    pin3.period_us(15);     // Period on one pin, gives period on all pins
+    pin3.period_us(15);     // If you give a period on one pin, c++ gives all pins this period
         
     //motor.attach(turn1,dt);