control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
Diff: actuators.cpp
- Revision:
- 121:6d8f1bdcda05
- Parent:
- 120:440f1516101b
- Child:
- 122:1a5df0765790
- Child:
- 124:f67ce69557db
diff -r 440f1516101b -r 6d8f1bdcda05 actuators.cpp --- 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;