Diff: main.cpp

Revision:

30:c4a3e868ef04

Parent:

29:d8e51f4cf080

Child:

31:91ad5b188bd9

--- a/main.cpp	Tue Oct 30 09:51:51 2018 +0000
+++ b/main.cpp	Tue Oct 30 14:17:13 2018 +0000
@@ -36,8 +36,8 @@
 
 // Utilisation of libraries
 MODSERIAL   pc(USBTX, USBRX);
-QEI         Encoder1(D11,D10,NC,4200);      // Counterclockwise motor rotation is the positive direction
-QEI         Encoder2(D9,D8,NC,4200);        // Counterclockwise motor rotation is the positive direction
+QEI         Encoder1(D9,D8,NC,4200);        // Counterclockwise motor rotation is the positive direction
+QEI         Encoder2(D11,D10,NC,4200);      // Counterclockwise motor rotation is the positive direction
 QEI         Encoder3(D13,D12,NC,4200);      // Counterclockwise motor rotation is the positive direction
 Ticker      motor;
 Ticker      encoders;
@@ -46,13 +46,24 @@
 const float     pi = 3.14159265358979;
 float           u3 = 0.0;         // Normalised variable for the movement of motor 3
 const float     fCountsRad = 4200.0;
-const double    dt = 0.5;
-const double    Kp = 17.5;    // given value is 17.5
-const double    Ki = 1.02;  // given value is 1.02
-const double    Kd = 23.2;      // given value is 23.2
-//const double    Ts = 0.0025; // Sample time in seconds
+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()
+{   // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
+    greenled = 1;
+    blueled = 1;
+    redled = 0;
+    pin3 = 0;
+    pin5 = 0;
+    pin6 = 0;
+    exit (0);                       //Abort mission!!
+}
+
     //Subfunctions
     int Counts1input()
     {   int     counts1;
@@ -72,131 +83,117 @@
 
     float   CurrentAngle(float counts)
     {   float   angle = ((float)counts*2.0*pi)/fCountsRad;
-        while (angle > pi)
-        {   angle = angle - 2.0*pi;
-        }
-        while (angle < pi)
-        {   angle = angle + 2.0*pi;
+        while   (angle > 2.0*pi)
+        {   angle = angle-2.0*pi;
         }
-        return  angle;
-    }
-    
-    double  ErrorCalc(double yref,double CurAngle)
-    {   double  error = yref - CurAngle;
-        return  error;
-    }
-            
-    double  Pcontroller(double yref,double CurAngle)
-    {   double  error = ErrorCalc(yref,CurAngle);
-        //double Kp = 50.0*pot1;    // Normalised variable for value of potmeter 1
-        // Proportional part:
-        double  u_k = Kp * error;
-        // Sum all parts and return it
-        //pc.printf("error: %d",error);
-        return  u_k;
+        while   (angle < -2.0*pi)
+        {   angle = angle+2.0*pi;
+        }
+        return angle;
     }
 
-    double  PIcontroller(double yref,double CurAngle)
-    {   double  error = ErrorCalc(yref,CurAngle);    
-        static double   error_integral = 0; 
+    float  ErrorCalc(float yref,float CurAngle)
+    {   float   error = yref - CurAngle;
+        return  error;
+    }
+        
+        float Kpcontr()
+        {   float   Kp = 50*pot2;
+            return  Kp;
+        }
+        
+        float Kdcontr()
+        {   float   Kd = 0.5*pot1;
+            return  Kd;
+        }               
+        
+    float PIDcontroller(float yref,float CurAngle)
+    {   //float   Kp = Kpcontr();
+        float   Kp = 10.09;
+        float   Ki = 1.02;
+        float   Kd = 0.05;
+        //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);
+        
         // Proportional part:
-        double  u_k = Kp * error;
+        float   u_k = Kp * error;
+        
         // Integral part
         error_integral = error_integral + error * dt;
-        double  u_i = Ki * error_integral;
+        float   u_i = Ki * error_integral;
+        
+        // Derivative part
+        float   error_derivative = (error - error_prev)/dt;
+        float   filtered_error_derivative = LowPassFilter.step(error_derivative);
+        float   u_d = Kd * filtered_error_derivative;
+        error_prev = error;
+        
         // Sum all parts and return it
-        return  u_k + u_i;
+        pc.printf ("Kp = %f     Kd = %f",Kp,Kd);
+        return  u_k + u_i + u_d;
     }
-    
-    double PIDcontroller(double yref,double CurAngle)
-    {   double  error = ErrorCalc(yref,CurAngle);
-        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 * error;
-        // Integral part
-        error_integral = error_integral + error * dt;
-        double u_i = Ki * error_integral;
-        // Derivative part
-        double error_derivative = (error - error_prev)/dt;
-        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;
-    }
+
+        float DesiredAngle()
+        {   float   angle = (pot1*2.0*pi)-pi;
+            return  angle;
+        }
 
 void    turn1()    // main function, all below functions with an extra tab are called
 {   
-    double  refvalue1 = pi/4;
+    //float   refvalue1 = pi/2.0;
+    float   refvalue1 = DesiredAngle();
     int     counts1 = Counts1input();
     float   angle1 = CurrentAngle(counts1);
-    double  PIDcontr = PIDcontroller(refvalue1,angle1);
-    double  error = ErrorCalc(refvalue1,angle1);
-     
-    pin6 = fabs(PIDcontr)/pi;
-    if  (error > 0)
+    float   PIDcontr = PIDcontroller(refvalue1,angle1);
+    float   error = ErrorCalc(refvalue1,angle1);
+    
+    float tmp = fabs(PIDcontr);
+    /*if (tmp < 0.1f) { 
+        tmp = 0.0f; 
+    }
+    */
+    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 (error < 0)
+    else if (PIDcontr < -0.1)
     {   pin7 = false;    
     }
-    //pc.printf("%i\r\n",refvalue1);
-    //pc.printf("Counts1,2,3: %i       Angle1,2,3: %f  \r\n",counts1,angle1);
+    else
+    {   pin6 = 0;      
+    }*/
+    pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error,refvalue1,angle1);
 }
-   
-/*double RefVelocity(float pot)
-{   // Returns reference velocity in rad/s. 
-    // Positive value means clockwise rotation.
-    const float maxVelocity=8.4; // in rad/s of course!    
-    double RefVel;  // in rad/s
-    
-    if (button1 == 1)   
-    {   // Clockwise rotation      
-        RefVel = pot*maxVelocity; 
-    } 
-    else
-    {   // Counterclockwise rotation       
-        RefVel = -1*pot*maxVelocity;   
-    }
-    return RefVel;
-}
-*/
 
-
-double ActualPosition(int counts, int offsetcounts)
+float ActualPosition(int counts, int offsetcounts)
 {   // After calibration, the counts are returned to 0;
-    double MotorPos = - (counts - offsetcounts) / fCountsRad;
+    float MotorPos = - (counts - offsetcounts) / fCountsRad;
     // minus sign to correct for direction convention
     return MotorPos;
 }
 
-void Emergency()
-{   // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
-    greenled = 1;
-    blueled = 1;
-    redled = 0;
-    pin3 = 0;
-    pin5 = 0;
-    pin6 = 0;
-    exit (0);                       //Abort mission!!
-}
-
 // Main program
 int main()
 {   
     pc.baud(115200);  
     
-    pin3.period(0.1);
-    pin5.period(0.1);
-    pin6.period(0.1);
-    motor.attach(turn1,dt);
+    pin3.period_us(15);
+    //pin5.period(0.05);
+    //pin6.period(0.05);
+    //motor.attach(turn1,dt);
 
     emergencybutton.rise(Emergency);              //If the button is pressed, stop program
+    //pin6 = 0.01; 
             
     while   (true)
     {   
-        // Nothing here
+        turn1();
+        wait(dt);    
     }
-}
\ No newline at end of file
+}