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:
- 14:0c0d1bfd94ea
- Parent:
- 13:4837b36b9a68
- Child:
- 19:e89eae07dece
diff -r 4837b36b9a68 -r 0c0d1bfd94ea actuators.cpp --- a/actuators.cpp Mon Oct 05 17:24:14 2015 +0000 +++ b/actuators.cpp Mon Oct 05 19:39:00 2015 +0200 @@ -9,27 +9,27 @@ bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) bool direction2 = false; -double motor1Pos = 0; -double motor2Pos = 0; +float motor1Pos = 0; +float motor2Pos = 0; -double motorSpeed1 = 0; -double motorSpeed2 = 0; +float motorSpeed1 = 0; +float motorSpeed2 = 0; -double motorSetSpeed1 = 0; -double motorSetSpeed2 = 0; +float motorSetSpeed1 = 0; +float motorSetSpeed2 = 0; -double motorPWM1 = 0; -double motorPWM2 = 0; +float motorPWM1 = 0; +float motorPWM2 = 0; // Set PID values -double Kp1 = 1; -double Ki1 = 1; -double Kd1 = 1; +float Kp1 = 1; +float Ki1 = 1; +float Kd1 = 1; -double Kp2 = 1; -double Ki2 = 1; -double Kd2 = 1; +float Kp2 = 1; +float Ki2 = 1; +float Kd2 = 1; void motorInit(){ // Initialze motors @@ -58,9 +58,13 @@ void initPID(){ // create PID instances for motors // PID pidname(input, output, setpoint, kp, ki, kd, direction) - PID PIDmotor1(&motorSpeed1, &motorPWM1, &motorSetSpeed1, Kp1, Ki1, Kd1, DIRECT); - PID PIDmotor2(&motorSpeed2, &motorPWM2, &motorSetSpeed2, Kp2, Ki2, Kd2, DIRECT); + PID PIDmotor1(Kp1, Ki1, Kd1); + PID PIDmotor2(Kp2, Ki2, Kd2); + PIDmotor1.setSetPoint(motorSetSpeed1); + PIDmotor2.setSetPoint(motorSetSpeed2); + PIDmotor1.setProccessValue(motorSpeed1); + PIDmotor2.setProccessValue(motorSpeed2); // set PID mode PIDmotor1.SetMode(AUTOMATIC); PIDmotor2.SetMode(AUTOMATIC); @@ -85,8 +89,8 @@ // translate to x/y speed // compute new PID parameters using setpoint speeds and x/y speeds - PIDmotor1.compute(); - PIDmotor2.compute(); + motorPWM1 = PIDmotor1.compute(); + motorPWM2 = PIDmotor2.compute(); // translate to motor rotation speed // write new values to motor's if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion)