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:
- 84:df770ec4df61
- Parent:
- 83:8fa05f53fc73
- Child:
- 85:03dc24043236
- Child:
- 91:6bbbbc6643c8
diff -r 8fa05f53fc73 -r df770ec4df61 actuators.cpp --- a/actuators.cpp Tue Oct 20 14:58:55 2015 +0200 +++ b/actuators.cpp Tue Oct 20 15:07:51 2015 +0200 @@ -133,13 +133,15 @@ motor1PID.Compute(); // calculate PID outputs, output changes automatically motor2PID.Compute(); // write new values to motor's + if (motor1SetSpeed=0){direction1=!direction1;} // change direction to cause braking. if (motor1SetSpeed > 0 ){ // CCW rotation direction1 = false; - motor1PID.SetOutputLimits(0,1); + motor1PID.SetOutputLimits(0,1); // change pid output direction }else{ direction1 = true; // CW rotation motor1PID.SetOutputLimits(-1,0); } + if (motor2SetSpeed=0){direction2=!direction2;} // change direction to cause braking. if (motor2SetSpeed > 0 ){ // CCW rotation direction2 = false; motor2PID.SetOutputLimits(0,1); @@ -167,36 +169,36 @@ } void calibrateMotors(){ - bool calibrating1 = true; - bool calibrating2 = true; - float motorCalSpeed = 0.1; // %PWM - motor1Dir.write(false); - motor2Dir.write(false); - // move motors CCW until they reach outer most position - // setmotorspeed contant - // first move motor 1 using PID - // wait until motor hits pin - // switch direction - // set motor speed1 0 - // repeat for motor 2 - while (calibrating1 || calibrating2){ - if(motor1AnglePin == 0){ - motor1.write(0.3f); - }else{ - motor1.write(0); - calibrating1 = false; - } - if(motor2AnglePin == 0){ - motor2.write(0.3f); - }else{ - motor2.write(0); - calibrating2 = false; - } - wait(0.2f); - } - // set the encoder values for angle. - encoder1.setValue(0); - encoder2.setValue(0); + // bool calibrating1 = true; + // bool calibrating2 = true; + // float motorCalSpeed = 0.1; // %PWM + // motor1Dir.write(false); + // motor2Dir.write(false); + // // move motors CCW until they reach outer most position + // // setmotorspeed contant + // // first move motor 1 using PID + // // wait until motor hits pin + // // switch direction + // // set motor speed1 0 + // // repeat for motor 2 + // while (calibrating1 || calibrating2){ + // if(motor1AnglePin == 0){ + // motor1.write(0.3f); + // }else{ + // motor1.write(0); + // calibrating1 = false; + // } + // if(motor2AnglePin == 0){ + // motor2.write(0.3f); + // }else{ + // motor2.write(0); + // calibrating2 = false; + // } + // wait(0.2f); + // } + // // set the encoder values for angle. + // encoder1.setValue(0); + // encoder2.setValue(0); safetyOn = true; // turn safety on after callibration } @@ -205,6 +207,8 @@ if (safetyOn){ if (safetyIn.read() != 1){ motorsEnable = false; + motor1Dir.write(!direction1); + motor2Dir.write(!direction2); } } }