![](/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:
- 3:47c76be6d402
- Parent:
- 2:95ba9f6f0128
- Child:
- 4:80e2280058ed
--- a/actuators.cpp Thu Oct 01 15:36:55 2015 +0200 +++ b/actuators.cpp Mon Oct 05 16:01:21 2015 +0200 @@ -1,8 +1,25 @@ // functions for controlling the motors void motorInit(){ - PwmOut motor1(motorpin1); - PwmOut motor2(motorpin2); + // Initialze motors + PwmOut motor1(motor1PWMPin); + PwmOut motor2(motor2PWMPin); + + // Set motor direction pins. + DigitalOut motor1Dir(motor1DirPin); + DigitalOut motor2Dir(motor2DirPin); + + // Set initial direction + motor1Dir.write(direction1); + motor2Dir.write(direction2); + + // Set motor PWM period + motor1.period(1/pwm_frequency); + motor2.period(1/pwm_frequency); + + // Initialize encoders + Encoder encoder1(enc1A, enc1B); + Encoder encoder2(enc2A, enc2B); // create PID instances for motors // PID pidname(input, output, setpoint, kp, ki, kd, direction) @@ -14,8 +31,8 @@ PIDmotor2.SetMode(AUTOMATIC); // set limits for PID output to avoid integrator build up. - PIDmotor1.SetOutputLimits(-1f, 1f); - PIDmotor2.SetOutputLimits(-1f, 1f); + PIDmotor1.SetOutputLimits(-1, 1); + PIDmotor2.SetOutputLimits(-1, 1); } @@ -23,14 +40,17 @@ if(motorEnable){ // only run motors if switch is enabled // get encoder positions + motor1Pos = encoder1.getPosition(); + motor2Pos = encoder2.getPosition(); + // check if motor's are within rotational boundarys // calculate encoder speeds - motorSpeed1=(motorPos1-prevMotorPos1)/(time.read()-prevTime); - motorSpeed2=(motorPos2-prevMotorPos2)/(time.read()-prevTime); + motorSpeed1=(motor1Pos-prevMotor1Pos)/(time.read()-prevTime); + motorSpeed2=(motor2Pos-prevMotor2Pos)/(time.read()-prevTime); // store current positions and time - prevMotorPos1 = motorPos1; - prevMotorPos2 = motorPos2; + prevMotor1Pos = motor1Pos; + prevMotor2Pos = motor2Pos; prevTime = time.read(); // translate to x/y speed // compute new PID parameters using setpoint speeds and x/y speeds @@ -59,7 +79,7 @@ void pumpControl(){ if (pumpButton == HIGH){ - // write pumpPin High + // write pumpPin High }else{ // write pumpPin Low }