Diff: main.cpp

Revision:

49:48363ca21a15

Parent:

48:36cdeaac67c5

Child:

50:522bb6eebda6

--- a/main.cpp	Wed Oct 31 19:31:06 2018 +0000
+++ b/main.cpp	Wed Oct 31 20:55:06 2018 +0000
@@ -46,7 +46,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.002f;
 
 float   currentanglel;
 float   errorl;
@@ -54,6 +54,8 @@
 float   errorr;
 float   currentanglef;
 float   errorf;
+float   Kp;
+float   Kd;
 
 // Functions
 void Emergency()        // Emgergency, if SW2 on biorobotics is pressed, everything will instantly abort and a red light goes on
@@ -100,21 +102,21 @@
     }
         
         float Kpcontr()     // Sets the Kp value for the controller dependent on the scaled angle of potmeter 2
-        {   float   Kp = 20.0f*pot2;
+        {   float   Kp = 40.0f*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;
+        {   float   Kd = 10.0f*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();
+    {   Kp = Kpcontr();
+        //float   Kp = 10.42f; 
+        float Ki = 1.02f;
+        //float   Kd = 0.0493f;
+        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!
@@ -140,8 +142,8 @@
         
 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
+    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
@@ -182,8 +184,8 @@
 
 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
+    //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
@@ -248,14 +250,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);
     
     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);
         wait(0.1f);   
     }
 }