![](/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:
- 4:80e2280058ed
- Parent:
- 3:47c76be6d402
- Child:
- 5:73bfad06b775
--- a/actuators.cpp Mon Oct 05 16:01:21 2015 +0200 +++ b/actuators.cpp Mon Oct 05 16:15:25 2015 +0200 @@ -17,11 +17,15 @@ motor1.period(1/pwm_frequency); motor2.period(1/pwm_frequency); - // Initialize encoders - Encoder encoder1(enc1A, enc1B); - Encoder encoder2(enc2A, enc2B); + // Initialize encoders (with speed calculation) + Encoder encoder1(enc1A, enc1B, true); + Encoder encoder2(enc2A, enc2B, true); - // create PID instances for motors + initPID(); +} + +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); @@ -31,9 +35,9 @@ PIDmotor2.SetMode(AUTOMATIC); // set limits for PID output to avoid integrator build up. - PIDmotor1.SetOutputLimits(-1, 1); - PIDmotor2.SetOutputLimits(-1, 1); - } + PIDmotor1.SetOutputLimits(-1.0, 1.0); + PIDmotor2.SetOutputLimits(-1.0, 1.0); +} void motorControl(){ @@ -44,13 +48,10 @@ motor2Pos = encoder2.getPosition(); // check if motor's are within rotational boundarys - // calculate encoder speeds - motorSpeed1=(motor1Pos-prevMotor1Pos)/(time.read()-prevTime); - motorSpeed2=(motor2Pos-prevMotor2Pos)/(time.read()-prevTime); + // get encoder speeds + motorSpeed1 = encoder1.getSpeed(); + motorSpeed2 = encoder2.getSpeed(); - // store current positions and time - prevMotor1Pos = motor1Pos; - prevMotor2Pos = motor2Pos; prevTime = time.read(); // translate to x/y speed // compute new PID parameters using setpoint speeds and x/y speeds @@ -75,12 +76,4 @@ // calculate x y translation of endpoint // find new x and y speed. - } - -void pumpControl(){ - if (pumpButton == HIGH){ - // write pumpPin High - }else{ - // write pumpPin Low - } - } \ No newline at end of file +}