![](/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:
- 109:026abd708dce
- Parent:
- 107:de47331612d9
- Child:
- 110:a6439e13be8b
--- a/actuators.cpp Mon Oct 26 12:41:37 2015 +0000 +++ b/actuators.cpp Mon Oct 26 12:45:41 2015 +0000 @@ -7,84 +7,84 @@ #include "buttons.h" #include "Servo.h" - // Motor control constants - #define pwm_frequency 50000 // still High, could be lowered - #define PI 3.14159265 +// Motor control constants +#define pwm_frequency 50000 // still High, could be lowered +#define PI 3.14159265 - // functions for controlling the motors - bool motorsEnable = false; - bool safetyOn = true; +// functions for controlling the motors +bool motorsEnable = false; +bool safetyOn = true; - double encoder1Counts = 0; - double encoder2Counts = 0; +double encoder1Counts = 0; +double encoder2Counts = 0; - 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; - double motor1Pos = 0; - double motor2Pos = 0; +double motor1Pos = 0; +double motor2Pos = 0; - double motor1Speed = 0; - double motor2Speed = 0; +double motor1Speed = 0; +double motor2Speed = 0; - double motor1SetSpeed = 0; - double motor2SetSpeed = 0; +double motor1SetSpeed = 0; +double motor2SetSpeed = 0; - double motor1PWM = 0; - double motor2PWM = 0; +double motor1PWM = 0; +double motor2PWM = 0; - // Set PID values - double Kp1 = 1; - double Ki1 = 0; - double Kd1 = 0; +// Set PID values +double Kp1 = 1; +double Ki1 = 0; +double Kd1 = 0; - double Kp2 = 0.008; - double Ki2 = 0.08; - double Kd2 = 0; +double Kp2 = 0.008; +double Ki2 = 0.08; +double Kd2 = 0; - double motor1PrevCounts = 0; - double motor2PrevCounts = 0; - double prevTime = 0; - double now = 0; - double timechange; +double motor1PrevCounts = 0; +double motor2PrevCounts = 0; +double prevTime = 0; +double now = 0; +double timechange; + +// Set servo values +const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg - // Set servo values - const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg - - // Set calibration values - double motorCalSpeed = 10; // deg/sec - double returnSpeed = -10; - bool springHit = false; - float lastCall = 0; - bool calibrating1 = true; - bool calibrating2 = false; +// 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 object instances +// Safety Pin +DigitalIn safetyIn(safetyPin); - // initialize Servo - Servo servo(servoPin); +// Initialze motors +PwmOut motor1(motor1PWMPin); +PwmOut motor2(motor2PWMPin); + +// initialize Servo +Servo servo(servoPin); - // Initialize encoders - Encoder encoder1(enc1A, enc1B); - Encoder encoder2(enc2A, enc2B); +// 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 motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1); - PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2); +// create PID instances +PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1); +PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2); - Timer t; +Timer t; void motorInit(){ @@ -243,7 +243,7 @@ } -bool kinematics(){ +//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); @@ -269,4 +269,4 @@ // setXSpeed*L1*cos(motor1Pos*PI/180) + \ // setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*L2*sin(motor2Pos*PI/180)); -} \ No newline at end of file +//} \ No newline at end of file