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@93:a8898eb80edc, 2015-10-21 (annotated)

Committer:
annesteenbeek
Date:
Wed Oct 21 13:16:24 2015 +0200
Revision:
93:a8898eb80edc
Parent:
91:6bbbbc6643c8
Child:
94:28e274481b60
Child:
97:0f67952051e5
working calibration

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 25:874675516927 6 #include "HIDScope.h"
annesteenbeek 34:f315b2b38555 7 #include "Servo.h"
annesteenbeek 93:a8898eb80edc 8 #include "buttons.h"
annesteenbeek 32:2006977785f5 9 // functions for controlling the motors
annesteenbeek 34:f315b2b38555 10 bool motorsEnable = false;
annesteenbeek 76:0aa90e728e4a 11 bool safetyOn = false; // start with safety off for calibration
annesteenbeek 76:0aa90e728e4a 12
annesteenbeek 6:b957d8809e7c 13
annesteenbeek 68:21cb054f1399 14 double encoder1Counts = 0;
annesteenbeek 68:21cb054f1399 15 double encoder2Counts = 0;
annesteenbeek 68:21cb054f1399 16
annesteenbeek 32:2006977785f5 17 bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
annesteenbeek 32:2006977785f5 18 bool direction2 = false;
annesteenbeek 6:b957d8809e7c 19
annesteenbeek 62:6c566e6f9664 20 double motor1Pos = 0;
annesteenbeek 62:6c566e6f9664 21 double motor2Pos = 0;
annesteenbeek 6:b957d8809e7c 22
annesteenbeek 62:6c566e6f9664 23 double motor1Speed = 0;
annesteenbeek 62:6c566e6f9664 24 double motor2Speed = 0;
annesteenbeek 6:b957d8809e7c 25
annesteenbeek 62:6c566e6f9664 26 double motor1SetSpeed = 0;
annesteenbeek 62:6c566e6f9664 27 double motor2SetSpeed = 0;
annesteenbeek 6:b957d8809e7c 28
annesteenbeek 62:6c566e6f9664 29 double servoPos = 0;
annesteenbeek 6:b957d8809e7c 30
annesteenbeek 62:6c566e6f9664 31 double motor1PWM = 0;
annesteenbeek 62:6c566e6f9664 32 double motor2PWM = 0;
annesteenbeek 6:b957d8809e7c 33
annesteenbeek 32:2006977785f5 34 // Set PID values
annesteenbeek 62:6c566e6f9664 35 double Kp1 = 1;
annesteenbeek 62:6c566e6f9664 36 double Ki1 = 0;
annesteenbeek 62:6c566e6f9664 37 double Kd1 = 0;
annesteenbeek 0:525558a26464 38
annesteenbeek 69:37f75a7d36d8 39 double Kp2 = 0.008;
annesteenbeek 69:37f75a7d36d8 40 double Ki2 = 0.08;
annesteenbeek 62:6c566e6f9664 41 double Kd2 = 0;
annesteenbeek 20:3cba803cd771 42
annesteenbeek 68:21cb054f1399 43 double motor1PrevCounts = 0;
annesteenbeek 68:21cb054f1399 44 double motor2PrevCounts = 0;
annesteenbeek 62:6c566e6f9664 45 double prevTime = 0;
annesteenbeek 62:6c566e6f9664 46 double now = 0;
annesteenbeek 62:6c566e6f9664 47 double timechange;
annesteenbeek 55:ee5257fb73df 48 bool pidOut = 0;
annesteenbeek 26:0a9e4147a31a 49
annesteenbeek 93:a8898eb80edc 50 // for Calibration:
annesteenbeek 93:a8898eb80edc 51 bool calibrating1 = true;
annesteenbeek 93:a8898eb80edc 52 bool calibrating2 = false;
annesteenbeek 93:a8898eb80edc 53 double motorCalSpeed = 10; // deg/sec
annesteenbeek 93:a8898eb80edc 54 double returnSpeed = -10;
annesteenbeek 93:a8898eb80edc 55 bool springHit = false;
annesteenbeek 93:a8898eb80edc 56 float lastCall = 0;
annesteenbeek 93:a8898eb80edc 57
annesteenbeek 93:a8898eb80edc 58
annesteenbeek 32:2006977785f5 59 // Create object instances
annesteenbeek 74:75be98779124 60 // Safety Pin
annesteenbeek 79:cf500b63f349 61 DigitalIn safetyIn(safetyPin);
annesteenbeek 74:75be98779124 62
annesteenbeek 32:2006977785f5 63 // Initialze motors
annesteenbeek 32:2006977785f5 64 PwmOut motor1(motor1PWMPin);
annesteenbeek 32:2006977785f5 65 PwmOut motor2(motor2PWMPin);
annesteenbeek 32:2006977785f5 66
annesteenbeek 32:2006977785f5 67 // Initialize encoders
annesteenbeek 32:2006977785f5 68 Encoder encoder1(enc1A, enc1B);
annesteenbeek 32:2006977785f5 69 Encoder encoder2(enc2A, enc2B);
annesteenbeek 25:874675516927 70
annesteenbeek 32:2006977785f5 71 // Set direction pins
annesteenbeek 32:2006977785f5 72 DigitalOut motor1Dir(motor1DirPin);
annesteenbeek 32:2006977785f5 73 DigitalOut motor2Dir(motor2DirPin);
annesteenbeek 26:0a9e4147a31a 74
annesteenbeek 32:2006977785f5 75 // create PID instances
annesteenbeek 52:2ac9dee099ce 76 PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
annesteenbeek 52:2ac9dee099ce 77 PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
annesteenbeek 31:8fbee6c92753 78
annesteenbeek 34:f315b2b38555 79 Servo servo(servoPin);
annesteenbeek 32:2006977785f5 80 Timer t;
annesteenbeek 25:874675516927 81
annesteenbeek 25:874675516927 82 void motorInit(){
annesteenbeek 25:874675516927 83
annesteenbeek 3:47c76be6d402 84 motor1Dir.write(direction1);
annesteenbeek 3:47c76be6d402 85 motor2Dir.write(direction2);
annesteenbeek 3:47c76be6d402 86
annesteenbeek 3:47c76be6d402 87 // Set motor PWM period
annesteenbeek 3:47c76be6d402 88 motor1.period(1/pwm_frequency);
annesteenbeek 3:47c76be6d402 89 motor2.period(1/pwm_frequency);
annesteenbeek 55:ee5257fb73df 90
annesteenbeek 55:ee5257fb73df 91 motor1PID.SetSampleTime(motorCall);
annesteenbeek 55:ee5257fb73df 92 motor2PID.SetSampleTime(motorCall);
annesteenbeek 55:ee5257fb73df 93
annesteenbeek 69:37f75a7d36d8 94 motor1PID.SetOutputLimits(0,1);
annesteenbeek 69:37f75a7d36d8 95 motor2PID.SetOutputLimits(0,1);
annesteenbeek 69:37f75a7d36d8 96
annesteenbeek 30:a20f16bf8dda 97 // Turn PID on
annesteenbeek 55:ee5257fb73df 98 motor1PID.SetMode(AUTOMATIC);
annesteenbeek 55:ee5257fb73df 99 motor2PID.SetMode(AUTOMATIC);
annesteenbeek 30:a20f16bf8dda 100
annesteenbeek 30:a20f16bf8dda 101 // start the timer
annesteenbeek 29:e4f3455aaa0b 102 t.start();
annesteenbeek 4:80e2280058ed 103 }
annesteenbeek 0:525558a26464 104
annesteenbeek 26:0a9e4147a31a 105
annesteenbeek 0:525558a26464 106 void motorControl(){
annesteenbeek 30:a20f16bf8dda 107 // get encoder positions in degrees
annesteenbeek 30:a20f16bf8dda 108 // 131.25:1 gear ratio
annesteenbeek 30:a20f16bf8dda 109 // getPosition uses X2 configuration, so 32 counts per revolution
annesteenbeek 30:a20f16bf8dda 110 // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive
annesteenbeek 68:21cb054f1399 111
annesteenbeek 68:21cb054f1399 112 encoder1Counts = encoder1.getPosition();
annesteenbeek 68:21cb054f1399 113 encoder2Counts = encoder2.getPosition();
annesteenbeek 68:21cb054f1399 114
annesteenbeek 68:21cb054f1399 115
annesteenbeek 68:21cb054f1399 116 motor1Pos = -((encoder1Counts/32)/131.25)*360;
annesteenbeek 68:21cb054f1399 117 motor2Pos = -((encoder2Counts/32)/131.25)*360;
annesteenbeek 3:47c76be6d402 118
annesteenbeek 0:525558a26464 119 // check if motor's are within rotational boundarys
annesteenbeek 30:a20f16bf8dda 120 // get encoder speeds in deg/sec
annesteenbeek 66:d1ab5904f8e5 121 now = t.read();
annesteenbeek 50:b0cf07ca53cf 122 timechange = (now - prevTime);
annesteenbeek 68:21cb054f1399 123 motor1Speed = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange;
annesteenbeek 68:21cb054f1399 124 motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange;
annesteenbeek 50:b0cf07ca53cf 125 prevTime = now;
annesteenbeek 68:21cb054f1399 126 motor1PrevCounts = encoder1Counts;
annesteenbeek 68:21cb054f1399 127 motor2PrevCounts = encoder2Counts;
annesteenbeek 29:e4f3455aaa0b 128
annesteenbeek 31:8fbee6c92753 129 // calculate motor setpoint speed in deg/sec from setpoint x/y speed
annesteenbeek 30:a20f16bf8dda 130
annesteenbeek 48:3cf1eaf34926 131
annesteenbeek 48:3cf1eaf34926 132 if(motorsEnable){ // only run motors if switch is enabled
annesteenbeek 32:2006977785f5 133 // compute new PID parameters using setpoint angle speeds and encoder speed
annesteenbeek 31:8fbee6c92753 134 writeMotors();
annesteenbeek 0:525558a26464 135 }else{
annesteenbeek 0:525558a26464 136 // write 0 to motors
annesteenbeek 2:95ba9f6f0128 137 motor1.write(0);
annesteenbeek 2:95ba9f6f0128 138 motor2.write(0);
annesteenbeek 0:525558a26464 139 }
annesteenbeek 0:525558a26464 140 }
annesteenbeek 0:525558a26464 141
annesteenbeek 31:8fbee6c92753 142 void writeMotors(){
annesteenbeek 54:c14c3bc48b8a 143 motor1PID.Compute(); // calculate PID outputs, output changes automatically
annesteenbeek 76:0aa90e728e4a 144 motor2PID.Compute();
annesteenbeek 31:8fbee6c92753 145 // write new values to motor's
annesteenbeek 82:4cc8f9ad3fec 146 if (motor1SetSpeed > 0 ){ // CCW rotation
annesteenbeek 31:8fbee6c92753 147 direction1 = false;
annesteenbeek 84:df770ec4df61 148 motor1PID.SetOutputLimits(0,1); // change pid output direction
annesteenbeek 31:8fbee6c92753 149 }else{
annesteenbeek 31:8fbee6c92753 150 direction1 = true; // CW rotation
annesteenbeek 83:8fa05f53fc73 151 motor1PID.SetOutputLimits(-1,0);
annesteenbeek 31:8fbee6c92753 152 }
annesteenbeek 82:4cc8f9ad3fec 153 if (motor2SetSpeed > 0 ){ // CCW rotation
annesteenbeek 68:21cb054f1399 154 direction2 = false;
annesteenbeek 83:8fa05f53fc73 155 motor2PID.SetOutputLimits(0,1);
annesteenbeek 68:21cb054f1399 156 }else{
annesteenbeek 68:21cb054f1399 157 direction2 = true; // CW rotation
annesteenbeek 83:8fa05f53fc73 158 motor2PID.SetOutputLimits(-1,0);
annesteenbeek 68:21cb054f1399 159 }
annesteenbeek 31:8fbee6c92753 160 motor1Dir.write(direction1);
annesteenbeek 31:8fbee6c92753 161 motor2Dir.write(direction2);
annesteenbeek 69:37f75a7d36d8 162
annesteenbeek 52:2ac9dee099ce 163 motor1.write(abs(motor1PWM));
annesteenbeek 76:0aa90e728e4a 164 motor2.write(abs(motor2PWM));
annesteenbeek 31:8fbee6c92753 165 }
annesteenbeek 31:8fbee6c92753 166
annesteenbeek 0:525558a26464 167 void servoControl(){
annesteenbeek 0:525558a26464 168 // use potMeter Value to set servo angle
annesteenbeek 74:75be98779124 169 if (motorsEnable){
annesteenbeek 74:75be98779124 170 servo.write(servoPos);
annesteenbeek 74:75be98779124 171 }
annesteenbeek 0:525558a26464 172 // (optionaly calculate xy position to keep balloon in position)
annesteenbeek 0:525558a26464 173 // calculate z position using angle
annesteenbeek 0:525558a26464 174 // calculate x y translation of endpoint
annesteenbeek 0:525558a26464 175 // find new x and y speed.
annesteenbeek 0:525558a26464 176
annesteenbeek 25:874675516927 177 }
annesteenbeek 25:874675516927 178
annesteenbeek 32:2006977785f5 179 void calibrateMotors(){
annesteenbeek 93:a8898eb80edc 180 safetyOn = false;
annesteenbeek 93:a8898eb80edc 181 redLed.write(0); greenLed.write(1); blueLed.write(1);
annesteenbeek 93:a8898eb80edc 182 while (calibrating1 || calibrating2){
annesteenbeek 93:a8898eb80edc 183 if (calibrating1){
annesteenbeek 93:a8898eb80edc 184 motor1SetSpeed = motorCalSpeed; // deg/sec
annesteenbeek 93:a8898eb80edc 185 redLed.write(1); greenLed.write(0); blueLed.write(1);
annesteenbeek 93:a8898eb80edc 186 if(safetyIn.read() !=1){ // check if arm reached safety position
annesteenbeek 93:a8898eb80edc 187 encoder1.setPosition(0); // set motor 1 cal angle
annesteenbeek 93:a8898eb80edc 188 motor1SetSpeed = returnSpeed; // move away
annesteenbeek 93:a8898eb80edc 189 springHit = true;
annesteenbeek 93:a8898eb80edc 190 }else{
annesteenbeek 93:a8898eb80edc 191 if(springHit){ // if hit and after is no longer touching spring
annesteenbeek 93:a8898eb80edc 192 motor1SetSpeed = 0;
annesteenbeek 93:a8898eb80edc 193 springHit = false;
annesteenbeek 93:a8898eb80edc 194 calibrating1 = false;
annesteenbeek 93:a8898eb80edc 195 calibrating2 = true; // start calibrating 2
annesteenbeek 93:a8898eb80edc 196 }
annesteenbeek 93:a8898eb80edc 197 }
annesteenbeek 93:a8898eb80edc 198 }
annesteenbeek 93:a8898eb80edc 199 if (calibrating2){
annesteenbeek 93:a8898eb80edc 200 motor2SetSpeed = motorCalSpeed;
annesteenbeek 93:a8898eb80edc 201 redLed.write(1); greenLed.write(1); blueLed.write(0);
annesteenbeek 93:a8898eb80edc 202 if(safetyIn.read() !=1){ // check if arm reached safety position
annesteenbeek 93:a8898eb80edc 203 encoder2.setPosition(0); // set motor 2 cal angle
annesteenbeek 93:a8898eb80edc 204 motor2SetSpeed = returnSpeed; // move away
annesteenbeek 93:a8898eb80edc 205 springHit = true;
annesteenbeek 93:a8898eb80edc 206 }else{
annesteenbeek 93:a8898eb80edc 207 if(springHit){ // if hit and after is no longer touching spring
annesteenbeek 93:a8898eb80edc 208 motor2SetSpeed = 0;
annesteenbeek 93:a8898eb80edc 209 springHit = false;
annesteenbeek 93:a8898eb80edc 210 calibrating2 = false; // stop calibrating 2
annesteenbeek 93:a8898eb80edc 211 }
annesteenbeek 93:a8898eb80edc 212 }
annesteenbeek 93:a8898eb80edc 213 }
annesteenbeek 93:a8898eb80edc 214 now = t.read(); // call motor using timer instead of wait
annesteenbeek 93:a8898eb80edc 215 if(now - lastCall > motorCall){
annesteenbeek 93:a8898eb80edc 216 writeMotors();
annesteenbeek 93:a8898eb80edc 217 lastCall = now;
annesteenbeek 93:a8898eb80edc 218 }
annesteenbeek 93:a8898eb80edc 219
annesteenbeek 93:a8898eb80edc 220 }
annesteenbeek 76:0aa90e728e4a 221 safetyOn = true; // turn safety on after callibration
annesteenbeek 74:75be98779124 222 }
annesteenbeek 74:75be98779124 223
annesteenbeek 74:75be98779124 224
annesteenbeek 74:75be98779124 225 void safety(){
annesteenbeek 74:75be98779124 226 if (safetyOn){
annesteenbeek 81:71e7e98deb2c 227 if (safetyIn.read() != 1){
annesteenbeek 79:cf500b63f349 228 motorsEnable = false;
annesteenbeek 79:cf500b63f349 229 }
annesteenbeek 74:75be98779124 230 }
annesteenbeek 79:cf500b63f349 231 }