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@106:1773bf7b95c5, 2015-10-26 (annotated)

Committer:
annesteenbeek
Date:
Mon Oct 26 12:46:29 2015 +0100
Revision:
106:1773bf7b95c5
Parent:
100:222c27f55b85
Child:
107:de47331612d9
before implementation of servo without PWM

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