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@5:73bfad06b775, 2015-10-05 (annotated)

Committer:
annesteenbeek
Date:
Mon Oct 05 17:35:02 2015 +0200
Revision:
5:73bfad06b775
Parent:
4:80e2280058ed
Child:
6:b957d8809e7c
added HIDScope in debugging

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 4:80e2280058ed 20 // Initialize encoders (with speed calculation)
annesteenbeek 4:80e2280058ed 21 Encoder encoder1(enc1A, enc1B, true);
annesteenbeek 4:80e2280058ed 22 Encoder encoder2(enc2A, enc2B, true);
annesteenbeek 1:80f098c05d4b 23
annesteenbeek 4:80e2280058ed 24 initPID();
annesteenbeek 4:80e2280058ed 25 }
annesteenbeek 4:80e2280058ed 26
annesteenbeek 4:80e2280058ed 27 void initPID(){
annesteenbeek 5:73bfad06b775 28 // create PID instances for motors
annesteenbeek 5:73bfad06b775 29 // PID pidname(input, output, setpoint, kp, ki, kd, direction)
annesteenbeek 1:80f098c05d4b 30 PID PIDmotor1(&motorSpeed1, &motorPWM1, &motorSetSpeed1, Kp1, Ki1, Kd1, DIRECT);
annesteenbeek 1:80f098c05d4b 31 PID PIDmotor2(&motorSpeed2, &motorPWM2, &motorSetSpeed2, Kp2, Ki2, Kd2, DIRECT);
annesteenbeek 1:80f098c05d4b 32
annesteenbeek 1:80f098c05d4b 33 // set PID mode
annesteenbeek 1:80f098c05d4b 34 PIDmotor1.SetMode(AUTOMATIC);
annesteenbeek 1:80f098c05d4b 35 PIDmotor2.SetMode(AUTOMATIC);
annesteenbeek 1:80f098c05d4b 36
annesteenbeek 2:95ba9f6f0128 37 // set limits for PID output to avoid integrator build up.
annesteenbeek 4:80e2280058ed 38 PIDmotor1.SetOutputLimits(-1.0, 1.0);
annesteenbeek 4:80e2280058ed 39 PIDmotor2.SetOutputLimits(-1.0, 1.0);
annesteenbeek 4:80e2280058ed 40 }
annesteenbeek 0:525558a26464 41
annesteenbeek 0:525558a26464 42
annesteenbeek 0:525558a26464 43 void motorControl(){
annesteenbeek 1:80f098c05d4b 44 if(motorEnable){ // only run motors if switch is enabled
annesteenbeek 1:80f098c05d4b 45
annesteenbeek 0:525558a26464 46 // get encoder positions
annesteenbeek 3:47c76be6d402 47 motor1Pos = encoder1.getPosition();
annesteenbeek 3:47c76be6d402 48 motor2Pos = encoder2.getPosition();
annesteenbeek 3:47c76be6d402 49
annesteenbeek 0:525558a26464 50 // check if motor's are within rotational boundarys
annesteenbeek 4:80e2280058ed 51 // get encoder speeds
annesteenbeek 4:80e2280058ed 52 motorSpeed1 = encoder1.getSpeed();
annesteenbeek 4:80e2280058ed 53 motorSpeed2 = encoder2.getSpeed();
annesteenbeek 2:95ba9f6f0128 54
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 5:73bfad06b775 61 if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion)
annesteenbeek 5:73bfad06b775 62 direction1 = false;
annesteenbeek 5:73bfad06b775 63 }else{
annesteenbeek 5:73bfad06b775 64 direction1 = true; // CW rotation
annesteenbeek 5:73bfad06b775 65 }
annesteenbeek 5:73bfad06b775 66 if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion)
annesteenbeek 5:73bfad06b775 67 direction2 = false;
annesteenbeek 5:73bfad06b775 68 }else{
annesteenbeek 5:73bfad06b775 69 direction2 = true; // CW rotation
annesteenbeek 5:73bfad06b775 70 }
annesteenbeek 5:73bfad06b775 71 motor1.write(abs(motorPWM1));
annesteenbeek 5:73bfad06b775 72 motor2.write(abs(motorPWM2));
annesteenbeek 2:95ba9f6f0128 73
annesteenbeek 0:525558a26464 74 }else{
annesteenbeek 0:525558a26464 75 // write 0 to motors
annesteenbeek 2:95ba9f6f0128 76 motor1.write(0);
annesteenbeek 2:95ba9f6f0128 77 motor2.write(0);
annesteenbeek 0:525558a26464 78 }
annesteenbeek 0:525558a26464 79 }
annesteenbeek 0:525558a26464 80
annesteenbeek 0:525558a26464 81 void servoControl(){
annesteenbeek 0:525558a26464 82 // use potMeter Value to set servo angle
annesteenbeek 0:525558a26464 83 // (optionaly calculate xy position to keep balloon in position)
annesteenbeek 0:525558a26464 84 // calculate z position using angle
annesteenbeek 0:525558a26464 85 // calculate x y translation of endpoint
annesteenbeek 0:525558a26464 86 // find new x and y speed.
annesteenbeek 0:525558a26464 87
annesteenbeek 4:80e2280058ed 88 }