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-15
- Revision:
- 66:d1ab5904f8e5
- Parent:
- 62:6c566e6f9664
- Child:
- 68:21cb054f1399
File content as of revision 66:d1ab5904f8e5:
#include "actuators.h" #include "PID.h" #include "mbed.h" #include "config.h" #include "encoder.h" #include "HIDScope.h" #include "Servo.h" // functions for controlling the motors bool motorsEnable = false; bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) bool direction2 = false; double motor1Pos = 0; double motor2Pos = 0; double motor1Speed = 0; double motor2Speed = 0; double motor1SetSpeed = 0; double motor2SetSpeed = 0; double servoPos = 0; double motor1PWM = 0; double motor2PWM = 0; // Set PID values double Kp1 = 1; double Ki1 = 0; double Kd1 = 0; double Kp2 = 0.0184; double Ki2 = 0.32; double Kd2 = 0; double motor1PrevPos = 0; double motor2PrevPos = 0; double prevTime = 0; double now = 0; double timechange; bool pidOut = 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 motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1); PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2); Servo servo(servoPin); Timer t; void motorInit(){ motor1Dir.write(direction1); motor2Dir.write(direction2); // Set motor PWM period motor1.period(1/pwm_frequency); motor2.period(1/pwm_frequency); motor1PID.SetSampleTime(motorCall); motor2PID.SetSampleTime(motorCall); // Turn PID on motor1PID.SetMode(AUTOMATIC); motor2PID.SetMode(AUTOMATIC); // start the timer t.start(); } void motorControl(){ // 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 now = t.read(); timechange = (now - prevTime); motor1Speed = -((((encoder1.getPosition() - motor1PrevPos)/32)/131.25)*360)/timechange; motor2Speed = -((((encoder2.getPosition() - motor2PrevPos)/32)/131.25)*360)/timechange; prevTime = now; motor1PrevPos = encoder1.getPosition(); motor2PrevPos = encoder2.getPosition(); // calculate motor setpoint speed in deg/sec from setpoint x/y speed if(motorsEnable){ // only run motors if switch is enabled // compute new PID parameters using setpoint angle speeds and encoder speed writeMotors(); }else{ // write 0 to motors motor1.write(0); motor2.write(0); } } void writeMotors(){ motor1PID.Compute(); // calculate PID outputs, output changes automatically pidOut = motor2PID.Compute(); // write new values to motor's if (motor1PWM >= 0 ){ // CCW rotation direction1 = false; }else{ direction1 = true; // CW rotation } // if (motor2PWM >= 0 ){ // CCW rotation // direction2 = false; // }else{ // direction2 = true; // CW rotation // } motor1Dir.write(direction1); motor2Dir.write(direction2); motor1.write(abs(motor1PWM)); motor2.write(abs(motor2SetSpeed)/300); } void servoControl(){ // use potMeter Value to set servo angle servo.write(servoPos); // (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. } void calibrateMotors(){ // bool calibrating1 = true; // bool calibrating2 = true; // motor1Dir.write(false); // motor2Dir.write(false); // // move motors CCW until they reach outer most position // while (calibrating1 || calibrating2){ // if(motor1AnglePin == 0){ // motor1.write(0.3f); // }else{ // motor1.write(0); // calibrating1 = false; // } // if(motor2AnglePin == 0){ // motor2.write(0.3f); // }else{ // motor2.write(0); // calibrating2 = false; // } // wait(0.2f); // } // // set the encoder values for angle. // encoder1.setValue(0); // encoder2.setValue(0); }