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:
- 30:a20f16bf8dda
- Parent:
- 29:e4f3455aaa0b
- Child:
- 31:8fbee6c92753
diff -r e4f3455aaa0b -r a20f16bf8dda actuators.cpp --- a/actuators.cpp Tue Oct 06 17:55:11 2015 +0000 +++ b/actuators.cpp Tue Oct 06 21:06:20 2015 +0000 @@ -25,12 +25,12 @@ // Set PID values float Kp1 = 1; -float Ki1 = 1; -float Kd1 = 1; +float Ki1 = 0; +float Kd1 = 0; float Kp2 = 1; -float Ki2 = 1; -float Kd2 = 1; +float Ki2 = 0; +float Kd2 = 0; float PIDinterval = 0.2; @@ -39,6 +39,7 @@ float prevTime = 0; float curTime = 0; +float pidError = 0; // Create object instances // Initialze motors PwmOut motor1(motor1PWMPin); @@ -71,13 +72,18 @@ PIDmotor1.setProcessValue(motorSpeed1); PIDmotor2.setProcessValue(motorSpeed2); - // set PID mode + + // 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); - - // set limits for PID output to avoid integrator build up. - PIDmotor1.setOutputLimits(-1.0, 1.0); - PIDmotor2.setOutputLimits(-1.0, 1.0); + + // start the timer t.start(); } @@ -85,13 +91,16 @@ void motorControl(){ if(motorEnable){ // only run motors if switch is enabled - // get encoder positions - motor1Pos = encoder1.getPosition(); - motor2Pos = encoder2.getPosition(); + // 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 - curTime = t.read() + // get encoder speeds in deg/sec + curTime = t.read(); motorSpeed1 = (motor1Pos - prevMotor1Pos)/(curTime-prevTime); motorSpeed2 = (motor2Pos - prevMotor2Pos)/(curTime-prevTime); prevTime = curTime; @@ -101,23 +110,33 @@ // 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 (unitcircle convetion) + if (motorPWM1 > 0 ){ // CCW rotation direction1 = false; }else{ direction1 = true; // CW rotation } - if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion) + if (motorPWM2 > 0 ){ // CCW rotation direction2 = false; }else{ direction2 = true; // CW rotation } - motor1.write(abs(motorPWM1)); - // motor2.write(abs(motorPWM2)); - motor2.write(motorSetSpeed2); + 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