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:
- 68:21cb054f1399
- Parent:
- 66:d1ab5904f8e5
- Child:
- 69:37f75a7d36d8
diff -r 68032a2382c2 -r 21cb054f1399 actuators.cpp --- a/actuators.cpp Thu Oct 15 16:08:03 2015 +0200 +++ b/actuators.cpp Thu Oct 15 16:15:05 2015 +0200 @@ -8,6 +8,9 @@ // functions for controlling the motors bool motorsEnable = false; + double encoder1Counts = 0; + double encoder2Counts = 0; + bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) bool direction2 = false; @@ -34,8 +37,8 @@ double Ki2 = 0.32; double Kd2 = 0; - double motor1PrevPos = 0; - double motor2PrevPos = 0; + double motor1PrevCounts = 0; + double motor2PrevCounts = 0; double prevTime = 0; double now = 0; double timechange; @@ -87,18 +90,23 @@ // 131.25:1 gear ratio // getPosition uses X2 configuration, so 32 counts per revolution // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive - motor1Pos = -((encoder1.getPosition()/32)/131.25)*360; - motor2Pos = -((encoder2.getPosition()/32)/131.25)*360; + + encoder1Counts = encoder1.getPosition(); + encoder2Counts = encoder2.getPosition(); + + + motor1Pos = -((encoder1Counts/32)/131.25)*360; + motor2Pos = -((encoder2Counts/32)/131.25)*360; // check if motor's are within rotational boundarys // 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 = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange; + motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange; prevTime = now; - motor1PrevPos = encoder1.getPosition(); - motor2PrevPos = encoder2.getPosition(); + motor1PrevCounts = encoder1Counts; + motor2PrevCounts = encoder2Counts; // calculate motor setpoint speed in deg/sec from setpoint x/y speed @@ -122,16 +130,16 @@ }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); motor1.write(abs(motor1PWM)); - motor2.write(abs(motor2SetSpeed)/300); + motor2.write(abs(motor2PWM)); } void servoControl(){