Diff: main.cpp

Revision:

6:acc2669ab20c

Parent:

5:77ffa336d6eb

Child:

7:75d2244d7745

--- a/main.cpp	Mon Oct 30 15:03:53 2017 +0000
+++ b/main.cpp	Mon Oct 30 15:32:52 2017 +0000
@@ -34,10 +34,10 @@
 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;
+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 Angle1_0 = pi/4 ,Angle2_0 = pi/4;    //initial angle of the motors
 
 //variables and constants for the PID
 const float KP = 200, KI = 0.1, KD = 6, N = 100;
@@ -108,8 +108,8 @@
 
 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;
+    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); 
@@ -145,7 +145,7 @@
     Motor1Dir = DIRECTION1;
     Motor2Vel = VELOCITY2;
     Motor2Dir = DIRECTION2;
-    pc.printf(" Angle1: %f Angle2: %f \r\n", Angle1 ,Angle2 );
+    pc.printf("Angle1: %f Angle2: %f XPos: %f YPos: %f \r\n", Angle1 ,Angle2, XPos, YPos );
     
 }