Diff: main.cpp

Revision:

25:76e9e5597416

Parent:

24:d255752065d1

Child:

26:b48708ed51ff

diff -r d255752065d1 -r 76e9e5597416 main.cpp
--- a/main.cpp	Mon Oct 29 13:11:39 2018 +0000
+++ b/main.cpp	Mon Oct 29 14:55:14 2018 +0000
@@ -42,11 +42,13 @@
 
 // Global variables
 const float     pi = 3.14159265358979;
-float u3 = 0.0;         // Normalised variable for the movement of motor 3
+float           u3 = 0.0;         // Normalised variable for the movement of motor 3
 const float     fCountsRad = 4200.0;
+const double    dt = 0.01;
+double          Kp = 17.5;
 
 // Functions
-float   AngleCalc(float counts)
+float   CurrentAngle(float counts)
 {   float angle = ((float)counts*2.0*pi)/fCountsRad;
     while (angle > 2.0*pi)
     {   angle = angle - 2.0*pi;
@@ -57,95 +59,107 @@
     return angle;
 }
 
-void Encoderinput()
+int Counts1input()
 {   int counts1;
-    int counts2;
-    int counts3;
-    float  angle1;
-    float  angle2;
-    float  angle3;
     counts1 = Encoder1.getPulses();
+    return counts1;
+}
+int Counts2input()
+{   int counts2;
     counts2 = Encoder2.getPulses();
+    return counts2;
+}
+int Counts3input()
+{   int counts3;
     counts3 = Encoder3.getPulses();
-    angle1 = AngleCalc(counts1);
-    angle2 = AngleCalc(counts2);
-    angle3 = AngleCalc(counts3);
-         
-    pc.printf("Counts1,2,3: %i  %i  %i      Angle1,2,3: %f  %f  %f\r\n",counts1,counts2,counts3,angle1,angle2,angle3);
+    return counts3;
 }
 
-void draaibuttons()         
-{   /*  Pressing button 2 concludes in a change of speed. While button 1 is pressed,
-        the direction of change of speed is reversed. So pressing button 1 and 2
-        simultaneously results for the turning speed of motor 3 in a slower movement,
-        and eventually the motor will turn the other way around.
-    */
-    if (button1 == 1 && button2 == 1)
-        {   u3 = u3 + 0.1f;  //In stapjes van 0.1            
-            if (u3>1.0f)
-            {   u3 = 1.0f;
-            }
-        }
-    else if (button1 == 0 && button2 == 1)
-        {   u3 = u3 - 0.1f;
-            if (u3>1.0f)
-            {   u3 = 1.0f;
-            }
-        }
+double Pcontroller(double yref,double CurAngle)
+{   double error = yref - CurAngle;
+    
+    // Proportional part:
+    double u_k = Kp * error;
+    // Sum all parts and return it
+    return u_k;
 }
-    
-void draai()    
+
+void turn1()    
+{    float u1 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 2
+    if (u1<0)
+    {   pin7 = true;
+    }
+    else if(u1>0)
+    {   pin7 = false;    
+    }
+    double  refvalue1 = fabs(u1);
+    int counts1 = Counts1input();
+    float angle1 = CurrentAngle(counts1);
+    pin6 = Pcontroller(refvalue1,angle1); 
+    //pc.printf("Counts1,2,3: %i       Angle1,2,3: %f  \r\n",counts1,angle1);
+}
+  
+void turn2()    
 /*  Function for the movement of all motors, using the potmeters for the moving
     direction and speed of motor 1 and 2, and using button 1 and 2 on the biorobotics
     shield for the moving direction and speed of motor 3.
 */
-{      
-    float u1 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 1
-    if (u1>0)
+{   float u2 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 2
+    if (u2<0)
     {   pin4 = true;
     }
-    else if(u1<0)
-    {   pin4 = false;    
-    }
-    pin5 = fabs(u1);
-    
-    float u2 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 2
-    if (u2<0)
-    {   pin7 = true;
-    }
     else if(u2>0)
-    {   pin7 = false;    
-    }
-    pin6 = fabs(u2);  
-    
-    if (u3>0)
-    {   pin2 = true;
+    {   pin4 = false;    
     }
-    else if(u3<0)
-    {   pin2 = false; 
-    }
+    double  refvalue2 = fabs(u2);
+    int counts2 = Counts2input();
+    float angle2 = CurrentAngle(counts2);
+    pin5 = Pcontroller(refvalue2,angle2); 
+    //pc.printf("Counts1,2,3: %i       Angle1,2,3: %f  \r\n",counts1,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
-    {   pin3 = 0;
+    {   // Counterclockwise rotation       
+        RefVel = -1*pot*maxVelocity;   
     }
-    pin3 = fabs(u3);   
+    return RefVel;
 }
+*/
+
+/*
+double ActualPosition(int counts, int offsetcounts)
+{   // After calibration, the counts are returned to 0;
+    double MotorPos = - (counts - offsetcounts) / dCountsRad;
+    // minus sign to correct for direction convention
+    return MotorPos;
+}
+*/
 
 // Main program
 int main()
 {   
     pc.baud(115200);
         
-    pin5.period(1.0/10000);
-    button1.rise(&draaibuttons);       
-    button2.rise(&draaibuttons);
     
-    pin3.period(0.2);
-    pin5.period(0.2);
-    pin6.period(0.2);
-    motor.attach(draai, 0.001);
+    pin3.period(0.1);
+    pin5.period(0.1);
+    pin6.period(0.1);
+    motor.attach(turn1,dt);
+    //motor.attach(turn2,dt);
 
             
     while   (true)
-    {   Encoderinput();
+    {   
+        
     }
 }
\ No newline at end of file