control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
actuators.cpp
- Committer:
- annesteenbeek
- Date:
- 2015-10-06
- Revision:
- 30:a20f16bf8dda
- Parent:
- 29:e4f3455aaa0b
- Child:
- 31:8fbee6c92753
File content as of revision 30:a20f16bf8dda:
#include "actuators.h" #include "PID.h" #include "mbed.h" #include "config.h" #include "encoder.h" #include "HIDScope.h" // functions for controlling the motors bool motorEnable = true; 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 = 0; float Kd1 = 0; float Kp2 = 1; float Ki2 = 0; float Kd2 = 0; float PIDinterval = 0.2; float prevMotor2Pos = 0; float prevMotor1Pos = 0; float prevTime = 0; float curTime = 0; float pidError = 0; // Create object instances // Initialze motors PwmOut motor1(motor1PWMPin); PwmOut motor2(motor2PWMPin); // Initialize encoders Encoder encoder1(enc1A, enc1B); Encoder encoder2(enc2A, enc2B); // Set direction pins DigitalOut motor1Dir(motor1DirPin); DigitalOut motor2Dir(motor2DirPin); // create PID instances PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); Timer t; void motorInit(){ motor1Dir.write(direction1); motor2Dir.write(direction2); // Set motor PWM period motor1.period(1/pwm_frequency); motor2.period(1/pwm_frequency); PIDmotor1.setSetPoint(motorSetSpeed1); PIDmotor2.setSetPoint(motorSetSpeed2); PIDmotor1.setProcessValue(motorSpeed1); PIDmotor2.setProcessValue(motorSpeed2); // set limits for PID output to avoid integrator build up. PIDmotor1.setInputLimits(-100, 300); PIDmotor2.setInputLimits(-100,300); PIDmotor1.setOutputLimits(-100, 300); PIDmotor2.setOutputLimits(-100, 300); // Turn PID on PIDmotor1.setMode(1); PIDmotor2.setMode(1); // start the timer t.start(); } void motorControl(){ if(motorEnable){ // only run motors if switch is enabled // 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 motor1Pos = -((encoder1.getPosition()/32)/131.25)*360; motor2Pos = -((encoder2.getPosition()/32)/131.25)*360; // check if motor's are within rotational boundarys // get encoder speeds in deg/sec curTime = t.read(); motorSpeed1 = (motor1Pos - prevMotor1Pos)/(curTime-prevTime); motorSpeed2 = (motor2Pos - prevMotor2Pos)/(curTime-prevTime); prevTime = curTime; prevMotor1Pos = motor1Pos; prevMotor2Pos = motor2Pos; // translate to x/y speed // compute new PID parameters using setpoint speeds and x/y speeds PIDmotor1.setSetPoint(motorSetSpeed1); PIDmotor2.setSetPoint(motorSetSpeed2); PIDmotor1.setProcessValue(motorSpeed1); PIDmotor2.setProcessValue(motorSpeed2); motorPWM1 = PIDmotor1.compute(); motorPWM2 = PIDmotor2.compute(); // translate to motor rotation speed // write new values to motor's if (motorPWM1 > 0 ){ // CCW rotation direction1 = false; }else{ direction1 = true; // CW rotation } if (motorPWM2 > 0 ){ // CCW rotation direction2 = false; }else{ direction2 = true; // CW rotation } motor1Dir.write(direction1); motor2Dir.write(direction2); motor1.write(abs(motorPWM1)/300); motor2.write(abs(motorPWM2)/300); // motor2.write(motorSetSpeed2); pidError = PIDmotor2.getError(); }else{ // write 0 to motors motor1.write(0); motor2.write(0); } } void servoControl(){ // use potMeter Value to set servo angle // (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. }