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:
- 25:874675516927
- Parent:
- 22:c562b9a4176d
- Child:
- 26:0a9e4147a31a
diff -r c562b9a4176d -r 874675516927 actuators.cpp --- a/actuators.cpp Mon Oct 05 21:27:52 2015 +0200 +++ b/actuators.cpp Tue Oct 06 14:24:40 2015 +0000 @@ -3,6 +3,7 @@ #include "mbed.h" #include "config.h" #include "encoder.h" +#include "HIDScope.h" // functions for controlling the motors bool motorEnable = false; @@ -33,15 +34,29 @@ float PIDinterval = 0.2; -void motorInit(){ - // Initialze motors + +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); + + +void motorInit(){ + // Initialze motors - // Set motor direction pins. - DigitalOut motor1Dir(motor1DirPin); - DigitalOut motor2Dir(motor2DirPin); + + // Initialize encoders (with speed calculation) + + // Set motor direction pins. + + // Set initial direction motor1Dir.write(direction1); motor2Dir.write(direction2); @@ -50,9 +65,7 @@ motor1.period(1/pwm_frequency); motor2.period(1/pwm_frequency); - // Initialize encoders (with speed calculation) - Encoder encoder1(enc1A, enc1B, true); - Encoder encoder2(enc2A, enc2B, true); + initPID(); } @@ -60,8 +73,8 @@ 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); @@ -76,7 +89,6 @@ PIDmotor2.setOutputLimits(-1.0, 1.0); } - void motorControl(){ if(motorEnable){ // only run motors if switch is enabled @@ -122,4 +134,5 @@ // calculate x y translation of endpoint // find new x and y speed. -} \ No newline at end of file +} +