![](/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: actuators.cpp
- Revision:
- 32:2006977785f5
- Parent:
- 31:8fbee6c92753
- Child:
- 34:f315b2b38555
diff -r 8fbee6c92753 -r 2006977785f5 actuators.cpp --- a/actuators.cpp Wed Oct 07 15:09:52 2015 +0200 +++ b/actuators.cpp Wed Oct 07 20:55:28 2015 +0200 @@ -5,61 +5,61 @@ #include "encoder.h" #include "HIDScope.h" #include "servo.h" -// functions for controlling the motors -bool motorEnable = true; + // functions for controlling the motors + bool motorEnable = true; -bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) -bool direction2 = false; + bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) + bool direction2 = false; -float motor1Pos = 0; -float motor2Pos = 0; + float motor1Pos = 0; + float motor2Pos = 0; -float motorSpeed1 = 0; -float motorSpeed2 = 0; + float motorSpeed1 = 0; + float motorSpeed2 = 0; -float motorSetSpeed1 = 0; -float motorSetSpeed2 = 0; + float motorSetSpeed1 = 0; + float motorSetSpeed2 = 0; - -float motorPWM1 = 0; -float motorPWM2 = 0; + float servoPos = 0; -// Set PID values -float Kp1 = 1; -float Ki1 = 0; -float Kd1 = 0; + float motorPWM1 = 0; + float motorPWM2 = 0; -float Kp2 = 1; -float Ki2 = 0; -float Kd2 = 0; + // Set PID values + float Kp1 = 1; + float Ki1 = 0; + float Kd1 = 0; -float PIDinterval = 0.2; + float Kp2 = 1; + float Ki2 = 0; + float Kd2 = 0; -float prevMotor2Pos = 0; -float prevMotor1Pos = 0; -float prevTime = 0; -float curTime = 0; + float PIDinterval = 0.2; -float pidError = 0; -// Create object instances -// Initialze motors -PwmOut motor1(motor1PWMPin); -PwmOut motor2(motor2PWMPin); + float prevMotor2Pos = 0; + float prevMotor1Pos = 0; + float prevTime = 0; + float curTime = 0; -// Initialize encoders -Encoder encoder1(enc1A, enc1B); -Encoder encoder2(enc2A, enc2B); + // Create object instances + // Initialze motors + PwmOut motor1(motor1PWMPin); + PwmOut motor2(motor2PWMPin); + + // Initialize encoders + Encoder encoder1(enc1A, enc1B); + Encoder encoder2(enc2A, enc2B); -// Set direction pins -DigitalOut motor1Dir(motor1DirPin); -DigitalOut motor2Dir(motor2DirPin); + // Set direction pins + DigitalOut motor1Dir(motor1DirPin); + DigitalOut motor2Dir(motor2DirPin); -// create PID instances -PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); -PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); + // create PID instances + PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); + PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); -servo Servo(servoPin); -Timer t; + servo Servo(servoPin); + Timer t; void motorInit(){ @@ -93,7 +93,6 @@ void motorControl(){ if(motorEnable){ // only run motors if switch is enabled - // get encoder positions in degrees // 131.25:1 gear ratio // getPosition uses X2 configuration, so 32 counts per revolution @@ -112,7 +111,7 @@ // calculate motor setpoint speed in deg/sec from setpoint x/y speed - // compute new PID parameters using setpoint speeds and x/y speeds + // compute new PID parameters using setpoint angle speeds and encoder speed writeMotors(); }else{ @@ -145,8 +144,8 @@ motor1Dir.write(direction1); motor2Dir.write(direction2); - motor1.write(abs(motorPWM1)/300); - motor2.write(abs(motorPWM2)/300); + motor1.write(abs(motorPWM1)); + motor2.write(abs(motorPWM2)); } void servoControl(){ @@ -159,3 +158,28 @@ } +void calibrateMotors(){ + // bool calibrating1 = true; + // bool calibrating2 = true; + // motor1Dir.write(false); + // motor2Dir.write(false); + // // move motors CCW until they reach outer most position + // while (calibrating1 || calibrating2){ + // if(motor1AnglePin == 0){ + // motor1.write(0.3f); + // }else{ + // motor1.write(0); + // calibrating1 = false; + // } + // if(motor2AnglePin == 0){ + // motor2.write(0.3f); + // }else{ + // motor2.write(0); + // calibrating2 = false; + // } + // wait(0.2f); + // } + // // set the encoder values for angle. + // encoder1.setValue(0); + // encoder2.setValue(0); +} \ No newline at end of file