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@104:750d7e13137d, 2015-10-23 (annotated)

Committer:
bjornnijhuis
Date:
Fri Oct 23 12:17:29 2015 +0000
Revision:
104:750d7e13137d
Parent:
103:4a37d19e8fcc
Child:
105:663b73bb2f81
Debugging copy: DO NOT USE FOR WORKING!!

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