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:
- 1:80f098c05d4b
- Parent:
- 0:525558a26464
- Child:
- 2:95ba9f6f0128
diff -r 525558a26464 -r 80f098c05d4b actuators.cpp --- a/actuators.cpp Mon Sep 28 13:54:34 2015 +0000 +++ b/actuators.cpp Thu Oct 01 15:09:24 2015 +0200 @@ -1,17 +1,34 @@ // functions for controlling the motors void motorInit(){ - + PwmOut motor1(motorpin1); + PwmOut motor2(motorpin2); + + // 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); + + // set PID mode + PIDmotor1.SetMode(AUTOMATIC); + PIDmotor2.SetMode(AUTOMATIC); + + PIDmotor1.SetOutputLimits(-1f, 1f); + PIDmotor2.SetOutputLimits(-1f, 1f); } void motorControl(){ - if(motorEnable == HIGH){ // only run motors if switch is enabled + if(motorEnable){ // only run motors if switch is enabled + // get encoder positions // check if motor's are within rotational boundarys // calculate encoder speeds // translate to x/y speed + // compute new PID parameters using setpoint speeds and x/y speeds + PIDmotor1.compute(); + PIDmotor2.compute(); // write new values to motor's }else{