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