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@98:25528494287d, 2015-10-22 (annotated)

Committer:
annesteenbeek
Date:
Thu Oct 22 10:06:46 2015 +0200
Revision:
98:25528494287d
Parent:
91:6bbbbc6643c8
Child:
99:7030e9790b1d
added EMG and implemented Servo

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