Diff: actuators.cpp

Revision:

121:6d8f1bdcda05

Parent:

120:440f1516101b

Child:

122:1a5df0765790

Child:

124:f67ce69557db

--- a/actuators.cpp	Wed Oct 28 11:22:48 2015 +0000
+++ b/actuators.cpp	Wed Oct 28 13:38:36 2015 +0100
@@ -268,22 +268,24 @@
 const double Xmin = -0.3;
 const double Ymax = 0.645;
 const double Ymin = 0.33;
+double Xpos = 0; // set values
+double Ypos =0;
 
 
 
 bool kinematics(){
     // calculate current x and Y
-    double X = (L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
-    double Y = (L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180);
+    Xpos = (L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
+    Ypos = (L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180);
     // check if x and y are within limits
     //  else  Store the constraint line
     //      check if movement is in direction of constraint
     //      else return false no movement (anglespeed = 0)
     // calculate required angle speeds
-    if( (X>Xmax && setXSpeed > 0 )|| \
-        (X<Xmin && setXSpeed < 0 )|| \
-        (Y>Ymax && setYSpeed > 0 )|| \
-        (Y<Ymin && setYSpeed < 0 )   \
+    if( (Xpos>Xmax && setXSpeed > 0 )|| \
+        (Xpos<Xmin && setXSpeed < 0 )|| \
+        (Ypos>Ymax && setYSpeed > 0 )|| \
+        (Ypos<Ymin && setYSpeed < 0 )   \
     ){
         motor1SetSpeed = 0;
         motor2SetSpeed = 0;