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@14:0c0d1bfd94ea, 2015-10-05 (annotated)

Committer:
annesteenbeek
Date:
Mon Oct 05 19:39:00 2015 +0200
Revision:
14:0c0d1bfd94ea
Parent:
13:4837b36b9a68
Child:
19:e89eae07dece
added additional header files, initialize PID with correct function definition

Who changed what in which revision?

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