![](/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:
- 98:25528494287d
- Parent:
- 91:6bbbbc6643c8
- Child:
- 99:7030e9790b1d
diff -r f0ae582fa3e1 -r 25528494287d actuators.cpp --- a/actuators.cpp Thu Oct 22 07:16:59 2015 +0000 +++ b/actuators.cpp Thu Oct 22 10:06:46 2015 +0200 @@ -4,7 +4,7 @@ #include "config.h" #include "encoder.h" #include "HIDScope.h" -#include "Servo.h" + // functions for controlling the motors bool motorsEnable = false; bool safetyOn = false; // start with safety off for calibration @@ -46,6 +46,14 @@ double timechange; bool pidOut = 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; + // Create object instances // Safety Pin DigitalIn safetyIn(safetyPin); @@ -54,6 +62,10 @@ PwmOut motor1(motor1PWMPin); PwmOut motor2(motor2PWMPin); + // initialize Servo + PwmOut servo(servoPin); + + // Initialize encoders Encoder encoder1(enc1A, enc1B); Encoder encoder2(enc2A, enc2B); @@ -66,7 +78,6 @@ PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1); PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2); - Servo servo(servoPin); Timer t; void motorInit(){ @@ -87,6 +98,10 @@ // Turn PID on motor1PID.SetMode(AUTOMATIC); motor2PID.SetMode(AUTOMATIC); + + // set servo period + servo.period(servoPeriod); + // start the timer t.start(); @@ -94,6 +109,11 @@ void motorControl(){ + // EMG signals to motor speeds + motor1SetSpeed = x_velocity; + motor2SetSpeed = y_velocity; + servoSpeed = z_velocity; + // get encoder positions in degrees // 131.25:1 gear ratio // getPosition uses X2 configuration, so 32 counts per revolution @@ -122,6 +142,7 @@ if(motorsEnable){ // only run motors if switch is enabled // compute new PID parameters using setpoint angle speeds and encoder speed writeMotors(); + servoControl(); }else{ // write 0 to motors motor1.write(0); @@ -155,44 +176,62 @@ } void servoControl(){ - // use potMeter Value to set servo angle - if (motorsEnable){ - servo.write(servoPos); + 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) { + if((servo_pos - servo_inc) >= -servo_range) { // If increment step does not exceed maximum range + servo_pos -= servo_inc; + } } - // (optionaly calculate xy position to keep balloon in position) - // calculate z position using angle - // calculate x y translation of endpoint - // find new x and y speed. - + servoPulsewidth = 0.0015 + (servo_pos/90)*0.001; + servo.pulsewidth(servoPulsewidth); } void calibrateMotors(){ -// safetyOn = false; -// bool calibrating1 = true; -// bool calibrating2 = true; -// double motorCalSpeed = 10; // deg/sec -// while (calibrating1 || calibrating2){ -// if (calibrating1){ -// motor1SetSpeed = motorCalSpeed; // deg/sec -// if(safetyIn.read() !=1){ // check if arm reached safety position -// calibrating1 = false; // done -// motor1SetSpeed = 0; // brake motor -// encoder1.setPosition(0); // set motor 1 cal angle -// // move away -// } -// } -// if (calibrating1){ -// motor2SetSpeed = motorCalSpeed; // deg/sec -// if(safetyIn.read() !=1){ // check if arm reached safety position -// calibrating2 = false; // done -// motor2SetSpeed = 0; // brake motor -// encoder2.setPosition(0); // set motor 2 cal angle -// // move away -// } -// } -// writeMotors(); -// wait(0.01f); -// } + 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 }