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:
- 107:de47331612d9
- Parent:
- 106:1773bf7b95c5
- Child:
- 109:026abd708dce
diff -r 1773bf7b95c5 -r de47331612d9 actuators.cpp --- a/actuators.cpp Mon Oct 26 12:46:29 2015 +0100 +++ b/actuators.cpp Mon Oct 26 13:04:22 2015 +0100 @@ -4,6 +4,8 @@ #include "config.h" #include "encoder.h" #include "HIDScope.h" +#include "buttons.h" +#include "Servo.h" // Motor control constants #define pwm_frequency 50000 // still High, could be lowered @@ -46,16 +48,9 @@ double prevTime = 0; double now = 0; 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; - double servoSpeed = 0; + const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg // Set calibration values double motorCalSpeed = 10; // deg/sec @@ -74,7 +69,7 @@ PwmOut motor2(motor2PWMPin); // initialize Servo - PwmOut servo(servoPin); + Servo servo(servoPin); // Initialize encoders @@ -111,21 +106,21 @@ motor2PID.SetMode(AUTOMATIC); // set servo period - servo.period(servoPeriod); + servo.Enable(servoStartPos, 20000); - // start the timer t.start(); } void motorControl(){ - // EMG signals to motor speeds - const double scaleVel = 20; - motor1SetSpeed = x_velocity*scaleVel; - motor2SetSpeed = y_velocity*scaleVel; - servoSpeed = z_velocity*scaleVel; - + #ifndef SETPOS // if not tuning with potmeters, switch to EMG + // EMG signals to motor speeds + const double scaleVel = 20; + motor1SetSpeed = x_velocity*scaleVel; + motor2SetSpeed = y_velocity*scaleVel; + servoSpeed = z_velocity*scaleVel; + #endif // get encoder positions in degrees // 131.25:1 gear ratio // getPosition uses X2 configuration, so 32 counts per revolution @@ -188,17 +183,7 @@ } 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) { - 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); + servo.SetPosition(1300 + 700*servoSpeed); } bool calibrateMotors(){ @@ -259,29 +244,29 @@ bool kinematics(){ - // calculate current x and Y - X = L2*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180); - Y = L2*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180); - // check if x and y are within limits - // else Store the constraint line - // check if movement is in direction of constraint - // else return false no movement (anglespeed = 0) - // calculate required angle speeds - if( (X>Xmax && setXSpeed > 0 )|| \ - (X<Xmin && setXSpeed < 0 )|| \ - (Y>Ymax && setYSpeed > 0 )|| \ - (Y<Ymin && setYSpeed < 0 ) \ - ){ - motor1SetSpeed = 0; - motor2SetSpeed = 0; - return false; - break; - } -motor1SetSpeed = (setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \ - setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180)); -motor2SetSpeed = -(setXSpeed*L2*cos((motor1Pos + motor2Pos)*PI/180) + \ - setYSpeed*L2*sin((motor1Pos + motor2Pos)*PI/180) + \ - setXSpeed*L1*cos(motor1Pos*PI/180) + \ - setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*L2*sin(motor2Pos*PI/180)); +// // calculate current x and Y +// X = L2*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180); +// Y = L2*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180); +// // check if x and y are within limits +// // else Store the constraint line +// // check if movement is in direction of constraint +// // else return false no movement (anglespeed = 0) +// // calculate required angle speeds +// if( (X>Xmax && setXSpeed > 0 )|| \ +// (X<Xmin && setXSpeed < 0 )|| \ +// (Y>Ymax && setYSpeed > 0 )|| \ +// (Y<Ymin && setYSpeed < 0 ) \ +// ){ +// motor1SetSpeed = 0; +// motor2SetSpeed = 0; +// return false; +// break; +// } +// motor1SetSpeed = (setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \ +// setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180)); +// motor2SetSpeed = -(setXSpeed*L2*cos((motor1Pos + motor2Pos)*PI/180) + \ +// setYSpeed*L2*sin((motor1Pos + motor2Pos)*PI/180) + \ +// setXSpeed*L1*cos(motor1Pos*PI/180) + \ +// setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*L2*sin(motor2Pos*PI/180)); } \ No newline at end of file