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@105:663b73bb2f81, 2015-10-26 (annotated)

Committer:
MartijnGJ
Date:
Mon Oct 26 11:25:25 2015 +0000
Revision:
105:663b73bb2f81
Parent:
104:750d7e13137d 
changed filter settings, fixed shaking cables

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
MartijnGJ 105:663b73bb2f81 9 // functions for controlling the motors
MartijnGJ 105:663b73bb2f81 10 bool motorsEnable = false;
MartijnGJ 105:663b73bb2f81 11 bool safetyOn = true; // start with safety off for calibration
annesteenbeek 76:0aa90e728e4a 12
annesteenbeek 6:b957d8809e7c 13
MartijnGJ 105:663b73bb2f81 14 double encoder1Counts = 0;
MartijnGJ 105:663b73bb2f81 15 double encoder2Counts = 0;
MartijnGJ 105:663b73bb2f81 16
MartijnGJ 105:663b73bb2f81 17 bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation)
MartijnGJ 105:663b73bb2f81 18 bool direction2 = false;
annesteenbeek 6:b957d8809e7c 19
MartijnGJ 105:663b73bb2f81 20 double motor1Pos = 0;
MartijnGJ 105:663b73bb2f81 21 double motor2Pos = 0;
annesteenbeek 6:b957d8809e7c 22
MartijnGJ 105:663b73bb2f81 23 double motor1Speed = 0;
MartijnGJ 105:663b73bb2f81 24 double motor2Speed = 0;
annesteenbeek 6:b957d8809e7c 25
MartijnGJ 105:663b73bb2f81 26 double motor1SetSpeed = 0;
MartijnGJ 105:663b73bb2f81 27 double motor2SetSpeed = 0;
annesteenbeek 6:b957d8809e7c 28
MartijnGJ 105:663b73bb2f81 29 double servoPos = 0;
MartijnGJ 105:663b73bb2f81 30
MartijnGJ 105:663b73bb2f81 31 double motor1PWM = 0;
MartijnGJ 105:663b73bb2f81 32 double motor2PWM = 0;
annesteenbeek 6:b957d8809e7c 33
MartijnGJ 105:663b73bb2f81 34 // Set PID values
MartijnGJ 105:663b73bb2f81 35 double Kp1 = 0.008;
MartijnGJ 105:663b73bb2f81 36 double Ki1 = 0.08;
MartijnGJ 105:663b73bb2f81 37 double Kd1 = 0;
annesteenbeek 0:525558a26464 38
MartijnGJ 105:663b73bb2f81 39 double Kp2 = 0.008;
MartijnGJ 105:663b73bb2f81 40 double Ki2 = 0.08;
MartijnGJ 105:663b73bb2f81 41 double Kd2 = 0;
annesteenbeek 20:3cba803cd771 42
MartijnGJ 105:663b73bb2f81 43 double motor1PrevCounts = 0;
MartijnGJ 105:663b73bb2f81 44 double motor2PrevCounts = 0;
MartijnGJ 105:663b73bb2f81 45 double prevTime = 0;
MartijnGJ 105:663b73bb2f81 46 double now = 0;
MartijnGJ 105:663b73bb2f81 47 double timechange;
MartijnGJ 105:663b73bb2f81 48 bool pidOut = 0;
annesteenbeek 26:0a9e4147a31a 49
MartijnGJ 105:663b73bb2f81 50 // Set servo values
MartijnGJ 105:663b73bb2f81 51 const double servoPeriod = 0.020;
MartijnGJ 105:663b73bb2f81 52 const double servo_range = 20; // Servo range (-range/ range) [deg]
MartijnGJ 105:663b73bb2f81 53 const double servo_vel = 15; // Servo velocity [deg/s]
MartijnGJ 105:663b73bb2f81 54 const double servo_inc = servo_vel * motorCall; // Servo postion increment per cycle
MartijnGJ 105:663b73bb2f81 55 double servo_pos = 0;
MartijnGJ 105:663b73bb2f81 56 double servoPulsewidth = 0.0015;
MartijnGJ 105:663b73bb2f81 57 double servoSpeed = 15;
MartijnGJ 105:663b73bb2f81 58 double scaleXSpeed = 10;
MartijnGJ 105:663b73bb2f81 59 double scaleYSpeed = 20;
MartijnGJ 105:663b73bb2f81 60 double scaleZSpeed = 1;
MartijnGJ 105:663b73bb2f81 61
MartijnGJ 105:663b73bb2f81 62
MartijnGJ 105:663b73bb2f81 63
MartijnGJ 105:663b73bb2f81 64 // Set calibration values
MartijnGJ 105:663b73bb2f81 65 double motorCalSpeed = 10; // deg/sec
MartijnGJ 105:663b73bb2f81 66 double returnSpeed = -10;
MartijnGJ 105:663b73bb2f81 67 bool springHit = false;
MartijnGJ 105:663b73bb2f81 68 float lastCall = 0;
MartijnGJ 105:663b73bb2f81 69 bool calibrating1 = true;
MartijnGJ 105:663b73bb2f81 70 bool calibrating2 = false;
annesteenbeek 98:25528494287d 71
MartijnGJ 105:663b73bb2f81 72 // Create object instances
MartijnGJ 105:663b73bb2f81 73 // Safety Pin
MartijnGJ 105:663b73bb2f81 74 DigitalIn safetyIn(safetyPin);
MartijnGJ 105:663b73bb2f81 75
MartijnGJ 105:663b73bb2f81 76 // Initialze motors
MartijnGJ 105:663b73bb2f81 77 PwmOut motor1(motor1PWMPin);
MartijnGJ 105:663b73bb2f81 78 PwmOut motor2(motor2PWMPin);
MartijnGJ 105:663b73bb2f81 79
MartijnGJ 105:663b73bb2f81 80 // initialize Servo
MartijnGJ 105:663b73bb2f81 81 PwmOut servo(servoPin);
annesteenbeek 98:25528494287d 82
annesteenbeek 98:25528494287d 83
MartijnGJ 105:663b73bb2f81 84 // Initialize encoders
MartijnGJ 105:663b73bb2f81 85 Encoder encoder1(enc1A, enc1B);
MartijnGJ 105:663b73bb2f81 86 Encoder encoder2(enc2A, enc2B);
annesteenbeek 25:874675516927 87
MartijnGJ 105:663b73bb2f81 88 // Set direction pins
MartijnGJ 105:663b73bb2f81 89 DigitalOut motor1Dir(motor1DirPin);
MartijnGJ 105:663b73bb2f81 90 DigitalOut motor2Dir(motor2DirPin);
annesteenbeek 26:0a9e4147a31a 91
MartijnGJ 105:663b73bb2f81 92 // create PID instances
MartijnGJ 105:663b73bb2f81 93 PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
MartijnGJ 105:663b73bb2f81 94 PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
annesteenbeek 31:8fbee6c92753 95
MartijnGJ 105:663b73bb2f81 96 Timer t;
MartijnGJ 105:663b73bb2f81 97
MartijnGJ 105:663b73bb2f81 98 void motorInit(){
MartijnGJ 105:663b73bb2f81 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);
MartijnGJ 105:663b73bb2f81 106
annesteenbeek 55:ee5257fb73df 107 motor1PID.SetSampleTime(motorCall);
annesteenbeek 55:ee5257fb73df 108 motor2PID.SetSampleTime(motorCall);
MartijnGJ 105:663b73bb2f81 109
annesteenbeek 69:37f75a7d36d8 110 motor1PID.SetOutputLimits(0,1);
annesteenbeek 69:37f75a7d36d8 111 motor2PID.SetOutputLimits(0,1);
MartijnGJ 105:663b73bb2f81 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
MartijnGJ 105:663b73bb2f81 118 servo.period(servoPeriod);
annesteenbeek 98:25528494287d 119
MartijnGJ 105:663b73bb2f81 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
MartijnGJ 105:663b73bb2f81 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
MartijnGJ 105:663b73bb2f81 134 // 131.25:1 gear ratio
MartijnGJ 105:663b73bb2f81 135 // getPosition uses X2 configuration, so 32 counts per revolution
MartijnGJ 105:663b73bb2f81 136 // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive
annesteenbeek 68:21cb054f1399 137
MartijnGJ 105:663b73bb2f81 138 encoder1Counts = encoder1.getPosition();
MartijnGJ 105:663b73bb2f81 139 encoder2Counts = encoder2.getPosition();
annesteenbeek 68:21cb054f1399 140
annesteenbeek 68:21cb054f1399 141
MartijnGJ 105:663b73bb2f81 142 motor1Pos = -((encoder1Counts/32)/131.25)*360;
MartijnGJ 105:663b73bb2f81 143 motor2Pos = -((encoder2Counts/32)/131.25)*360;
annesteenbeek 3:47c76be6d402 144
MartijnGJ 105:663b73bb2f81 145 // check if motor's are within rotational boundarys
annesteenbeek 30:a20f16bf8dda 146 // get encoder speeds in deg/sec
MartijnGJ 105:663b73bb2f81 147 now = t.read();
MartijnGJ 105:663b73bb2f81 148 timechange = (now - prevTime);
MartijnGJ 105:663b73bb2f81 149 motor1Speed = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange;
MartijnGJ 105:663b73bb2f81 150 motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange;
MartijnGJ 105:663b73bb2f81 151 prevTime = now;
MartijnGJ 105:663b73bb2f81 152 motor1PrevCounts = encoder1Counts;
MartijnGJ 105:663b73bb2f81 153 motor2PrevCounts = encoder2Counts;
MartijnGJ 105:663b73bb2f81 154
annesteenbeek 31:8fbee6c92753 155 // calculate motor setpoint speed in deg/sec from setpoint x/y speed
annesteenbeek 30:a20f16bf8dda 156
MartijnGJ 105:663b73bb2f81 157
MartijnGJ 105:663b73bb2f81 158 if(motorsEnable){ // only run motors if switch is enabled
MartijnGJ 105:663b73bb2f81 159 // compute new PID parameters using setpoint angle speeds and encoder speed
annesteenbeek 31:8fbee6c92753 160 writeMotors();
MartijnGJ 105:663b73bb2f81 161 servoControl();
MartijnGJ 105:663b73bb2f81 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
MartijnGJ 105:663b73bb2f81 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
MartijnGJ 105:663b73bb2f81 173 if (motor1SetSpeed > 0 ){ // CCW rotation
annesteenbeek 31:8fbee6c92753 174 direction1 = false;
annesteenbeek 84:df770ec4df61 175 motor1PID.SetOutputLimits(0,1); // change pid output direction
MartijnGJ 105:663b73bb2f81 176 }else{
annesteenbeek 31:8fbee6c92753 177 direction1 = true; // CW rotation
annesteenbeek 83:8fa05f53fc73 178 motor1PID.SetOutputLimits(-1,0);
annesteenbeek 31:8fbee6c92753 179 }
MartijnGJ 105:663b73bb2f81 180 if (motor2SetSpeed > 0 ){ // CCW rotation
annesteenbeek 68:21cb054f1399 181 direction2 = false;
annesteenbeek 83:8fa05f53fc73 182 motor2PID.SetOutputLimits(0,1);
MartijnGJ 105:663b73bb2f81 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
MartijnGJ 105:663b73bb2f81 190 motor1.write(abs(motor1PWM));
MartijnGJ 105:663b73bb2f81 191 motor2.write(abs(motor2PWM));
annesteenbeek 31:8fbee6c92753 192 }
annesteenbeek 31:8fbee6c92753 193
MartijnGJ 105:663b73bb2f81 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 }
MartijnGJ 105:663b73bb2f81 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 }
MartijnGJ 105:663b73bb2f81 204 servoPulsewidth = 0.0015 + (servo_pos/90)*0.001;
MartijnGJ 105:663b73bb2f81 205 servo.pulsewidth(servoPulsewidth);
annesteenbeek 25:874675516927 206 }
annesteenbeek 25:874675516927 207
MartijnGJ 105:663b73bb2f81 208 void calibrateMotors(){
MartijnGJ 105:663b73bb2f81 209 safetyOn = false; // safety springs off
MartijnGJ 105:663b73bb2f81 210 motorsEnable = true; // motors on
MartijnGJ 105:663b73bb2f81 211 redLed.write(0); greenLed.write(1); blueLed.write(1);
MartijnGJ 105:663b73bb2f81 212 while (calibrating1 || calibrating2){
MartijnGJ 105:663b73bb2f81 213 if (calibrating1){
MartijnGJ 105:663b73bb2f81 214 redLed.write(1); greenLed.write(0); blueLed.write(1);
MartijnGJ 105:663b73bb2f81 215 if(safetyIn.read() !=1){ // check if arm reached safety position
MartijnGJ 105:663b73bb2f81 216 encoder1.setPosition(0); // set motor 1 cal angle
MartijnGJ 105:663b73bb2f81 217 motor1SetSpeed = returnSpeed; // move away
MartijnGJ 105:663b73bb2f81 218 springHit = true;
MartijnGJ 105:663b73bb2f81 219 }else{
MartijnGJ 105:663b73bb2f81 220 if(springHit){ // if hit and after is no longer touching spring
MartijnGJ 105:663b73bb2f81 221 motor1SetSpeed = 0;
MartijnGJ 105:663b73bb2f81 222 springHit = false;
MartijnGJ 105:663b73bb2f81 223 calibrating1 = false;
MartijnGJ 105:663b73bb2f81 224 calibrating2 = true; // start calibrating 2
MartijnGJ 105:663b73bb2f81 225 }
MartijnGJ 105:663b73bb2f81 226 }
MartijnGJ 105:663b73bb2f81 227 }
MartijnGJ 105:663b73bb2f81 228 if (calibrating2){
MartijnGJ 105:663b73bb2f81 229 motor2SetSpeed = motorCalSpeed;
MartijnGJ 105:663b73bb2f81 230 redLed.write(1); greenLed.write(1); blueLed.write(0);
MartijnGJ 105:663b73bb2f81 231 if(safetyIn.read() !=1){ // check if arm reached safety position
MartijnGJ 105:663b73bb2f81 232 encoder2.setPosition(0); // set motor 2 cal angle
MartijnGJ 105:663b73bb2f81 233 motor2SetSpeed = returnSpeed; // move away
MartijnGJ 105:663b73bb2f81 234 springHit = true;
MartijnGJ 105:663b73bb2f81 235 }else{
MartijnGJ 105:663b73bb2f81 236 if(springHit){ // if hit and after is no longer touching spring
MartijnGJ 105:663b73bb2f81 237 motor2SetSpeed = 0;
MartijnGJ 105:663b73bb2f81 238 springHit = false;
MartijnGJ 105:663b73bb2f81 239 calibrating2 = false; // stop calibrating 2
MartijnGJ 105:663b73bb2f81 240 }
MartijnGJ 105:663b73bb2f81 241 }
MartijnGJ 105:663b73bb2f81 242 }
MartijnGJ 105:663b73bb2f81 243 now = t.read(); // call motor using timer instead of wait
MartijnGJ 105:663b73bb2f81 244 if(now - lastCall > motorCall){
MartijnGJ 105:663b73bb2f81 245 motorControl();
MartijnGJ 105:663b73bb2f81 246 lastCall = now;
MartijnGJ 105:663b73bb2f81 247 }
annesteenbeek 98:25528494287d 248
MartijnGJ 105:663b73bb2f81 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
MartijnGJ 105:663b73bb2f81 255 void safety(){
MartijnGJ 105:663b73bb2f81 256 if (safetyOn){
MartijnGJ 105:663b73bb2f81 257 if (safetyIn.read() != 1){
annesteenbeek 79:cf500b63f349 258 motorsEnable = false;
annesteenbeek 79:cf500b63f349 259 }
annesteenbeek 74:75be98779124 260 }
annesteenbeek 79:cf500b63f349 261 }