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:
- 104:750d7e13137d
- Parent:
- 103:4a37d19e8fcc
- Child:
- 105:663b73bb2f81
diff -r 4a37d19e8fcc -r 750d7e13137d actuators.cpp --- a/actuators.cpp Thu Oct 22 14:26:08 2015 +0000 +++ b/actuators.cpp Fri Oct 23 12:17:29 2015 +0000 @@ -6,124 +6,126 @@ #include "HIDScope.h" #include "buttons.h" - // functions for controlling the motors - bool motorsEnable = false; - bool safetyOn = true; // start with safety off for calibration +// functions for controlling the motors +bool motorsEnable = false; +bool safetyOn = true; // start with safety off for calibration - double encoder1Counts = 0; - double encoder2Counts = 0; - - bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) - bool direction2 = false; +double encoder1Counts = 0; +double encoder2Counts = 0; - double motor1Pos = 0; - double motor2Pos = 0; +bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) +bool direction2 = false; - double motor1Speed = 0; - double motor2Speed = 0; +double motor1Pos = 0; +double motor2Pos = 0; - double motor1SetSpeed = 0; - double motor2SetSpeed = 0; +double motor1Speed = 0; +double motor2Speed = 0; - double servoPos = 0; - - double motor1PWM = 0; - double motor2PWM = 0; +double motor1SetSpeed = 0; +double motor2SetSpeed = 0; - // Set PID values - double Kp1 = 0.008; - double Ki1 = 0.08; - double Kd1 = 0; +double servoPulsewidth; +double servoPos = 0; - double Kp2 = 0.008; - double Ki2 = 0.08; - double Kd2 = 0; +double motor1PWM = 0; +double motor2PWM = 0; - double motor1PrevCounts = 0; - double motor2PrevCounts = 0; - double prevTime = 0; - double now = 0; - double timechange; - bool pidOut = 0; +// Set PID values +double Kp1 = 0.008; +double Ki1 = 0.08; +double Kd1 = 0; + +double Kp2 = 0.008; +double Ki2 = 0.08; +double Kd2 = 0; - // Set servo values - const double servoPeriod = 0.020; - const double servo_range = 20; // Servo range (-range/ range) [deg] - const double servo_vel = 15; // Servo velocity [deg/s] - const double servo_inc = servo_vel * motorCall; // Servo postion increment per cycle - double servo_pos = 0; - double servoPulsewidth = 0.0015; - double servoSpeed = 15; - double scaleXSpeed = 10; - double scaleYSpeed = 20; - double scaleZSpeed = 1; - - - - // Set calibration values - double motorCalSpeed = 10; // deg/sec - double returnSpeed = -10; - bool springHit = false; - float lastCall = 0; - bool calibrating1 = true; - bool calibrating2 = false; +double motor1PrevCounts = 0; +double motor2PrevCounts = 0; +double prevTime = 0; +double now = 0; +double timechange; +bool pidOut = 0; - // Create object instances - // Safety Pin - DigitalIn safetyIn(safetyPin); - - // Initialze motors - PwmOut motor1(motor1PWMPin); - PwmOut motor2(motor2PWMPin); - - // initialize Servo - PwmOut servo(servoPin); +// Set servo values +const double servoPeriod = 0.020; +const double servo_range = 20; // Servo range (-range/ range) [deg] +const double servo_vel = 15; // Servo velocity [deg/s] +const double servo_inc = servo_vel * servoCall; // Servo postion increment per cycle +double servo_pos = 0; +double servoSpeed = -1; +double scaleXSpeed = 10; +double scaleYSpeed = 20; +double scaleZSpeed = 1; - // Initialize encoders - Encoder encoder1(enc1A, enc1B); - Encoder encoder2(enc2A, enc2B); - // Set direction pins - DigitalOut motor1Dir(motor1DirPin); - DigitalOut motor2Dir(motor2DirPin); +// Set calibration values +double motorCalSpeed = 10; // deg/sec +double returnSpeed = -10; +bool springHit = false; +float lastCall = 0; +bool calibrating1 = true; +bool calibrating2 = false; + +// Create object instances +// Safety Pin +DigitalIn safetyIn(safetyPin); + +// Initialze motors +PwmOut motor1(motor1PWMPin); +PwmOut motor2(motor2PWMPin); - // create PID instances - PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1); - PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2); +// initialize Servo +PwmOut servo(servoPin); + + +// Initialize encoders +Encoder encoder1(enc1A, enc1B); +Encoder encoder2(enc2A, enc2B); - Timer t; - -void motorInit(){ - +// Set direction pins +DigitalOut motor1Dir(motor1DirPin); +DigitalOut motor2Dir(motor2DirPin); + +// create PID instances +PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1); +PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2); + +Timer t; + +void motorInit() +{ + motor1Dir.write(direction1); motor2Dir.write(direction2); // Set motor PWM period motor1.period(1/pwm_frequency); motor2.period(1/pwm_frequency); - + motor1PID.SetSampleTime(motorCall); motor2PID.SetSampleTime(motorCall); - + motor1PID.SetOutputLimits(0,1); motor2PID.SetOutputLimits(0,1); - + // Turn PID on motor1PID.SetMode(AUTOMATIC); motor2PID.SetMode(AUTOMATIC); // set servo period - servo.period(servoPeriod); + servo.period(servoPeriod); - + // start the timer t.start(); } -void motorControl(){ +void motorControl() +{ // EMG signals to motor speeds // motor1SetSpeed = x_velocity*scaleXSpeed; // motor2SetSpeed = y_velocity*scaleYSpeed; @@ -131,131 +133,146 @@ // get encoder positions in degrees - // 131.25:1 gear ratio - // getPosition uses X2 configuration, so 32 counts per revolution - // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive + // 131.25:1 gear ratio + // getPosition uses X2 configuration, so 32 counts per revolution + // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive - encoder1Counts = encoder1.getPosition(); - encoder2Counts = encoder2.getPosition(); + encoder1Counts = encoder1.getPosition(); + encoder2Counts = encoder2.getPosition(); - motor1Pos = -((encoder1Counts/32)/131.25)*360; - motor2Pos = -((encoder2Counts/32)/131.25)*360; + motor1Pos = -((encoder1Counts/32)/131.25)*360; + motor2Pos = -((encoder2Counts/32)/131.25)*360; - // check if motor's are within rotational boundarys + // check if motor's are within rotational boundarys // get encoder speeds in deg/sec - now = t.read(); - timechange = (now - prevTime); - motor1Speed = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange; - motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange; - prevTime = now; - motor1PrevCounts = encoder1Counts; - motor2PrevCounts = encoder2Counts; - + now = t.read(); + timechange = (now - prevTime); + motor1Speed = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange; + motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange; + prevTime = now; + motor1PrevCounts = encoder1Counts; + motor2PrevCounts = encoder2Counts; + // calculate motor setpoint speed in deg/sec from setpoint x/y speed - - if(motorsEnable){ // only run motors if switch is enabled - // compute new PID parameters using setpoint angle speeds and encoder speed + + if(motorsEnable) { // only run motors if switch is enabled + // compute new PID parameters using setpoint angle speeds and encoder speed writeMotors(); - servoControl(); - }else{ + + } else { // write 0 to motors motor1.write(0); motor2.write(0); } } -void writeMotors(){ +void writeMotors() +{ motor1PID.Compute(); // calculate PID outputs, output changes automatically motor2PID.Compute(); // write new values to motor's - if (motor1SetSpeed > 0 ){ // CCW rotation + if (motor1SetSpeed > 0 ) { // CCW rotation direction1 = false; motor1PID.SetOutputLimits(0,1); // change pid output direction - }else{ + } else { direction1 = true; // CW rotation motor1PID.SetOutputLimits(-1,0); } - if (motor2SetSpeed > 0 ){ // CCW rotation + if (motor2SetSpeed > 0 ) { // CCW rotation direction2 = false; motor2PID.SetOutputLimits(0,1); - }else{ + } else { direction2 = true; // CW rotation motor2PID.SetOutputLimits(-1,0); } motor1Dir.write(direction1); motor2Dir.write(direction2); - motor1.write(abs(motor1PWM)); - motor2.write(abs(motor2PWM)); + // motor1.write(abs(motor1PWM)); + // motor2.write(abs(motor2PWM)); } -void servoControl(){ +void servoControl() +{ if (servoSpeed > 0) { if((servo_pos + servo_inc) <= servo_range) { // If increment step does not exceed maximum range servo_pos += servo_inc; } - }else if (servoSpeed < 0) { + } else if (servoSpeed < 0) { if((servo_pos - servo_inc) >= -servo_range) { // If increment step does not exceed maximum range servo_pos -= servo_inc; } } - servoPulsewidth = 0.0015 + (servo_pos/90)*0.001; - servo.pulsewidth(servoPulsewidth); + servoPulsewidth = 0.0015 + (servo_pos/90)*0.001; + if(motorsEnable) { + servo.pulsewidth(servoPulsewidth); + } else if (!motorsEnable) { + servo.pulsewidth(0.0017); + } } -void calibrateMotors(){ - safetyOn = false; // safety springs off - motorsEnable = true; // motors on - redLed.write(0); greenLed.write(1); blueLed.write(1); - while (calibrating1 || calibrating2){ - if (calibrating1){ - redLed.write(1); greenLed.write(0); blueLed.write(1); - if(safetyIn.read() !=1){ // check if arm reached safety position - encoder1.setPosition(0); // set motor 1 cal angle - motor1SetSpeed = returnSpeed; // move away - springHit = true; - }else{ - if(springHit){ // if hit and after is no longer touching spring - motor1SetSpeed = 0; - springHit = false; - calibrating1 = false; - calibrating2 = true; // start calibrating 2 - } - } - } - if (calibrating2){ - motor2SetSpeed = motorCalSpeed; - redLed.write(1); greenLed.write(1); blueLed.write(0); - if(safetyIn.read() !=1){ // check if arm reached safety position - encoder2.setPosition(0); // set motor 2 cal angle - motor2SetSpeed = returnSpeed; // move away - springHit = true; - }else{ - if(springHit){ // if hit and after is no longer touching spring - motor2SetSpeed = 0; - springHit = false; - calibrating2 = false; // stop calibrating 2 - } - } - } - now = t.read(); // call motor using timer instead of wait - if(now - lastCall > motorCall){ - motorControl(); - lastCall = now; - } +void calibrateMotors() +{ + safetyOn = false; // safety springs off + motorsEnable = true; // motors on + redLed.write(0); + greenLed.write(1); + blueLed.write(1); + while (calibrating1 || calibrating2) { + if (calibrating1) { + redLed.write(1); + greenLed.write(0); + blueLed.write(1); + if(safetyIn.read() !=1) { // check if arm reached safety position + encoder1.setPosition(0); // set motor 1 cal angle + motor1SetSpeed = returnSpeed; // move away + springHit = true; + } else { + if(springHit) { // if hit and after is no longer touching spring + motor1SetSpeed = 0; + springHit = false; + calibrating1 = false; + calibrating2 = true; // start calibrating 2 + } + } + } + if (calibrating2) { + motor2SetSpeed = motorCalSpeed; + redLed.write(1); + greenLed.write(1); + blueLed.write(0); + if(safetyIn.read() !=1) { // check if arm reached safety position + encoder2.setPosition(0); // set motor 2 cal angle + motor2SetSpeed = returnSpeed; // move away + springHit = true; + } else { + if(springHit) { // if hit and after is no longer touching spring + motor2SetSpeed = 0; + springHit = false; + calibrating2 = false; // stop calibrating 2 + } + } + } + now = t.read(); // call motor using timer instead of wait + if(now - lastCall > motorCall) { + motorControl(); + lastCall = now; + } - } + } motorsEnable = false; // turn motor's off again safetyOn = true; // turn safety on after callibration } -void safety(){ - if (safetyOn){ - if (safetyIn.read() != 1){ +void safety() +{ + if (safetyOn) { + if (safetyIn.read() != 1) { motorsEnable = false; + redLed.write(!redLed.read()); } } }