Dammenisvoorcasuals / Mbed 2 deprecated chessRobot

control for robotic arm that can play chess using a granular gripper

Dependencies:   Encoder mbed HIDScope Servo MODSERIAL

Fork of chessRobot by a steenbeek

actuators.cpp@2:95ba9f6f0128, 2015-10-01 (annotated)

Committer:
annesteenbeek
Date:
Thu Oct 01 15:36:55 2015 +0200
Revision:
2:95ba9f6f0128
Parent:
1:80f098c05d4b
Child:
3:47c76be6d402
Changed motor configuration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
annesteenbeek 0:525558a26464 1 // functions for controlling the motors
annesteenbeek 0:525558a26464 2
annesteenbeek 0:525558a26464 3 void motorInit(){
annesteenbeek 1:80f098c05d4b 4 PwmOut motor1(motorpin1);
annesteenbeek 1:80f098c05d4b 5 PwmOut motor2(motorpin2);
annesteenbeek 1:80f098c05d4b 6
annesteenbeek 1:80f098c05d4b 7 // create PID instances for motors
annesteenbeek 1:80f098c05d4b 8 // PID pidname(input, output, setpoint, kp, ki, kd, direction)
annesteenbeek 1:80f098c05d4b 9 PID PIDmotor1(&motorSpeed1, &motorPWM1, &motorSetSpeed1, Kp1, Ki1, Kd1, DIRECT);
annesteenbeek 1:80f098c05d4b 10 PID PIDmotor2(&motorSpeed2, &motorPWM2, &motorSetSpeed2, Kp2, Ki2, Kd2, DIRECT);
annesteenbeek 1:80f098c05d4b 11
annesteenbeek 1:80f098c05d4b 12 // set PID mode
annesteenbeek 1:80f098c05d4b 13 PIDmotor1.SetMode(AUTOMATIC);
annesteenbeek 1:80f098c05d4b 14 PIDmotor2.SetMode(AUTOMATIC);
annesteenbeek 1:80f098c05d4b 15
annesteenbeek 2:95ba9f6f0128 16 // set limits for PID output to avoid integrator build up.
annesteenbeek 1:80f098c05d4b 17 PIDmotor1.SetOutputLimits(-1f, 1f);
annesteenbeek 1:80f098c05d4b 18 PIDmotor2.SetOutputLimits(-1f, 1f);
annesteenbeek 0:525558a26464 19 }
annesteenbeek 0:525558a26464 20
annesteenbeek 0:525558a26464 21
annesteenbeek 0:525558a26464 22 void motorControl(){
annesteenbeek 1:80f098c05d4b 23 if(motorEnable){ // only run motors if switch is enabled
annesteenbeek 1:80f098c05d4b 24
annesteenbeek 0:525558a26464 25 // get encoder positions
annesteenbeek 0:525558a26464 26 // check if motor's are within rotational boundarys
annesteenbeek 0:525558a26464 27 // calculate encoder speeds
annesteenbeek 2:95ba9f6f0128 28 motorSpeed1=(motorPos1-prevMotorPos1)/(time.read()-prevTime);
annesteenbeek 2:95ba9f6f0128 29 motorSpeed2=(motorPos2-prevMotorPos2)/(time.read()-prevTime);
annesteenbeek 2:95ba9f6f0128 30
annesteenbeek 2:95ba9f6f0128 31 // store current positions and time
annesteenbeek 2:95ba9f6f0128 32 prevMotorPos1 = motorPos1;
annesteenbeek 2:95ba9f6f0128 33 prevMotorPos2 = motorPos2;
annesteenbeek 2:95ba9f6f0128 34 prevTime = time.read();
annesteenbeek 0:525558a26464 35 // translate to x/y speed
annesteenbeek 0:525558a26464 36 // compute new PID parameters using setpoint speeds and x/y speeds
annesteenbeek 1:80f098c05d4b 37 PIDmotor1.compute();
annesteenbeek 1:80f098c05d4b 38 PIDmotor2.compute();
annesteenbeek 2:95ba9f6f0128 39 // translate to motor rotation speed
annesteenbeek 0:525558a26464 40 // write new values to motor's
annesteenbeek 2:95ba9f6f0128 41 motor1.write(motorPWM1);
annesteenbeek 2:95ba9f6f0128 42 motor2.write(motorPWM2);
annesteenbeek 2:95ba9f6f0128 43
annesteenbeek 0:525558a26464 44 }else{
annesteenbeek 0:525558a26464 45 // write 0 to motors
annesteenbeek 2:95ba9f6f0128 46 motor1.write(0);
annesteenbeek 2:95ba9f6f0128 47 motor2.write(0);
annesteenbeek 0:525558a26464 48 }
annesteenbeek 0:525558a26464 49 }
annesteenbeek 0:525558a26464 50
annesteenbeek 0:525558a26464 51 void servoControl(){
annesteenbeek 0:525558a26464 52 // use potMeter Value to set servo angle
annesteenbeek 0:525558a26464 53 // (optionaly calculate xy position to keep balloon in position)
annesteenbeek 0:525558a26464 54 // calculate z position using angle
annesteenbeek 0:525558a26464 55 // calculate x y translation of endpoint
annesteenbeek 0:525558a26464 56 // find new x and y speed.
annesteenbeek 0:525558a26464 57
annesteenbeek 0:525558a26464 58 }
annesteenbeek 0:525558a26464 59
annesteenbeek 0:525558a26464 60 void pumpControl(){
annesteenbeek 0:525558a26464 61 if (pumpButton == HIGH){
annesteenbeek 0:525558a26464 62 // write pumpPin High
annesteenbeek 0:525558a26464 63 }else{
annesteenbeek 0:525558a26464 64 // write pumpPin Low
annesteenbeek 0:525558a26464 65 }
annesteenbeek 0:525558a26464 66 }