Diff: main.cpp

Revision:

2:e36213affbda

Parent:

1:14b685c3abbd

Child:

3:03db694efdbe

--- a/main.cpp	Thu Oct 12 21:01:29 2017 +0000
+++ b/main.cpp	Sun Oct 15 14:15:10 2017 +0000
@@ -11,106 +11,135 @@
 PwmOut Motor2Vel(D5);            //motor 2 velocity control
 DigitalOut Motor2Dir(D4);        //motor 2 direction
 DigitalOut led1(D2);
-DigitalOut led2(D3);
-AnalogIn emgx(A0);
-AnalogIn emgy(A1);
-//DigitalOut Button(D10);
+AnalogIn emg1(A0);
+AnalogIn emg2(A1);
+DigitalOut Button(D3);
 //DigitalOut ledb(LED1);
 
 QEI encoder1(D8,D9,NC,16);
 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;    //        ||
+const float PulsesPerRev = 16.0;    //        ||
 const float GearRatio = 131.15;   //        ||
 const float Time = 0.01;          //to control the tickers
 
-float limitx =3, limity = 3, positionx = 1, positiony = 1;
+float R = 160.0;              //radius of the work space
+
 
-int i = 0;                         //counter to go back to initial position if there is no activity
+void positionxy(float Angle1, float Angle2,float &XPosition,float &YPosition)
+{
+    XPosition = L2*cos(Angle1)+L3*cos(Angle2);
+    YPosition = L2*sin(Angle1)+L3*sin(Angle2);
+}
 
-void movex()
-{                            //move motor in the positive x axis
-    led1 = 1;
+void moveXpYp()
+{ 
+    Motor1Vel = emg1*2-1;
+    Motor2Vel = emg2*2-1;
+    Motor1Dir = 1;
+    Motor2Dir = 1; 
 }
-void movey()
-{                            //move motor in the y axis
-    led2 = 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 movexy()
-{                            //move motor in the x and y axis
-    led2 = 1;
-    led1 = 1;
+void moveXnYn()
+{ 
+    Motor1Vel = 1-emg1*2;
+    Motor2Vel = 1-emg2*2;
+    Motor1Dir = 0;
+    Motor2Dir = 0; 
+}
+void StopMotors()
+{
+    Motor1Vel = 0;
+    Motor2Vel = 0;    
 }
-void initialPosition()
-{                             //move the motor back to the initial position
-    led2 = 0;
-    led1 = 0;
-}
-int choosecase(float emgx, float emgy)
+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 (positionx >= limitx || positiony >= limity)  // limit so that the robot does not break. Maybe try approximating with an elipse.
+    if (Button == 1)
+    {
+        // not do anything
+    }
+    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.
+    {
+        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;    
+    }
+    else if (cons1*10 < 5 && cons2*10 >= 5)
     {
         a = 4;
-        i = 0;
-    }
-    else if (emgx > 0 && emgy == 0)
-    {
-        a = 1;
-        i = 0;
-    }
-    else if (emgx == 0 && emgy > 0)
-    {
-        a = 2;
-        i = 0;
-    }
-    else if (emgx > 0 && emgy > 0)
+    } 
+    else if (cons1*10 < 5 && cons2*10 < 5)
     {
-        a = 3;
-        i = 0;
+        a = 5;
     }
-    else if (i > 199)          // after some time of inactivity (2s) the motor goes to initial position
-    {
-        a = 4;
-        i = 0;
-    }
-    else 
-    {
-        i++;
-    }
-    //pc.printf("emgx: %f  emgy: %f \r\n",emgx,emgy);
+    pc.printf("case: %d \r\n",a);
     return a;
 
 }
 void selectcase()
 {                                 //call function to move the motors
-    int switchcons = choosecase(emgx,emgy);
+    int switchcons = choosecase(emg1.read(),emg2.read());
     switch(switchcons)
     {
         case 1:
-            movex();
+            StopMotors();
             break;
         case 2:
-            movey();
+            moveXpYp();
             break;
         case 3:
-            movexy();
+            moveXpYn();
             break;
         case 4:
-            initialPosition();
+            moveXnYp();
+            break;
+        case 5:
+            moveXnYn();
             break;
         default:
             break;
     }
-    pc.printf("Chosen case: %d \r\n",switchcons);
+    
 }
 main()
 {
+    Motor1Dir = 1;
+    Motor2Dir = 1;
     pc.baud(115200);
-    switch_tick.attach(&selectcase,0.01);
-    while (true) {
-
+    switch_tick.attach(&selectcase,Time);
+    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