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:
- 52:2ac9dee099ce
- Parent:
- 51:1e6334b993a3
- Child:
- 54:c14c3bc48b8a
diff -r 1e6334b993a3 -r 2ac9dee099ce actuators.cpp --- a/actuators.cpp Fri Oct 09 13:32:29 2015 +0200 +++ b/actuators.cpp Fri Oct 09 13:42:41 2015 +0200 @@ -14,16 +14,16 @@ float motor1Pos = 0; float motor2Pos = 0; - float motorSpeed1 = 0; - float motorSpeed2 = 0; + float motor1Speed = 0; + float motor2Speed = 0; - float motorSetSpeed1 = 0; - float motorSetSpeed2 = 0; + float motor1SetSpeed = 0; + float motor2SetSpeed = 0; float servoPos = 0; - float motorPWM1 = 0; - float motorPWM2 = 0; + float motor1PWM = 0; + float motor2PWM = 0; // Set PID values float Kp1 = 1; @@ -34,8 +34,8 @@ float Ki2 = 0; float Kd2 = 0; - float prevMotor2Pos = 0; - float prevMotor1Pos = 0; + float motor1PrevPos = 0; + float motor2PrevPos = 0; float prevTime = 0; float now = 0; float timechange; @@ -54,8 +54,8 @@ DigitalOut motor2Dir(motor2DirPin); // create PID instances - PID PIDmotor1(Kp1, Ki1, Kd1, motorCall); // motorCall is period motorControll() gets called - PID PIDmotor2(Kp2, Ki2, Kd2, motorCall); + PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1); + PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2); Servo servo(servoPin); Timer t; @@ -69,21 +69,9 @@ motor1.period(1/pwm_frequency); motor2.period(1/pwm_frequency); - PIDmotor1.setSetPoint(&abs(motorSetSpeed1)); - PIDmotor2.setSetPoint(&abs(motorSetSpeed2)); - - PIDmotor1.setProcessValue(&abs(motorSpeed1)); - PIDmotor2.setProcessValue(&abs(motorSpeed2)); - - // set limits for PID output to avoid integrator build up. - PIDmotor1.setInputLimits(0, 300); - PIDmotor2.setInputLimits(0, 300); - PIDmotor1.setOutputLimits(0, 1); - PIDmotor2.setOutputLimits(0, 1); - // Turn PID on - PIDmotor1.setMode(1); - PIDmotor2.setMode(1); + motor1PID.SetMode(1); + motor2PID.SetMode(1); // start the timer t.start(); @@ -102,11 +90,11 @@ // get encoder speeds in deg/sec now = t.read(); timechange = (now - prevTime); - motorSpeed1 = (motor1Pos - prevMotor1Pos)/timechange; - motorSpeed2 = (motor2Pos - prevMotor2Pos)/timechange; + motor1Speed = (motor1Pos - motor1PrevPos)/timechange; + motor2Speed = (motor2Pos - motor2PrevPos)/timechange; prevTime = now; - prevMotor1Pos = motor1Pos; - prevMotor2Pos = motor2Pos; + motor1PrevPos = motor1Pos; + motor2PrevPos = motor2Pos; // calculate motor setpoint speed in deg/sec from setpoint x/y speed @@ -122,21 +110,15 @@ } void writeMotors(){ - // PIDmotor1.setSetPoint(abs(motorSetSpeed1)); - // PIDmotor2.setSetPoint(abs(motorSetSpeed2)); - - // PIDmotor1.setProcessValue(abs(motorSpeed1)); - // PIDmotor2.setProcessValue(abs(motorSpeed2)); - - motorPWM1 = PIDmotor1.compute(); - motorPWM2 = PIDmotor2.compute(); + motor1PID.compute(); // calculate PID outputs, output changes automatically + motor2PID.compute(); // write new values to motor's - if (motorSetSpeed1 >= 0 ){ // CCW rotation + if (motor1PWM >= 0 ){ // CCW rotation direction1 = false; }else{ direction1 = true; // CW rotation } - if (motorSetSpeed2 >= 0 ){ // CCW rotation + if (motor2PWM >= 0 ){ // CCW rotation direction2 = false; }else{ direction2 = true; // CW rotation @@ -144,8 +126,8 @@ motor1Dir.write(direction1); motor2Dir.write(direction2); - motor1.write(abs(motorPWM1)); - motor2.write(abs(motorPWM2)); + motor1.write(abs(motor1PWM)); + motor2.write(abs(motor2PWM)); } void servoControl(){