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:
- 61:157df6f8ceec
- Parent:
- 60:20945383ad1b
- Child:
- 62:6c566e6f9664
diff -r 20945383ad1b -r 157df6f8ceec actuators.cpp --- a/actuators.cpp Mon Oct 12 11:46:05 2015 +0000 +++ b/actuators.cpp Tue Oct 13 18:28:39 2015 +0200 @@ -94,11 +94,11 @@ // get encoder speeds in deg/sec now = t.read(); timechange = (now - prevTime); - motor1Speed = (motor1Pos - motor1PrevPos)/timechange; - motor2Speed = (motor2Pos - motor2PrevPos)/timechange; + motor1Speed = ((((encoder1.getPosition() - motor1PrevPos)/32)/131.25)*360)/timechange; + motor2Speed = ((((encoder2.getPosition() - motor2PrevPos)/32)/131.25)*360)/timechange; prevTime = now; - motor1PrevPos = motor1Pos; - motor2PrevPos = motor2Pos; + motor1PrevPos = encoder1.getPosition(); + motor2PrevPos = encoder2.getPosition(); // calculate motor setpoint speed in deg/sec from setpoint x/y speed @@ -122,11 +122,11 @@ }else{ direction1 = true; // CW rotation } - if (motor2PWM >= 0 ){ // CCW rotation - direction2 = false; - }else{ - direction2 = true; // CW rotation - } + // if (motor2PWM >= 0 ){ // CCW rotation + // direction2 = false; + // }else{ + // direction2 = true; // CW rotation + // } motor1Dir.write(direction1); motor2Dir.write(direction2);