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:
- 50:b0cf07ca53cf
- Parent:
- 48:3cf1eaf34926
- Child:
- 51:1e6334b993a3
diff -r b58958e6cacd -r b0cf07ca53cf actuators.cpp --- a/actuators.cpp Thu Oct 08 16:26:39 2015 +0200 +++ b/actuators.cpp Fri Oct 09 10:49:56 2015 +0200 @@ -34,12 +34,11 @@ float Ki2 = 0; float Kd2 = 0; - float PIDinterval = 0.1; - float prevMotor2Pos = 0; float prevMotor1Pos = 0; float prevTime = 0; - float curTime = 0; + float now = 0; + float timechange; // Create object instances // Initialze motors @@ -55,8 +54,8 @@ DigitalOut motor2Dir(motor2DirPin); // create PID instances - PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); - PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); + PID PIDmotor1(Kp1, Ki1, Kd1, motorCall); // motorCall is period motorControll() gets called + PID PIDmotor2(Kp2, Ki2, Kd2, motorCall); Servo servo(servoPin); Timer t; @@ -101,10 +100,11 @@ // 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; + now = t.read(); + timechange = (now - prevTime); + motorSpeed1 = (motor1Pos - prevMotor1Pos)/timechange; + motorSpeed2 = (motor2Pos - prevMotor2Pos)/timechange; + prevTime = now; prevMotor1Pos = motor1Pos; prevMotor2Pos = motor2Pos; @@ -131,12 +131,12 @@ motorPWM1 = PIDmotor1.compute(); motorPWM2 = PIDmotor2.compute(); // write new values to motor's - if (motorSetSpeed1 > 0 ){ // CCW rotation + if (motorSetSpeed1 >= 0 ){ // CCW rotation direction1 = false; }else{ direction1 = true; // CW rotation } - if (motorSetSpeed2 > 0 ){ // CCW rotation + if (motorSetSpeed2 >= 0 ){ // CCW rotation direction2 = false; }else{ direction2 = true; // CW rotation