Diff: main.cpp

Revision:

26:b48708ed51ff

Parent:

25:76e9e5597416

Child:

27:3430dfb4c9fb

--- a/main.cpp	Mon Oct 29 14:55:14 2018 +0000
+++ b/main.cpp	Mon Oct 29 18:22:56 2018 +0000
@@ -24,13 +24,16 @@
 DigitalIn pin13(D13);   // Encoder 3 A
 
 // Output
-DigitalOut pin2(D2);    // Motor 3 direction
-FastPWM pin3(D3);       // Motor 3 pwm
-DigitalOut pin4(D4);    // Motor 2 direction
-FastPWM pin5(D5);       // Motor 2 pwm
-FastPWM pin6(D6);       // Motor 1 pwm
-DigitalOut pin7(D7);    // Motor 1 direction
-//float u1  = pot1;
+DigitalOut  pin2(D2);    // Motor 3 direction
+FastPWM     pin3(D3);       // Motor 3 pwm
+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  greenled(LED_GREEN,1);
+DigitalOut  redled(LED_RED,1);
+DigitalOut  blueled(LED_BLUE,1);
 
 // Utilisation of libraries
 MODSERIAL   pc(USBTX, USBRX);
@@ -44,80 +47,79 @@
 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.01;
-double          Kp = 17.5;
+const double    dt = 0.001;
+const double    Kp = 17.5;    // given value is 17.5
+const double    Ki = 1.02;  // given value is 1.02
+//const double    Ts = 0.0025; // Sample time in seconds
 
 // Functions
-float   CurrentAngle(float counts)
-{   float angle = ((float)counts*2.0*pi)/fCountsRad;
-    while (angle > 2.0*pi)
-    {   angle = angle - 2.0*pi;
-    }
-    while (angle < -2.0*pi)
-    {   angle = angle + 2.0*pi;
+void turn1()    // main function, all below functions with an extra tab are called
+{   
+    double refvalue1 = pi/4;
+    int counts1 = Counts1input();
+    float angle1 = CurrentAngle(counts1);
+    /*if (refvalue1 - angle1 < 0)
+    {   pin7 = true;
     }
-    return angle;
-}
-
-int Counts1input()
-{   int counts1;
-    counts1 = Encoder1.getPulses();
-    return counts1;
-}
-int Counts2input()
-{   int counts2;
-    counts2 = Encoder2.getPulses();
-    return counts2;
-}
-int Counts3input()
-{   int counts3;
-    counts3 = Encoder3.getPulses();
-    return counts3;
+    else if(refvalue1 - angle1 > 0)
+    {   pin7 = false;    
+    }*/
+    pin6 = Pcontroller(refvalue1,angle1); 
+    //pc.printf("%i\r\n",refvalue1);
+    //pc.printf("Counts1,2,3: %i       Angle1,2,3: %f  \r\n",counts1,angle1);
 }
 
-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 turn1()    
-{    float u1 = 2.0*(pot2 - 0.5);    // Normalised variable for the movement of motor 2
-    if (u1<0)
-    {   pin7 = true;
+    //Subfunctions
+    int Counts1input()
+    {   int counts1;
+        counts1 = Encoder1.getPulses();
+        return counts1;
     }
-    else if(u1>0)
-    {   pin7 = false;    
+    int Counts2input()
+    {   int counts2;
+        counts2 = Encoder2.getPulses();
+        return counts2;
+    }
+    int Counts3input()
+    {   int counts3;
+        counts3 = Encoder3.getPulses();
+        return counts3;
     }
-    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 u2 = 2.0*(pot1 - 0.5);    // Normalised variable for the movement of motor 2
-    if (u2<0)
-    {   pin4 = true;
+
+    float   CurrentAngle(float counts)
+    {   float angle = ((float)counts*2.0*pi)/fCountsRad;
+        while (angle > 2.0*pi)
+        {   angle = angle - 2.0*pi;
+        }
+        while (angle < -2.0*pi)
+        {   angle = angle + 2.0*pi;
+        }
+        return angle;
     }
-    else if(u2>0)
-    {   pin4 = false;    
+
+    double  Pcontroller(double yref,double CurAngle)
+    {   double error = 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
+        return u_k;
     }
-    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 PIcontroller(double yref,double CurAngle)
+    {   double error = yref - CurAngle;
+    
+        static double error_integral = 0; 
+        // Proportional part:
+        double u_k = Kp * error;
+        // Integral part
+        error_integral = error_integral + error * dt;
+        double u_i = Ki * error_integral;
+        // Sum all parts and return it
+        return u_k + u_i;
+    }
+
+   
 /*double RefVelocity(float pot)
 {   // Returns reference velocity in rad/s. 
     // Positive value means clockwise rotation.
@@ -145,18 +147,28 @@
 }
 */
 
+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);
-        
+    pc.baud(115200);  
     
     pin3.period(0.1);
     pin5.period(0.1);
     pin6.period(0.1);
     motor.attach(turn1,dt);
-    //motor.attach(turn2,dt);
 
+    emergencybutton.rise(Emergency);              //If the button is pressed, stop program
             
     while   (true)
     {