![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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:
- 29:e4f3455aaa0b
- Parent:
- 28:40a931dfe840
- Child:
- 30:a20f16bf8dda
--- a/actuators.cpp Tue Oct 06 16:55:31 2015 +0200 +++ b/actuators.cpp Tue Oct 06 17:55:11 2015 +0000 @@ -34,6 +34,10 @@ float PIDinterval = 0.2; +float prevMotor2Pos = 0; +float prevMotor1Pos = 0; +float prevTime = 0; +float curTime = 0; // Create object instances // Initialze motors @@ -41,8 +45,8 @@ PwmOut motor2(motor2PWMPin); // Initialize encoders -Encoder encoder1(enc1A, enc1B, true); -Encoder encoder2(enc2A, enc2B, true); +Encoder encoder1(enc1A, enc1B); +Encoder encoder2(enc2A, enc2B); // Set direction pins DigitalOut motor1Dir(motor1DirPin); @@ -51,7 +55,7 @@ // create PID instances PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval); PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval); - +Timer t; void motorInit(){ @@ -74,7 +78,7 @@ // set limits for PID output to avoid integrator build up. PIDmotor1.setOutputLimits(-1.0, 1.0); PIDmotor2.setOutputLimits(-1.0, 1.0); - + t.start(); } @@ -87,9 +91,14 @@ // check if motor's are within rotational boundarys // get encoder speeds - motorSpeed1 = encoder1.getSpeed(); - motorSpeed2 = encoder2.getSpeed(); - + curTime = t.read() + motorSpeed1 = (motor1Pos - prevMotor1Pos)/(curTime-prevTime); + motorSpeed2 = (motor2Pos - prevMotor2Pos)/(curTime-prevTime); + prevTime = curTime; + prevMotor1Pos = motor1Pos; + prevMotor2Pos = motor2Pos; + + // translate to x/y speed // compute new PID parameters using setpoint speeds and x/y speeds motorPWM1 = PIDmotor1.compute();