![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
Diff: main.cpp
- Branch:
- onefile
- Revision:
- 24:2d7e11441eee
- Parent:
- 23:3f5d30b4784d
--- a/main.cpp Mon Oct 05 23:10:31 2015 +0200 +++ b/main.cpp Mon Oct 05 21:46:51 2015 +0000 @@ -4,11 +4,7 @@ #include "PID.h" #include "EMG.h" -//#define DEBUG // send debug data to HIDScope -#ifdef DEBUG - #include "debug.h" - dubugInit(); -#endif + bool motorEnable = false; @@ -38,25 +34,35 @@ float Kd2 = 1; float PIDinterval = 0.2; - - + Encoder encoder1(enc1A, enc1B, true); + Encoder encoder2(enc2A, enc2B, true); + PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); + PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); + PwmOut motor1(motor1PWMPin); + PwmOut motor2(motor2PWMPin); + AnalogIn pot1(pot1Pin); + +void initPID(){ + // create PID instances for motors + // PID pidname(input, output, setpoint, kp, ki, kd, direction) -int main(){ - setPins(); - motorInit(); - while (true) { - checkSwitches(); - // readEMG(); - motorControl(); - // servoControl(); - } + PIDmotor1.setSetPoint(motorSetSpeed1); + PIDmotor2.setSetPoint(motorSetSpeed2); + + PIDmotor1.setProcessValue(motorSpeed1); + PIDmotor2.setProcessValue(motorSpeed2); + // set PID mode + PIDmotor1.setMode(1); + PIDmotor2.setMode(1); + + // set limits for PID output to avoid integrator build up. + PIDmotor1.setOutputLimits(-1.0, 1.0); + PIDmotor2.setOutputLimits(-1.0, 1.0); } - void motorInit(){ // Initialze motors - PwmOut motor1(motor1PWMPin); - PwmOut motor2(motor2PWMPin); + // Set motor direction pins. DigitalOut motor1Dir(motor1DirPin); @@ -71,30 +77,11 @@ motor2.period(1/pwm_frequency); // Initialize encoders (with speed calculation) - Encoder encoder1(enc1A, enc1B, true); - Encoder encoder2(enc2A, enc2B, true); initPID(); } -void initPID(){ - // create PID instances for motors - // PID pidname(input, output, setpoint, kp, ki, kd, direction) - PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); - PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); - PIDmotor1.setSetPoint(motorSetSpeed1); - PIDmotor2.setSetPoint(motorSetSpeed2); - PIDmotor1.setProcessValue(motorSpeed1); - PIDmotor2.setProcessValue(motorSpeed2); - // set PID mode - PIDmotor1.setMode(1); - PIDmotor2.setMode(1); - - // set limits for PID output to avoid integrator build up. - PIDmotor1.setOutputLimits(-1.0, 1.0); - PIDmotor2.setOutputLimits(-1.0, 1.0); -} void motorControl(){ @@ -137,7 +124,7 @@ void setPins(){ // set input/output pins - AnalogIn pot1(pot1Pin); + } @@ -155,4 +142,17 @@ // read killswitches - } \ No newline at end of file + } + +int main(){ + setPins(); + motorInit(); + while (true) { + checkSwitches(); + // readEMG(); + motorControl(); + // servoControl(); + } +} + +