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:
- 123:815ace09b753
- Parent:
- 122:1a5df0765790
diff -r 1a5df0765790 -r 815ace09b753 actuators.cpp --- a/actuators.cpp Thu Oct 29 12:26:50 2015 +0000 +++ b/actuators.cpp Thu Oct 29 14:41:54 2015 +0000 @@ -38,8 +38,8 @@ double motor2PWM = 0; // Set PID values -double Kp1 = 1; -double Ki1 = 0; +double Kp1 = 0.008; +double Ki1 = 0.08; double Kd1 = 0; double Kp2 = 0.008; @@ -140,16 +140,13 @@ // calculate motor setpoint speed in deg/sec from setpoint x/y speed // exclude kinematics when still calibrating -// if (calReady){ -// kinematics(); -// } + if (calReady){ + kinematics(); + } if(motorsEnable){ // only run motors if switch is enabled // compute new PID parameters using setpoint angle speeds and encoder speed - motor1Dir.write(true); - motor2Dir.write(true); - motor1.write(0); - motor2.write(0.05f); + writeMotors(); servoControl(); }else{ // write 0 to motors @@ -162,6 +159,9 @@ motor1PID.Compute(); // calculate PID outputs, output changes automatically motor2PID.Compute(); // write new values to motor's + if (motor1SetSpeed ==0 ){ + motor1PID.SetOutputLimits(0,0); + } if (motor1SetSpeed > 0 ){ // CCW rotation direction1 = true; motor1PID.SetOutputLimits(0,1); // change pid output direction @@ -169,6 +169,9 @@ direction1 = false; // CW rotation motor1PID.SetOutputLimits(-1,0); } + if (motor2SetSpeed ==0 ){ + motor2PID.SetOutputLimits(0,0); + } if (motor2SetSpeed > 0 ){ // CCW rotation direction2 = true; motor2PID.SetOutputLimits(0,1);