Diff: main.cpp

Revision:

42:cef1b3187e4c

Parent:

40:1be9dfad0a10

--- a/main.cpp	Wed Oct 31 13:42:39 2018 +0000
+++ b/main.cpp	Wed Oct 31 14:49:57 2018 +0000
@@ -44,7 +44,7 @@
 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 float     dt = 0.01f;
 
 // Functions
 void Emergency()        // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
@@ -100,7 +100,7 @@
             return  Kd;
         }               
                
-    float PIDcontroller(float refvalue,float CurAngle)  // PID controller for the motors, based on the reference value and the current angle of the motor
+    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;
@@ -109,30 +109,30 @@
         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);
+        static  BiQuad  PIDfilterl(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;
+        //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 = PIDfilter.step(error_derivative);
+        float   filtered_error_derivative = PIDfilterl.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;
+        pc.printf ("l\t u_p = %f \tu_i = %f\t u_d = %f\t\t ",u_k,0,u_d);
+        return  u_k + 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;
-            return  angle;
+        {   return   (pot1*2.0f*pi)-pi;
+            //return  angle;
         }
         
         float DesiredAngler()        // Sets the desired angle for the controller dependent on the scaled angle of potmeter 1
-        {   float   angle = (pot2*2.0f*pi)-pi;
-            return  angle;
+        {   return (pot2*2.0f*pi)-pi;
+            //return  angle;
         }        
         
 float*    turnl()    // main function for movement of motor 1, all above functions with an extra tab are called
@@ -141,14 +141,14 @@
     float   refvalue = DesiredAnglel();                             // different minus sign per motor
     int     counts = Countslinput();                               // different encoder pins per motor
     float   currentangle = CurrentAngle(counts);                  // different minus sign per motor
-    float   PIDcontrol = PIDcontroller(refvalue,currentangle);   // same for every motor
+    float   PIDcontrol = PIDcontrollerl(refvalue,currentangle);   // same for every motor
     float   error = ErrorCalc(refvalue,currentangle);            // same for every motor
     
-    pin6 = fabs(PIDcontrol);                                       // different pins for every motor
+    //pin6 = fabs(PIDcontrol);                                       // different pins for every motor
     pin7 = PIDcontrol > 0.0f;                                      // different pins for every motor
     //pin6 = 0.4+0.6*fabs(PIDcontr);     //geschaald
     //pin6 = fabs(PIDcontr)/pi;
-    //pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error,refvalue,currentangle);
+    //pc.printf("     countsl:  %i      currentanglel: %f     PIDl:  %f     errorl: %f",counts,currentangle,PIDcontrol,error);
     float*  outcome;
     float   turninfo[3] = {error, refvalue, currentangle};
     //float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3
@@ -156,20 +156,47 @@
     return outcome;
 }
 
+    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_integral = 0.0;
+        static float    error_prev = 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;
+        // 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 = PIDfilterr.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);
+        pc.printf ("r\t u_p = %f \tu_i = %f\t u_d = %f\r\n",u_k,u_i,u_d);
+        return  u_k + u_d;
+    }
+
 float*    turnr()    // main function for movement of motor 1, all above functions with an extra tab are called
 {   
     //float   refvalue = pi/4.0f;
     float   refvalue = DesiredAngler();                             // different minus sign per motor
     int     counts = Countsrinput();                               // different encoder pins per motor
     float   currentangle = CurrentAngle(counts);                  // different minus sign per motor
-    float   PIDcontrol = PIDcontroller(refvalue,currentangle);   // same for every motor
+    float   PIDcontrol = PIDcontrollerr(refvalue,currentangle);   // same for every motor
     float   error = ErrorCalc(refvalue,currentangle);            // same for every motor
     
-    pin5 = fabs(PIDcontrol);                                       // different pins for every motor
+    //pin5 = fabs(PIDcontrol);                                       // different pins for every motor
     pin4 = PIDcontrol > 0.0f;                                      // different pins for every motor
     //pin6 = 0.4+0.6*fabs(PIDcontr);     //geschaald
     //pin6 = fabs(PIDcontr)/pi;
     //pc.printf("     error:  %f      ref: %f     angle:  %f \r\n",error,refvalue,currentangle);
+        //pc.printf("     countsr:  %i      currentangler: %f     PIDr:  %f     errorr: %f\r\n",counts,currentangle,PIDcontrol,error);
     float*  outcome;
     float   turninfo[3] = {error, refvalue, currentangle};
     //float ( &fillarr( float (&outcome)[3] ) )[3] { // no decay; argument must be size 3