![](/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
main.cpp
- Committer:
- annesteenbeek
- Date:
- 2015-10-05
- Branch:
- onefile
- Revision:
- 23:3f5d30b4784d
- Parent:
- 15:5fa388ba22cb
- Child:
- 24:2d7e11441eee
File content as of revision 23:3f5d30b4784d:
#include "mbed.h" #include "config.h" // settings and pin configurations #include "encoder.h" #include "PID.h" #include "EMG.h" //#define DEBUG // send debug data to HIDScope #ifdef DEBUG #include "debug.h" dubugInit(); #endif bool motorEnable = false; bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) bool direction2 = false; float motor1Pos = 0; float motor2Pos = 0; float motorSpeed1 = 0; float motorSpeed2 = 0; float motorSetSpeed1 = 0; float motorSetSpeed2 = 0; float motorPWM1 = 0; float motorPWM2 = 0; // Set PID values float Kp1 = 1; float Ki1 = 1; float Kd1 = 1; float Kp2 = 1; float Ki2 = 1; float Kd2 = 1; float PIDinterval = 0.2; int main(){ setPins(); motorInit(); while (true) { checkSwitches(); // readEMG(); motorControl(); // servoControl(); } } void motorInit(){ // Initialze motors PwmOut motor1(motor1PWMPin); PwmOut motor2(motor2PWMPin); // Set motor direction pins. DigitalOut motor1Dir(motor1DirPin); DigitalOut motor2Dir(motor2DirPin); // Set initial direction motor1Dir.write(direction1); motor2Dir.write(direction2); // Set motor PWM period motor1.period(1/pwm_frequency); motor2.period(1/pwm_frequency); // Initialize encoders (with speed calculation) Encoder encoder1(enc1A, enc1B, true); Encoder encoder2(enc2A, enc2B, true); initPID(); } void initPID(){ // create PID instances for motors // PID pidname(input, output, setpoint, kp, ki, kd, direction) PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); PIDmotor1.setSetPoint(motorSetSpeed1); PIDmotor2.setSetPoint(motorSetSpeed2); PIDmotor1.setProcessValue(motorSpeed1); PIDmotor2.setProcessValue(motorSpeed2); // set PID mode PIDmotor1.setMode(1); PIDmotor2.setMode(1); // set limits for PID output to avoid integrator build up. PIDmotor1.setOutputLimits(-1.0, 1.0); PIDmotor2.setOutputLimits(-1.0, 1.0); } void motorControl(){ if(motorEnable){ // only run motors if switch is enabled // get encoder positions motor1Pos = encoder1.getPosition(); motor2Pos = encoder2.getPosition(); // check if motor's are within rotational boundarys // get encoder speeds motorSpeed1 = encoder1.getSpeed(); motorSpeed2 = encoder2.getSpeed(); // translate to x/y speed // compute new PID parameters using setpoint speeds and x/y speeds motorPWM1 = PIDmotor1.compute(); motorPWM2 = PIDmotor2.compute(); // translate to motor rotation speed // write new values to motor's if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion) direction1 = false; }else{ direction1 = true; // CW rotation } if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion) direction2 = false; }else{ direction2 = true; // CW rotation } motor1.write(abs(motorPWM1)); motor2.write(abs(motorPWM2)); }else{ // write 0 to motors motor1.write(0); motor2.write(0); } } void setPins(){ // set input/output pins AnalogIn pot1(pot1Pin); } void checkSwitches(){ // read motor enable switch // read pump enable switch // read servo potmeter position // read x speed potmeter position float motorSetSpeed1 = pot1.read(); // read y speed potmeter position // read killswitches }