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@115:d8d2968981f3, 2015-10-27 (annotated)

Committer:
annesteenbeek
Date:
Tue Oct 27 16:21:20 2015 +0100
Revision:
115:d8d2968981f3
Parent:
110:a6439e13be8b
Child:
116:8b812e268b85
connected calibration with ticker, when calibration ready, start rest of tickers

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