Diff: main.cpp

Revision:

3:03db694efdbe

Parent:

2:e36213affbda

Child:

4:2786719c73fd

--- a/main.cpp	Sun Oct 15 14:15:10 2017 +0000
+++ b/main.cpp	Thu Oct 26 21:07:45 2017 +0000
@@ -3,7 +3,7 @@
 #include "MODSERIAL.h"
 //#include "HIDScope.h"
 
-
+//there is something wrong with the code, the motor turns twice when the potmeters are at the minimum
 Serial pc (USBTX,USBRX);
 
 PwmOut Motor1Vel(D6);            //motor 1 velocity control
@@ -20,126 +20,159 @@
 QEI encoder2(D10,D11,NC,16);
 
 Ticker switch_tick;
-float Angle1,Angle2;                   //real angles of the motor
-const float Angle1_0 = 0.8411 ,Angle2_0 = -0.8411;    //initial angle of the motors
-float XPosition = 0.0, YPosition = 0.0;          //real position of end point
-float L2 = 300.0; 
-float L3 = 300.0;           //length of the arms of the robot
-const float pi = 3.14159265359;   //to convert pulses to radians 
-const float PulsesPerRev = 16.0;    //        ||
-const float GearRatio = 131.15;   //        ||
-const float Time = 0.01;          //to control the tickers
+float Angle1,Angle2;              //real angles of the motor
+float XPos, YPos;                 //position of the end-effector
+float XSet, YSet;                 //desired position of the end-effector
+float L = 300.0;                  //length of the arms of the robot
+const float Ts = 0.01;            //to control the tickers
+const float X0 = 14.5, X1 = 21.85, X2 = 31.94, slope = -17.34/6.6;  //values of special points in the workspace
+const float KV = 1;               //velocity constant, to scale the desired velocity of the end-effector
+float X,Y;                        //emg input for the motors
+
+const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.15;   //to convert pulses to radians 
+const float Angle1_0 = pi/2 ,Angle2_0 = 0;    //initial angle of the motors
+
+//variables and constants for the PID
+const float KP = 10, KI = 5, KD = 2, N = 100;
+float M1_v1 = 0, M1_v2 = 0, M2_v1 = 0, M2_v2 = 0;
+
+volatile bool ReturnIP;
+int counter;                      //to count the time of inactivity
+
+// PID controller (Tustin approximation)
+float PID(float err, const float KP, const float KI, const float KD,const float Ts, const float N, float &v1, float &v2)
+{
+    const float a1 = -(N*Ts+2);
+    const float a2 = -(N*Ts-2)/(N*Ts+2);
+    const float b0 = (4*KP+4*KD*N+2*KI*Ts+2*KP*N*Ts+KI*N*pow(Ts,2))/(2*N*Ts+4);
+    const float b1 = (KI*N*pow(Ts,2)-4*KP-4*KD*N)/(N*Ts+2);
+    const float b2 = (4*KP+4*KD*N-2*KI*Ts-2*KP*N*pow(Ts,2))/(2*N*Ts+4);
+    
+    float v = err-a1*v1-a2*v2;
+    float u = abs(b0*v+b1*v1+b2*v2);
+    v2 = v1; v1 = v;
+    return u;
+}
 
-float R = 160.0;              //radius of the work space
-
+void positionxy(float Angle1, float Angle2,float &XPos,float &YPos)
+{ 
+    Angle1 = (encoder1.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle1_0;   //angles of the arms driven by the motors in radians
+    Angle2 = (encoder2.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle2_0;
+    
+    XPos = L*cos(Angle1)+L*cos(Angle2);    // calculate the position of the end-effector
+    YPos = L*sin(Angle1)+L*sin(Angle2);
+}
 
-void positionxy(float Angle1, float Angle2,float &XPosition,float &YPosition)
-{
-    XPosition = L2*cos(Angle1)+L3*cos(Angle2);
-    YPosition = L2*sin(Angle1)+L3*sin(Angle2);
+int Direction(float err)
+{   //choose the direction of the motor, using the difference between the set angle and the actual angle.
+    int a;
+    if(err >= 0) 
+        a = 1;
+    else
+        a = 0;
+    return a;
+}
+
+void MoveMotors()    // move the motors using the inverse kinematic equations of the robot
+{   
+    float VX = KV*(XSet - XPos);     //set the desired velocity, proportional to the difference between the set point and the end-effector position.
+    float VY = KV*(YSet-YPos);
+    
+    float W1 = (VX*(XPos-L*cos(Angle1))+VY*(YPos-L*sin(Angle1)))/(L*YPos*cos(Angle1)-L*XPos*sin(Angle1)); // calculate the needed angular velocity to achieve the desired velocity
+    float W2 = (-VX*XPos-VY*YPos)/(L*YPos*cos(Angle1)-L*XPos*sin(Angle1));
+    
+    float Angle1Set = Angle1 + W1*Ts;   //calculate the set angle, angle at which the motor should be after 1 period with the calculated angular velocity
+    float Angle2Set = Angle2 + W2*Ts;
+    
+    Motor1Vel = PID(Angle1Set-Angle1,KP,KI,KD,Ts,N,M1_v1,M1_v2);   //PID controller to choose the velocity of the motors
+    Motor1Dir = Direction(Angle1Set-Angle1);
+    Motor2Vel = PID(Angle2Set-Angle2,KP,KI,KD,Ts,N,M2_v1,M2_v2);
+    Motor2Dir = Direction(Angle2Set-Angle2);
 }
 
-void moveXpYp()
-{ 
-    Motor1Vel = emg1*2-1;
-    Motor2Vel = emg2*2-1;
-    Motor1Dir = 1;
-    Motor2Dir = 1; 
-}
-void moveXpYn()
-{ 
-    Motor1Vel = emg1*2-1;
-    Motor2Vel = 1-emg2*2;
-    Motor1Dir = 1;
-    Motor2Dir = 0; 
-}
-void moveXnYp()
-{ 
-    Motor1Vel = 1-emg1*2;
-    Motor2Vel = emg2*2-1;
-    Motor1Dir = 0;
-    Motor2Dir = 1; 
+void GoToSet()
+{   
+    XSet += X;  // keep increasing the set point, depending on the input
+    YSet += Y;
+    positionxy(Angle1,Angle2,XPos,YPos);
+    MoveMotors();    
 }
-void moveXnYn()
-{ 
-    Motor1Vel = 1-emg1*2;
-    Motor2Vel = 1-emg2*2;
-    Motor1Dir = 0;
-    Motor2Dir = 0; 
-}
-void StopMotors()
-{
-    Motor1Vel = 0;
-    Motor2Vel = 0;    
-}
-int choosecase(float cons1, float cons2)
-{                       //decide which case has to be used
-    Angle1 = (encoder1.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle1_0;   //angles of the motors in radians
-    Angle2 = (encoder2.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle2_0;
-    positionxy(Angle1,Angle2,XPosition,YPosition);     //calculate the position of the end point
-    int a = 0;
-    if (Button == 1)
+void InitialPosition()  //Go back to the initial position
+{  
+    const float  AllowedError = 1;
+    
+    positionxy(Angle1,Angle2,XPos,YPos);
+    if(sqrt(pow(XPos-XSet,2)+pow(YPos-YSet,2))<AllowedError)  //to call the function until the end effector is at the initial position
+    {
+        ReturnIP = false;
+    }
+    else
     {
-        // not do anything
+        ReturnIP = true; 
+    }
+    //The following if-else statement is to choose a set point so that the end-effector does not go out of the work space while returning 
+    if(XPos<X1)
+    {
+        XSet = X0;
+        YSet = 0;
     }
-    else if (pow((XPosition-400),2) + pow(YPosition,2) > pow(R,2))  // limit so that the robot does not break. Maybe try approximating with an elipse.
+    else if(XPos < X2 && (XPos < slope*YPos+X0 || XPos < slope*YPos+X0 ))
+    {
+        XSet = XPos;
+        YSet = 0;
+    }
+    else if(XPos >= X2 && (XPos < slope*YPos+X0 || XPos < slope*YPos+X0))
+    {
+        XSet = X2;
+        YSet = 0;
+    }
+    else
     {
-        a = 1;       // don't move, the robot has reached the end of the workspace
-    }
-    else if (cons1*10 >= 5 && cons2*10 >= 5)
-    {
-        a = 2;
-    }
-    else if (cons1*10 >= 5 && cons2*10 < 5)
-    {
-        a = 3;    
+        XSet = X0;
+        YSet = 0;
     }
-    else if (cons1*10 < 5 && cons2*10 >= 5)
-    {
-        a = 4;
-    } 
-    else if (cons1*10 < 5 && cons2*10 < 5)
-    {
-        a = 5;
-    }
-    pc.printf("case: %d \r\n",a);
-    return a;
-
+    MoveMotors();
 }
-void selectcase()
-{                                 //call function to move the motors
-    int switchcons = choosecase(emg1.read(),emg2.read());
-    switch(switchcons)
+void CallFuncMovement()   //decide which case has to be used
+{                       
+    positionxy(Angle1,Angle2,XPos,YPos);     //calculate the position of the end point
+    if (Button == 1)
+    {  //stop motors
+        Motor1Vel = 0;
+        Motor2Vel = 0;
+    }
+    else if (ReturnIP)
+    {   //when InitialPosition is called, keep calling until the end-effector gets there;
+        InitialPosition();
+        counter = 0;
+    }
+    else if (sqrt(pow(XPos-300,2) + pow(YPos,2)) > 280 || sqrt(pow(XPos,2) + pow(YPos,2)) > 570 || sqrt(pow(XPos,2) + pow(YPos-300,2)) > 320 || sqrt(pow(XPos,2) + pow(YPos+300,2)) > 320 ) 
+    { // limit so that the robot does not break. 
+        InitialPosition();
+        counter = 0;       
+    }
+    else if (counter == 2/Ts)
+    {  //after 2 seconds of inactivity the robot goes back to the initial position
+        InitialPosition();
+        counter = 0;
+    }
+    else if (X > 0 || Y != 0)
     {
-        case 1:
-            StopMotors();
-            break;
-        case 2:
-            moveXpYp();
-            break;
-        case 3:
-            moveXpYn();
-            break;
-        case 4:
-            moveXnYp();
-            break;
-        case 5:
-            moveXnYn();
-            break;
-        default:
-            break;
+        GoToSet(); 
+        counter = 0;   
     }
-    
+    else 
+    {
+        counter++;
+    } 
 }
+
+
 main()
 {
-    Motor1Dir = 1;
-    Motor2Dir = 1;
     pc.baud(115200);
-    switch_tick.attach(&selectcase,Time);
+    switch_tick.attach(&CallFuncMovement,Ts);
     while (true)
     {
-        pc.printf("XP:  %f  YP:  %f  EMG1:  %f  EMG2:  %f\r\n", XPosition, YPosition,emg1.read(),emg2.read());
-        wait(0.5f);
     }
 }
\ No newline at end of file