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:
- 5:73bfad06b775
- Parent:
- 4:80e2280058ed
- Child:
- 6:b957d8809e7c
diff -r 80e2280058ed -r 73bfad06b775 actuators.cpp --- a/actuators.cpp Mon Oct 05 16:15:25 2015 +0200 +++ b/actuators.cpp Mon Oct 05 17:35:02 2015 +0200 @@ -25,8 +25,8 @@ } void initPID(){ - // create PID instances for motors - // PID pidname(input, output, setpoint, kp, ki, kd, direction) + // create PID instances for motors + // PID pidname(input, output, setpoint, kp, ki, kd, direction) PID PIDmotor1(&motorSpeed1, &motorPWM1, &motorSetSpeed1, Kp1, Ki1, Kd1, DIRECT); PID PIDmotor2(&motorSpeed2, &motorPWM2, &motorSetSpeed2, Kp2, Ki2, Kd2, DIRECT); @@ -52,15 +52,24 @@ motorSpeed1 = encoder1.getSpeed(); motorSpeed2 = encoder2.getSpeed(); - prevTime = time.read(); // translate to x/y speed // compute new PID parameters using setpoint speeds and x/y speeds PIDmotor1.compute(); PIDmotor2.compute(); // translate to motor rotation speed // write new values to motor's - motor1.write(motorPWM1); - motor2.write(motorPWM2); + if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion) + direction1 = false; + }else{ + direction1 = true; // CW rotation + } + if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion) + direction2 = false; + }else{ + direction2 = true; // CW rotation + } + motor1.write(abs(motorPWM1)); + motor2.write(abs(motorPWM2)); }else{ // write 0 to motors