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:
- 62:6c566e6f9664
- Parent:
- 61:157df6f8ceec
- Child:
- 66:d1ab5904f8e5
diff -r 157df6f8ceec -r 6c566e6f9664 actuators.cpp --- a/actuators.cpp Tue Oct 13 18:28:39 2015 +0200 +++ b/actuators.cpp Wed Oct 14 13:52:16 2015 +0200 @@ -11,34 +11,34 @@ bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) bool direction2 = false; - float motor1Pos = 0; - float motor2Pos = 0; + double motor1Pos = 0; + double motor2Pos = 0; - float motor1Speed = 0; - float motor2Speed = 0; + double motor1Speed = 0; + double motor2Speed = 0; - float motor1SetSpeed = 0; - float motor2SetSpeed = 0; + double motor1SetSpeed = 0; + double motor2SetSpeed = 0; - float servoPos = 0; + double servoPos = 0; - float motor1PWM = 0; - float motor2PWM = 0; + double motor1PWM = 0; + double motor2PWM = 0; // Set PID values - float Kp1 = 1; - float Ki1 = 0; - float Kd1 = 0; + double Kp1 = 1; + double Ki1 = 0; + double Kd1 = 0; - float Kp2 = 1; - float Ki2 = 0; - float Kd2 = 0; + double Kp2 = 1; + double Ki2 = 0; + double Kd2 = 0; - float motor1PrevPos = 0; - float motor2PrevPos = 0; - float prevTime = 0; - float now = 0; - float timechange; + double motor1PrevPos = 0; + double motor2PrevPos = 0; + double prevTime = 0; + double now = 0; + double timechange; bool pidOut = 0; // Create object instances @@ -94,8 +94,8 @@ // get encoder speeds in deg/sec now = t.read(); timechange = (now - prevTime); - motor1Speed = ((((encoder1.getPosition() - motor1PrevPos)/32)/131.25)*360)/timechange; - motor2Speed = ((((encoder2.getPosition() - motor2PrevPos)/32)/131.25)*360)/timechange; + motor1Speed = -((((encoder1.getPosition() - motor1PrevPos)/32)/131.25)*360)/timechange; + motor2Speed = -((((encoder2.getPosition() - motor2PrevPos)/32)/131.25)*360)/timechange; prevTime = now; motor1PrevPos = encoder1.getPosition(); motor2PrevPos = encoder2.getPosition();