Revision 7:75d2244d7745, committed 2017-10-31

Comitter:

Luuk

Date:

Tue Oct 31 11:21:16 2017 +0000

Parent:

6:acc2669ab20c

Commit message:

with calibration

Changed in this revision

--- a/main.cpp	Mon Oct 30 15:32:52 2017 +0000
+++ b/main.cpp	Tue Oct 31 11:21:16 2017 +0000
@@ -7,10 +7,10 @@
 Serial pc (USBTX,USBRX);
 //HIDScope scope(2);
 
-PwmOut Motor1Vel(D6);            //motor 1 velocity control
+PwmOut Motor1pwm(D6);            //motor 1 velocity control
 DigitalOut Motor1Dir(D7);        //motor 1 direction
 PwmOut Motor2Vel(D5);            //motor 2 velocity control
-DigitalOut Motor2Dir(D4);        //motor 2 direction
+DigitalOut Motor2pwm(D4);        //motor 2 direction
 DigitalOut led1(D2);
 AnalogIn emg1(A0);
 AnalogIn emg2(A1);
@@ -18,6 +18,7 @@
 DigitalOut Button(D3);
 DigitalOut ledb(LED_BLUE);
 DigitalOut ledg(LED_GREEN);
+DigitalOut ledr(LED_RED);
 
 QEI encoder1(D10,D11,NC,16);      //define encoder pins
 QEI encoder2(D8,D9,NC,16);
@@ -30,14 +31,14 @@
 
 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 XSet = 42, YSet = 0;                 //desired position of the end-effector
 float L = 30.0;                  //length of the arms of the robot
 const float Ts = 0.01;            //to control the tickers
 const float KV = 1;               //velocity constant, to scale the desired velocity of the end-effector
 const float P = 0.1;
 
-const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.15*3;   //to convert pulses to radians 
-const float Angle1_0 = pi/4 ,Angle2_0 = pi/4;    //initial angle of the motors
+const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.25*3;   //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 = 200, KI = 0.1, KD = 6, N = 100;
@@ -47,7 +48,8 @@
 double Y1, Y2;                // Y1 and Y2 are taken to conform Y to be used to change the set position X is also used to change the set position
 volatile double Y, X;          
 
-volatile bool ReturnIP = false;
+volatile bool ReturnHP = false;       //to keep calling HomePosition until getting to the setpoint
+volatile bool calibration = true;     //to call FirstMovement the first loop, when the program is run
 int counter = 0;                      //to count the time of inactivity
 
 double emg1filt()
@@ -98,7 +100,7 @@
     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*Ts+2*KP*N*pow(Ts,2))/(2*N*Ts+4);
+    const float b2 = (4*KP+4*KD*N-2*KI*Ts-2*KP*N*Ts+2*KI*N*pow(Ts,2))/(2*N*Ts+4);
     
     float v = err-a1*v1-a2*v2;
     float u = abs(b0*v+b1*v1+b2*v2);
@@ -109,7 +111,7 @@
 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);
+    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); 
@@ -137,31 +139,43 @@
     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;
     
-    float VELOCITY1 = PID(Angle1Set-Angle1,KP,KI,KD,Ts,N,M1_v1,M1_v2)*P;   //PID controller to choose the velocity of the motors
-    float DIRECTION1 = Direction(Angle1Set-Angle1);
-    float VELOCITY2 = PID(Angle2Set-Angle2,KP,KI,KD,Ts,N,M2_v1,M2_v2)*P;
-    float DIRECTION2 = !Direction(Angle2Set-Angle2);
-    Motor1Vel = VELOCITY1;
-    Motor1Dir = DIRECTION1;
-    Motor2Vel = VELOCITY2;
-    Motor2Dir = DIRECTION2;
-    pc.printf("Angle1: %f Angle2: %f XPos: %f YPos: %f \r\n", Angle1 ,Angle2, XPos, YPos );
+    Motor1pwm = PID(Angle1Set-Angle1,KP,KI,KD,Ts,N,M1_v1,M1_v2)*P;;
+    Motor1Dir = Direction(Angle1Set-Angle1);;
+    Motor2Vel = PID(Angle2Set-Angle2,KP,KI,KD,Ts,N,M2_v1,M2_v2)*P;;
+    Motor2pwm = !Direction(Angle2Set-Angle2);
+    pc.printf("Angle1: %f Angle2: %f XPos: %f YPos: %f \r\n", Angle1, Angle2, XPos, YPos );
     
 }
 
+void FirstMovement()//function to get the end-effector to the workspace (with safety margin) to go from there to the home position
+{
+    ledr = 0;
+    XSet = 30;
+    YSet = 0;
+    positionxy(Angle1,Angle2,XPos,YPos); 
+    MoveMotors(XSet,YSet);
+    
+    if(pow(XSet-XPos,2)+pow(YSet-YPos,2) < pow(0.2,2))
+    {//when the motor is close to the set angle(inside the safe workspace) go to the home position
+        ledr = 1;
+        calibration = false;
+        ReturnHP = true;
+    }
+}
+
 void GoToSet()
 {   
     ledg = 0;
     ledb = 1;
-    /*XSet += X;  // keep increasing the set point, depending on the input
-    YSet += Y;*/
-    XSet = 46;
+    XSet = 42+X*10;  // keep increasing the set point, depending on the input
+    //YSet += Y*0.1;
+    //XSet = 46;
     YSet = 0;
     MoveMotors(XSet,YSet);  
       
 }
 
-void InitialPosition()  //Go back to the initial position
+void HomePosition()  //Go back to the initial position
 {  
     ledb = 0;
     ledg = 1;
@@ -171,11 +185,11 @@
     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;
+        ReturnHP = false;
     }
     else
     {
-        ReturnIP = true; 
+        ReturnHP = 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)
@@ -203,27 +217,32 @@
 
 void CallFuncMovement()   //decide which function has to be used
 {   
+
     X = emg1.read(); 
     Y = emg2.read();                   
     positionxy(Angle1,Angle2,XPos,YPos);     //calculate the position of the end point
     if (Button == 1)
     {  //stop motors
-        Motor1Vel = 0;
-        Motor2Vel = 0;
+        Motor1pwm = 0;
+        Motor2pwm = 0;
     }
-    else if (ReturnIP)
+    else if(calibration)
+    {
+        FirstMovement();  
+    }
+    else if(ReturnHP)
     {   //when InitialPosition is called, keep calling until the end-effector gets there;
-        InitialPosition();
+        HomePosition();
         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();
+        HomePosition();
         counter = 0;    
     }
     else if (counter >= 200)
     {  //after 2 seconds of inactivity the robot goes back to the initial position
-        InitialPosition();
+        HomePosition();
         counter = 0;
     }
     else if (X > 0 || Y != 0)