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@3:47c76be6d402, 2015-10-05 (annotated)

Committer:
annesteenbeek
Date:
Mon Oct 05 16:01:21 2015 +0200
Revision:
3:47c76be6d402
Parent:
2:95ba9f6f0128
Child:
4:80e2280058ed
pin layout set and motorControl

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 3:47c76be6d402 4 // Initialze motors
annesteenbeek 3:47c76be6d402 5 PwmOut motor1(motor1PWMPin);
annesteenbeek 3:47c76be6d402 6 PwmOut motor2(motor2PWMPin);
annesteenbeek 3:47c76be6d402 7
annesteenbeek 3:47c76be6d402 8 // Set motor direction pins.
annesteenbeek 3:47c76be6d402 9 DigitalOut motor1Dir(motor1DirPin);
annesteenbeek 3:47c76be6d402 10 DigitalOut motor2Dir(motor2DirPin);
annesteenbeek 3:47c76be6d402 11
annesteenbeek 3:47c76be6d402 12 // Set initial direction
annesteenbeek 3:47c76be6d402 13 motor1Dir.write(direction1);
annesteenbeek 3:47c76be6d402 14 motor2Dir.write(direction2);
annesteenbeek 3:47c76be6d402 15
annesteenbeek 3:47c76be6d402 16 // Set motor PWM period
annesteenbeek 3:47c76be6d402 17 motor1.period(1/pwm_frequency);
annesteenbeek 3:47c76be6d402 18 motor2.period(1/pwm_frequency);
annesteenbeek 3:47c76be6d402 19
annesteenbeek 3:47c76be6d402 20 // Initialize encoders
annesteenbeek 3:47c76be6d402 21 Encoder encoder1(enc1A, enc1B);
annesteenbeek 3:47c76be6d402 22 Encoder encoder2(enc2A, enc2B);
annesteenbeek 1:80f098c05d4b 23
annesteenbeek 1:80f098c05d4b 24 // create PID instances for motors
annesteenbeek 1:80f098c05d4b 25 // PID pidname(input, output, setpoint, kp, ki, kd, direction)
annesteenbeek 1:80f098c05d4b 26 PID PIDmotor1(&motorSpeed1, &motorPWM1, &motorSetSpeed1, Kp1, Ki1, Kd1, DIRECT);
annesteenbeek 1:80f098c05d4b 27 PID PIDmotor2(&motorSpeed2, &motorPWM2, &motorSetSpeed2, Kp2, Ki2, Kd2, DIRECT);
annesteenbeek 1:80f098c05d4b 28
annesteenbeek 1:80f098c05d4b 29 // set PID mode
annesteenbeek 1:80f098c05d4b 30 PIDmotor1.SetMode(AUTOMATIC);
annesteenbeek 1:80f098c05d4b 31 PIDmotor2.SetMode(AUTOMATIC);
annesteenbeek 1:80f098c05d4b 32
annesteenbeek 2:95ba9f6f0128 33 // set limits for PID output to avoid integrator build up.
annesteenbeek 3:47c76be6d402 34 PIDmotor1.SetOutputLimits(-1, 1);
annesteenbeek 3:47c76be6d402 35 PIDmotor2.SetOutputLimits(-1, 1);
annesteenbeek 0:525558a26464 36 }
annesteenbeek 0:525558a26464 37
annesteenbeek 0:525558a26464 38
annesteenbeek 0:525558a26464 39 void motorControl(){
annesteenbeek 1:80f098c05d4b 40 if(motorEnable){ // only run motors if switch is enabled
annesteenbeek 1:80f098c05d4b 41
annesteenbeek 0:525558a26464 42 // get encoder positions
annesteenbeek 3:47c76be6d402 43 motor1Pos = encoder1.getPosition();
annesteenbeek 3:47c76be6d402 44 motor2Pos = encoder2.getPosition();
annesteenbeek 3:47c76be6d402 45
annesteenbeek 0:525558a26464 46 // check if motor's are within rotational boundarys
annesteenbeek 0:525558a26464 47 // calculate encoder speeds
annesteenbeek 3:47c76be6d402 48 motorSpeed1=(motor1Pos-prevMotor1Pos)/(time.read()-prevTime);
annesteenbeek 3:47c76be6d402 49 motorSpeed2=(motor2Pos-prevMotor2Pos)/(time.read()-prevTime);
annesteenbeek 2:95ba9f6f0128 50
annesteenbeek 2:95ba9f6f0128 51 // store current positions and time
annesteenbeek 3:47c76be6d402 52 prevMotor1Pos = motor1Pos;
annesteenbeek 3:47c76be6d402 53 prevMotor2Pos = motor2Pos;
annesteenbeek 2:95ba9f6f0128 54 prevTime = time.read();
annesteenbeek 0:525558a26464 55 // translate to x/y speed
annesteenbeek 0:525558a26464 56 // compute new PID parameters using setpoint speeds and x/y speeds
annesteenbeek 1:80f098c05d4b 57 PIDmotor1.compute();
annesteenbeek 1:80f098c05d4b 58 PIDmotor2.compute();
annesteenbeek 2:95ba9f6f0128 59 // translate to motor rotation speed
annesteenbeek 0:525558a26464 60 // write new values to motor's
annesteenbeek 2:95ba9f6f0128 61 motor1.write(motorPWM1);
annesteenbeek 2:95ba9f6f0128 62 motor2.write(motorPWM2);
annesteenbeek 2:95ba9f6f0128 63
annesteenbeek 0:525558a26464 64 }else{
annesteenbeek 0:525558a26464 65 // write 0 to motors
annesteenbeek 2:95ba9f6f0128 66 motor1.write(0);
annesteenbeek 2:95ba9f6f0128 67 motor2.write(0);
annesteenbeek 0:525558a26464 68 }
annesteenbeek 0:525558a26464 69 }
annesteenbeek 0:525558a26464 70
annesteenbeek 0:525558a26464 71 void servoControl(){
annesteenbeek 0:525558a26464 72 // use potMeter Value to set servo angle
annesteenbeek 0:525558a26464 73 // (optionaly calculate xy position to keep balloon in position)
annesteenbeek 0:525558a26464 74 // calculate z position using angle
annesteenbeek 0:525558a26464 75 // calculate x y translation of endpoint
annesteenbeek 0:525558a26464 76 // find new x and y speed.
annesteenbeek 0:525558a26464 77
annesteenbeek 0:525558a26464 78 }
annesteenbeek 0:525558a26464 79
annesteenbeek 0:525558a26464 80 void pumpControl(){
annesteenbeek 0:525558a26464 81 if (pumpButton == HIGH){
annesteenbeek 3:47c76be6d402 82 // write pumpPin High
annesteenbeek 0:525558a26464 83 }else{
annesteenbeek 0:525558a26464 84 // write pumpPin Low
annesteenbeek 0:525558a26464 85 }
annesteenbeek 0:525558a26464 86 }