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:
- 69:37f75a7d36d8
- Parent:
- 68:21cb054f1399
- Child:
- 74:75be98779124
diff -r 21cb054f1399 -r 37f75a7d36d8 actuators.cpp --- a/actuators.cpp Thu Oct 15 16:15:05 2015 +0200 +++ b/actuators.cpp Thu Oct 15 18:52:09 2015 +0200 @@ -33,8 +33,8 @@ double Ki1 = 0; double Kd1 = 0; - double Kp2 = 0.0184; - double Ki2 = 0.32; + double Kp2 = 0.008; + double Ki2 = 0.08; double Kd2 = 0; double motor1PrevCounts = 0; @@ -76,6 +76,9 @@ motor1PID.SetSampleTime(motorCall); motor2PID.SetSampleTime(motorCall); + motor1PID.SetOutputLimits(0,1); + motor2PID.SetOutputLimits(0,1); + // Turn PID on motor1PID.SetMode(AUTOMATIC); motor2PID.SetMode(AUTOMATIC); @@ -125,12 +128,12 @@ motor1PID.Compute(); // calculate PID outputs, output changes automatically pidOut = motor2PID.Compute(); // write new values to motor's - if (motor1PWM >= 0 ){ // CCW rotation + if (motor1SetSpeed >= 0 ){ // CCW rotation direction1 = false; }else{ direction1 = true; // CW rotation } - if (motor2PWM >= 0 ){ // CCW rotation + if (motor2SetSpeed >= 0 ){ // CCW rotation direction2 = false; }else{ direction2 = true; // CW rotation @@ -138,8 +141,10 @@ motor1Dir.write(direction1); motor2Dir.write(direction2); + double motor2FF = 0.0012*motor2SetSpeed+0.02; + motor1.write(abs(motor1PWM)); - motor2.write(abs(motor2PWM)); + motor2.write(abs(motor2PWM)+motor2FF); } void servoControl(){