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:
- 26:0a9e4147a31a
- Parent:
- 25:874675516927
- Child:
- 28:40a931dfe840
diff -r 874675516927 -r 0a9e4147a31a actuators.cpp --- a/actuators.cpp Tue Oct 06 14:24:40 2015 +0000 +++ b/actuators.cpp Tue Oct 06 16:37:07 2015 +0200 @@ -5,7 +5,7 @@ #include "encoder.h" #include "HIDScope.h" // functions for controlling the motors -bool motorEnable = false; +bool motorEnable = true; bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) bool direction2 = false; @@ -35,29 +35,26 @@ float PIDinterval = 0.2; +// Create object instances +// Initialze motors +PwmOut motor1(motor1PWMPin); +PwmOut motor2(motor2PWMPin); + +// Initialize encoders Encoder encoder1(enc1A, enc1B, true); Encoder encoder2(enc2A, enc2B, true); - PwmOut motor1(motor1PWMPin); - PwmOut motor2(motor2PWMPin); - - DigitalOut motor1Dir(motor1DirPin); - DigitalOut motor2Dir(motor2DirPin); - PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); - PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); +// Set direction pins +DigitalOut motor1Dir(motor1DirPin); +DigitalOut motor2Dir(motor2DirPin); + +// create PID instances +PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); +PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); void motorInit(){ - // Initialze motors - - - // Initialize encoders (with speed calculation) - - - // Set motor direction pins. - - // Set initial direction motor1Dir.write(direction1); motor2Dir.write(direction2); @@ -65,16 +62,6 @@ motor1.period(1/pwm_frequency); motor2.period(1/pwm_frequency); - - - initPID(); -} - -void initPID(){ - // create PID instances for motors - // PID pidname(input, output, setpoint, kp, ki, kd, direction) - - PIDmotor1.setSetPoint(motorSetSpeed1); PIDmotor2.setSetPoint(motorSetSpeed2); @@ -87,8 +74,10 @@ // set limits for PID output to avoid integrator build up. PIDmotor1.setOutputLimits(-1.0, 1.0); PIDmotor2.setOutputLimits(-1.0, 1.0); + } + void motorControl(){ if(motorEnable){ // only run motors if switch is enabled