control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
actuators.cpp
- Committer:
- annesteenbeek
- Date:
- 2015-10-01
- Revision:
- 2:95ba9f6f0128
- Parent:
- 1:80f098c05d4b
- Child:
- 3:47c76be6d402
File content as of revision 2:95ba9f6f0128:
// 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); // set limits for PID output to avoid integrator build up. PIDmotor1.SetOutputLimits(-1f, 1f); PIDmotor2.SetOutputLimits(-1f, 1f); } void motorControl(){ if(motorEnable){ // only run motors if switch is enabled // get encoder positions // check if motor's are within rotational boundarys // calculate encoder speeds motorSpeed1=(motorPos1-prevMotorPos1)/(time.read()-prevTime); motorSpeed2=(motorPos2-prevMotorPos2)/(time.read()-prevTime); // store current positions and time prevMotorPos1 = motorPos1; prevMotorPos2 = motorPos2; prevTime = time.read(); // translate to x/y speed // compute new PID parameters using setpoint speeds and x/y speeds PIDmotor1.compute(); PIDmotor2.compute(); // translate to motor rotation speed // write new values to motor's motor1.write(motorPWM1); motor2.write(motorPWM2); }else{ // write 0 to motors motor1.write(0); motor2.write(0); } } void servoControl(){ // use potMeter Value to set servo angle // (optionaly calculate xy position to keep balloon in position) // calculate z position using angle // calculate x y translation of endpoint // find new x and y speed. } void pumpControl(){ if (pumpButton == HIGH){ // write pumpPin High }else{ // write pumpPin Low } }