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@6:b957d8809e7c, 2015-10-05 (annotated)

Committer:
annesteenbeek
Date:
Mon Oct 05 17:51:59 2015 +0200
Revision:
6:b957d8809e7c
Parent:
5:73bfad06b775
Child:
12:61759f94c07a
moved actuator set pointers to actuator.cpp

Who changed what in which revision?

UserRevisionLine numberNew contents of line
annesteenbeek 0:525558a26464 1 // functions for controlling the motors
annesteenbeek 6:b957d8809e7c 2 bool motorEnable = false;
annesteenbeek 6:b957d8809e7c 3
annesteenbeek 6:b957d8809e7c 4 bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
annesteenbeek 6:b957d8809e7c 5 bool direction2 = false;
annesteenbeek 6:b957d8809e7c 6
annesteenbeek 6:b957d8809e7c 7 double motor1Pos = 0;
annesteenbeek 6:b957d8809e7c 8 double motor2Pos = 0;
annesteenbeek 6:b957d8809e7c 9
annesteenbeek 6:b957d8809e7c 10 double motorSpeed1 = 0;
annesteenbeek 6:b957d8809e7c 11 double motorSpeed2 = 0;
annesteenbeek 6:b957d8809e7c 12
annesteenbeek 6:b957d8809e7c 13 double motorSetSpeed1 = 0;
annesteenbeek 6:b957d8809e7c 14 double motorSetSpeed2 = 0;
annesteenbeek 6:b957d8809e7c 15
annesteenbeek 6:b957d8809e7c 16
annesteenbeek 6:b957d8809e7c 17 double motorPWM1 = 0;
annesteenbeek 6:b957d8809e7c 18 double motorPWM2 = 0;
annesteenbeek 6:b957d8809e7c 19
annesteenbeek 6:b957d8809e7c 20 // Set PID values
annesteenbeek 6:b957d8809e7c 21 double Kp1 = 1;
annesteenbeek 6:b957d8809e7c 22 double Ki1 = 1;
annesteenbeek 6:b957d8809e7c 23 double Kd1 = 1;
annesteenbeek 6:b957d8809e7c 24
annesteenbeek 6:b957d8809e7c 25 double Kp2 = 1;
annesteenbeek 6:b957d8809e7c 26 double Ki2 = 1;
annesteenbeek 6:b957d8809e7c 27 double Kd2 = 1;
annesteenbeek 0:525558a26464 28
annesteenbeek 0:525558a26464 29 void motorInit(){
annesteenbeek 3:47c76be6d402 30 // Initialze motors
annesteenbeek 3:47c76be6d402 31 PwmOut motor1(motor1PWMPin);
annesteenbeek 3:47c76be6d402 32 PwmOut motor2(motor2PWMPin);
annesteenbeek 3:47c76be6d402 33
annesteenbeek 3:47c76be6d402 34 // Set motor direction pins.
annesteenbeek 3:47c76be6d402 35 DigitalOut motor1Dir(motor1DirPin);
annesteenbeek 3:47c76be6d402 36 DigitalOut motor2Dir(motor2DirPin);
annesteenbeek 3:47c76be6d402 37
annesteenbeek 3:47c76be6d402 38 // Set initial direction
annesteenbeek 3:47c76be6d402 39 motor1Dir.write(direction1);
annesteenbeek 3:47c76be6d402 40 motor2Dir.write(direction2);
annesteenbeek 3:47c76be6d402 41
annesteenbeek 3:47c76be6d402 42 // Set motor PWM period
annesteenbeek 3:47c76be6d402 43 motor1.period(1/pwm_frequency);
annesteenbeek 3:47c76be6d402 44 motor2.period(1/pwm_frequency);
annesteenbeek 3:47c76be6d402 45
annesteenbeek 4:80e2280058ed 46 // Initialize encoders (with speed calculation)
annesteenbeek 4:80e2280058ed 47 Encoder encoder1(enc1A, enc1B, true);
annesteenbeek 4:80e2280058ed 48 Encoder encoder2(enc2A, enc2B, true);
annesteenbeek 1:80f098c05d4b 49
annesteenbeek 4:80e2280058ed 50 initPID();
annesteenbeek 4:80e2280058ed 51 }
annesteenbeek 4:80e2280058ed 52
annesteenbeek 4:80e2280058ed 53 void initPID(){
annesteenbeek 5:73bfad06b775 54 // create PID instances for motors
annesteenbeek 5:73bfad06b775 55 // PID pidname(input, output, setpoint, kp, ki, kd, direction)
annesteenbeek 1:80f098c05d4b 56 PID PIDmotor1(&motorSpeed1, &motorPWM1, &motorSetSpeed1, Kp1, Ki1, Kd1, DIRECT);
annesteenbeek 1:80f098c05d4b 57 PID PIDmotor2(&motorSpeed2, &motorPWM2, &motorSetSpeed2, Kp2, Ki2, Kd2, DIRECT);
annesteenbeek 1:80f098c05d4b 58
annesteenbeek 1:80f098c05d4b 59 // set PID mode
annesteenbeek 1:80f098c05d4b 60 PIDmotor1.SetMode(AUTOMATIC);
annesteenbeek 1:80f098c05d4b 61 PIDmotor2.SetMode(AUTOMATIC);
annesteenbeek 1:80f098c05d4b 62
annesteenbeek 2:95ba9f6f0128 63 // set limits for PID output to avoid integrator build up.
annesteenbeek 4:80e2280058ed 64 PIDmotor1.SetOutputLimits(-1.0, 1.0);
annesteenbeek 4:80e2280058ed 65 PIDmotor2.SetOutputLimits(-1.0, 1.0);
annesteenbeek 4:80e2280058ed 66 }
annesteenbeek 0:525558a26464 67
annesteenbeek 0:525558a26464 68
annesteenbeek 0:525558a26464 69 void motorControl(){
annesteenbeek 1:80f098c05d4b 70 if(motorEnable){ // only run motors if switch is enabled
annesteenbeek 1:80f098c05d4b 71
annesteenbeek 0:525558a26464 72 // get encoder positions
annesteenbeek 3:47c76be6d402 73 motor1Pos = encoder1.getPosition();
annesteenbeek 3:47c76be6d402 74 motor2Pos = encoder2.getPosition();
annesteenbeek 3:47c76be6d402 75
annesteenbeek 0:525558a26464 76 // check if motor's are within rotational boundarys
annesteenbeek 4:80e2280058ed 77 // get encoder speeds
annesteenbeek 4:80e2280058ed 78 motorSpeed1 = encoder1.getSpeed();
annesteenbeek 4:80e2280058ed 79 motorSpeed2 = encoder2.getSpeed();
annesteenbeek 2:95ba9f6f0128 80
annesteenbeek 0:525558a26464 81 // translate to x/y speed
annesteenbeek 0:525558a26464 82 // compute new PID parameters using setpoint speeds and x/y speeds
annesteenbeek 1:80f098c05d4b 83 PIDmotor1.compute();
annesteenbeek 1:80f098c05d4b 84 PIDmotor2.compute();
annesteenbeek 2:95ba9f6f0128 85 // translate to motor rotation speed
annesteenbeek 0:525558a26464 86 // write new values to motor's
annesteenbeek 5:73bfad06b775 87 if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion)
annesteenbeek 5:73bfad06b775 88 direction1 = false;
annesteenbeek 5:73bfad06b775 89 }else{
annesteenbeek 5:73bfad06b775 90 direction1 = true; // CW rotation
annesteenbeek 5:73bfad06b775 91 }
annesteenbeek 5:73bfad06b775 92 if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion)
annesteenbeek 5:73bfad06b775 93 direction2 = false;
annesteenbeek 5:73bfad06b775 94 }else{
annesteenbeek 5:73bfad06b775 95 direction2 = true; // CW rotation
annesteenbeek 5:73bfad06b775 96 }
annesteenbeek 5:73bfad06b775 97 motor1.write(abs(motorPWM1));
annesteenbeek 5:73bfad06b775 98 motor2.write(abs(motorPWM2));
annesteenbeek 2:95ba9f6f0128 99
annesteenbeek 0:525558a26464 100 }else{
annesteenbeek 0:525558a26464 101 // write 0 to motors
annesteenbeek 2:95ba9f6f0128 102 motor1.write(0);
annesteenbeek 2:95ba9f6f0128 103 motor2.write(0);
annesteenbeek 0:525558a26464 104 }
annesteenbeek 0:525558a26464 105 }
annesteenbeek 0:525558a26464 106
annesteenbeek 0:525558a26464 107 void servoControl(){
annesteenbeek 0:525558a26464 108 // use potMeter Value to set servo angle
annesteenbeek 0:525558a26464 109 // (optionaly calculate xy position to keep balloon in position)
annesteenbeek 0:525558a26464 110 // calculate z position using angle
annesteenbeek 0:525558a26464 111 // calculate x y translation of endpoint
annesteenbeek 0:525558a26464 112 // find new x and y speed.
annesteenbeek 0:525558a26464 113
annesteenbeek 4:80e2280058ed 114 }