Diff: main.cpp

Revision:

5:77ffa336d6eb

Parent:

4:2786719c73fd

Child:

6:acc2669ab20c

--- a/main.cpp	Thu Oct 26 22:02:24 2017 +0000
+++ b/main.cpp	Mon Oct 30 15:03:53 2017 +0000
@@ -5,6 +5,7 @@
 //#include "HIDScope.h"
 
 Serial pc (USBTX,USBRX);
+//HIDScope scope(2);
 
 PwmOut Motor1Vel(D6);            //motor 1 velocity control
 DigitalOut Motor1Dir(D7);        //motor 1 direction
@@ -13,12 +14,13 @@
 DigitalOut led1(D2);
 AnalogIn emg1(A0);
 AnalogIn emg2(A1);
-AnalogIn emg3(A3);
+AnalogIn emg3(A2);
 DigitalOut Button(D3);
-//DigitalOut ledb(LED1);
+DigitalOut ledb(LED_BLUE);
+DigitalOut ledg(LED_GREEN);
 
-QEI encoder1(D8,D9,NC,16);      //define encoder pins
-QEI encoder2(D10,D11,NC,16);
+QEI encoder1(D10,D11,NC,16);      //define encoder pins
+QEI encoder2(D8,D9,NC,16);
 
 BiQuad bq1 ( 0.9150 , -1.8299 , 0.9150 , 1.0000 , -1.8227 , 0.8372); //Highpass
 BiQuad bq2 ( 0.0001 ,   0.0002  ,  0.0001  ,  1.0000 ,  -1.9733  ,  0.9737); //Lowpass
@@ -29,26 +31,26 @@
 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
+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.3;
 
-volatile double y=0;
 const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.15*3;   //to convert pulses to radians 
-const float Angle1_0 = pi/2 ,Angle2_0 = 0;    //initial angle of the motors
+const float Angle1_0 = pi/4 ,Angle2_0 = -pi/4;    //initial angle of the motors
 
 //variables and constants for the PID
-const float KP = 10, KI = 5, KD = 2, N = 100;
+const float KP = 200, KI = 0.1, KD = 6, N = 100;
 float M1_v1 = 0, M1_v2 = 0, M2_v1 = 0, M2_v2 = 0;
 
 volatile double a = 0, b = 0, c = 0;      //variables to store the highest emg signal to normalize the data
 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;
-int counter;                      //to count the time of inactivity
+volatile bool ReturnIP = false;
+int counter = 0;                      //to count the time of inactivity
 
-double emg1filt(double X)
+double emg1filt()
 {
     double xtemp=(bq2.step(fabs(bq1.step(emg1))));
     if (xtemp>a)
@@ -58,7 +60,7 @@
     return X;
 }
 
-double emg2filt(double Y1)
+double emg2filt()
 {
     double y1temp = (bq2.step(fabs(bq1.step(emg1))));
     if (y1temp>b)
@@ -68,7 +70,7 @@
     return Y1;
 }
 
-int emg3filt(int Y2)
+int emg3filt()
 {
     double y2temp = (bq2.step(fabs(bq1.step(emg1))));
     if (y2temp>c)
@@ -76,7 +78,7 @@
         c=y2temp;
     }    
     Y2 = y2temp/c;
-    if(Y1 < 0.3)
+    if(Y2 < 0.3)
         Y2 = 1;
     else
         Y2 = -1;
@@ -85,19 +87,18 @@
 
 void callemgfilt()
 {
-    X = emg1filt(X);
-    Y = emg2filt(Y1)*emg3filt(Y2);  // emg2filt gives the amplitude of Y and emg3filt the sign
-
+    X = emg1filt();
+    Y = emg2filt()/**emg3filt()*/;  // emg2filt gives the amplitude of Y and emg3filt the sign    
 }
 
 // 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 a1 = -4/(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);
+    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);
     
     float v = err-a1*v1-a2*v2;
     float u = abs(b0*v+b1*v1+b2*v2);
@@ -105,13 +106,14 @@
     return u;
 }
 
-void positionxy(float Angle1, float Angle2,float &XPos,float &YPos)
+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);
+    YPos = L*sin(Angle1)+L*sin(Angle2); 
+    //pc.printf("Angle1: %f Angle2: %f\r\n",Angle1,Angle2);
 }
 
 int Direction(float err)
@@ -124,7 +126,7 @@
     return a;
 }
 
-void MoveMotors()    // move the motors using the inverse kinematic equations of the robot
+void MoveMotors(float XSet, float YSet)    // 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);
@@ -135,24 +137,36 @@
     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);
+    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 \r\n", Angle1 ,Angle2 );
+    
 }
 
 void GoToSet()
 {   
-    XSet += X;  // keep increasing the set point, depending on the input
-    YSet += Y;
-    positionxy(Angle1,Angle2,XPos,YPos);
-    MoveMotors();    
+    ledg = 0;
+    ledb = 1;
+    /*XSet += X;  // keep increasing the set point, depending on the input
+    YSet += Y;*/
+    XSet = 46;
+    YSet = 0;
+    MoveMotors(XSet,YSet);  
+      
 }
 
 void InitialPosition()  //Go back to the initial position
 {  
+    ledb = 0;
+    ledg = 1;
     const float  AllowedError = 0.5;  
-    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 X0 = 25, X1 = 21.85, X2 = 31.94, slope = -17.34/6.6;  //values of special points in the workspace
     
     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
@@ -184,11 +198,13 @@
         XSet = X0;
         YSet = 0;
     }
-    MoveMotors();
+    MoveMotors(XSet,YSet);
 }
 
-void CallFuncMovement()   //decide which case has to be used
-{                       
+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
@@ -203,9 +219,9 @@
     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;       
+        counter = 0;    
     }
-    else if (counter == 2/Ts)
+    else if (counter >= 200)
     {  //after 2 seconds of inactivity the robot goes back to the initial position
         InitialPosition();
         counter = 0;
@@ -213,7 +229,7 @@
     else if (X > 0 || Y != 0)
     {
         GoToSet(); 
-        counter = 0;   
+        counter = 0; 
     }
     else 
     {
@@ -223,9 +239,11 @@
 
 main()
 {
+    ledb = 1;
+    ledg = 1;
     pc.baud(115200);
-    emg_tick.attach(&callemgfilt,0.001);
+    //emg_tick.attach(&callemgfilt,0.001);
     motors_tick.attach(&CallFuncMovement,Ts);
     while (true)
-    { /*keep the program running*/ }
+    { /*keep the program running*/}
 }
\ No newline at end of file