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:
- 31:8fbee6c92753
- Parent:
- 30:a20f16bf8dda
- Child:
- 32:2006977785f5
diff -r a20f16bf8dda -r 8fbee6c92753 actuators.cpp --- a/actuators.cpp Tue Oct 06 21:06:20 2015 +0000 +++ b/actuators.cpp Wed Oct 07 15:09:52 2015 +0200 @@ -4,6 +4,7 @@ #include "config.h" #include "encoder.h" #include "HIDScope.h" +#include "servo.h" // functions for controlling the motors bool motorEnable = true; @@ -56,6 +57,8 @@ // create PID instances PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); + +servo Servo(servoPin); Timer t; void motorInit(){ @@ -74,10 +77,10 @@ PIDmotor2.setProcessValue(motorSpeed2); // set limits for PID output to avoid integrator build up. - PIDmotor1.setInputLimits(-100, 300); - PIDmotor2.setInputLimits(-100,300); - PIDmotor1.setOutputLimits(-100, 300); - PIDmotor2.setOutputLimits(-100, 300); + PIDmotor1.setInputLimits(-300, 300); + PIDmotor2.setInputLimits(-300, 300); + PIDmotor1.setOutputLimits(-1, 1); + PIDmotor2.setOutputLimits(-1, 1); // Turn PID on PIDmotor1.setMode(1); @@ -107,36 +110,10 @@ prevMotor1Pos = motor1Pos; prevMotor2Pos = motor2Pos; - - // translate to x/y speed - // compute new PID parameters using setpoint speeds and x/y speeds - PIDmotor1.setSetPoint(motorSetSpeed1); - PIDmotor2.setSetPoint(motorSetSpeed2); + // calculate motor setpoint speed in deg/sec from setpoint x/y speed - PIDmotor1.setProcessValue(motorSpeed1); - PIDmotor2.setProcessValue(motorSpeed2); - - motorPWM1 = PIDmotor1.compute(); - motorPWM2 = PIDmotor2.compute(); - // translate to motor rotation speed - // write new values to motor's - if (motorPWM1 > 0 ){ // CCW rotation - direction1 = false; - }else{ - direction1 = true; // CW rotation - } - if (motorPWM2 > 0 ){ // CCW rotation - direction2 = false; - }else{ - direction2 = true; // CW rotation - } - motor1Dir.write(direction1); - motor2Dir.write(direction2); - - motor1.write(abs(motorPWM1)/300); - motor2.write(abs(motorPWM2)/300); -// motor2.write(motorSetSpeed2); - pidError = PIDmotor2.getError(); + // compute new PID parameters using setpoint speeds and x/y speeds + writeMotors(); }else{ // write 0 to motors @@ -145,8 +122,36 @@ } } +void writeMotors(){ + PIDmotor1.setSetPoint(motorSetSpeed1); + PIDmotor2.setSetPoint(motorSetSpeed2); + + PIDmotor1.setProcessValue(motorSpeed1); + PIDmotor2.setProcessValue(motorSpeed2); + + motorPWM1 = PIDmotor1.compute(); + motorPWM2 = PIDmotor2.compute(); +// write new values to motor's + if (motorPWM1 > 0 ){ // CCW rotation + direction1 = false; + }else{ + direction1 = true; // CW rotation + } + if (motorPWM2 > 0 ){ // CCW rotation + direction2 = false; + }else{ + direction2 = true; // CW rotation + } + motor1Dir.write(direction1); + motor2Dir.write(direction2); + + motor1.write(abs(motorPWM1)/300); + motor2.write(abs(motorPWM2)/300); +} + void servoControl(){ // use potMeter Value to set servo angle + Servo.write(servoPos); // (optionaly calculate xy position to keep balloon in position) // calculate z position using angle // calculate x y translation of endpoint