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@88:dd198c58f053, 2015-10-20 (annotated)

Committer:
annesteenbeek
Date:
Tue Oct 20 16:31:22 2015 +0200
Revision:
88:dd198c58f053
Parent:
87:7cc7d4d68ef7
Child:
89:9063484e8a1b
fixed double pin defs

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 34:f315b2b38555 7 #include "Servo.h"
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 32:2006977785f5 49 // Create object instances
annesteenbeek 74:75be98779124 50 // Safety Pin
annesteenbeek 79:cf500b63f349 51 DigitalIn safetyIn(safetyPin);
annesteenbeek 74:75be98779124 52
annesteenbeek 32:2006977785f5 53 // Initialze motors
annesteenbeek 32:2006977785f5 54 PwmOut motor1(motor1PWMPin);
annesteenbeek 32:2006977785f5 55 PwmOut motor2(motor2PWMPin);
annesteenbeek 32:2006977785f5 56
annesteenbeek 32:2006977785f5 57 // Initialize encoders
annesteenbeek 32:2006977785f5 58 Encoder encoder1(enc1A, enc1B);
annesteenbeek 32:2006977785f5 59 Encoder encoder2(enc2A, enc2B);
annesteenbeek 25:874675516927 60
annesteenbeek 32:2006977785f5 61 // Set direction pins
annesteenbeek 32:2006977785f5 62 DigitalOut motor1Dir(motor1DirPin);
annesteenbeek 32:2006977785f5 63 DigitalOut motor2Dir(motor2DirPin);
annesteenbeek 26:0a9e4147a31a 64
annesteenbeek 32:2006977785f5 65 // create PID instances
annesteenbeek 52:2ac9dee099ce 66 PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1);
annesteenbeek 52:2ac9dee099ce 67 PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2);
annesteenbeek 31:8fbee6c92753 68
annesteenbeek 34:f315b2b38555 69 Servo servo(servoPin);
annesteenbeek 32:2006977785f5 70 Timer t;
annesteenbeek 25:874675516927 71
annesteenbeek 25:874675516927 72 void motorInit(){
annesteenbeek 25:874675516927 73
annesteenbeek 3:47c76be6d402 74 motor1Dir.write(direction1);
annesteenbeek 3:47c76be6d402 75 motor2Dir.write(direction2);
annesteenbeek 3:47c76be6d402 76
annesteenbeek 3:47c76be6d402 77 // Set motor PWM period
annesteenbeek 3:47c76be6d402 78 motor1.period(1/pwm_frequency);
annesteenbeek 3:47c76be6d402 79 motor2.period(1/pwm_frequency);
annesteenbeek 55:ee5257fb73df 80
annesteenbeek 55:ee5257fb73df 81 motor1PID.SetSampleTime(motorCall);
annesteenbeek 55:ee5257fb73df 82 motor2PID.SetSampleTime(motorCall);
annesteenbeek 55:ee5257fb73df 83
annesteenbeek 69:37f75a7d36d8 84 motor1PID.SetOutputLimits(0,1);
annesteenbeek 69:37f75a7d36d8 85 motor2PID.SetOutputLimits(0,1);
annesteenbeek 69:37f75a7d36d8 86
annesteenbeek 30:a20f16bf8dda 87 // Turn PID on
annesteenbeek 55:ee5257fb73df 88 motor1PID.SetMode(AUTOMATIC);
annesteenbeek 55:ee5257fb73df 89 motor2PID.SetMode(AUTOMATIC);
annesteenbeek 30:a20f16bf8dda 90
annesteenbeek 30:a20f16bf8dda 91 // start the timer
annesteenbeek 29:e4f3455aaa0b 92 t.start();
annesteenbeek 4:80e2280058ed 93 }
annesteenbeek 0:525558a26464 94
annesteenbeek 26:0a9e4147a31a 95
annesteenbeek 0:525558a26464 96 void motorControl(){
annesteenbeek 30:a20f16bf8dda 97 // get encoder positions in degrees
annesteenbeek 30:a20f16bf8dda 98 // 131.25:1 gear ratio
annesteenbeek 30:a20f16bf8dda 99 // getPosition uses X2 configuration, so 32 counts per revolution
annesteenbeek 30:a20f16bf8dda 100 // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive
annesteenbeek 68:21cb054f1399 101
annesteenbeek 68:21cb054f1399 102 encoder1Counts = encoder1.getPosition();
annesteenbeek 68:21cb054f1399 103 encoder2Counts = encoder2.getPosition();
annesteenbeek 68:21cb054f1399 104
annesteenbeek 68:21cb054f1399 105
annesteenbeek 68:21cb054f1399 106 motor1Pos = -((encoder1Counts/32)/131.25)*360;
annesteenbeek 68:21cb054f1399 107 motor2Pos = -((encoder2Counts/32)/131.25)*360;
annesteenbeek 3:47c76be6d402 108
annesteenbeek 0:525558a26464 109 // check if motor's are within rotational boundarys
annesteenbeek 30:a20f16bf8dda 110 // get encoder speeds in deg/sec
annesteenbeek 66:d1ab5904f8e5 111 now = t.read();
annesteenbeek 50:b0cf07ca53cf 112 timechange = (now - prevTime);
annesteenbeek 68:21cb054f1399 113 motor1Speed = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange;
annesteenbeek 68:21cb054f1399 114 motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange;
annesteenbeek 50:b0cf07ca53cf 115 prevTime = now;
annesteenbeek 68:21cb054f1399 116 motor1PrevCounts = encoder1Counts;
annesteenbeek 68:21cb054f1399 117 motor2PrevCounts = encoder2Counts;
annesteenbeek 29:e4f3455aaa0b 118
annesteenbeek 31:8fbee6c92753 119 // calculate motor setpoint speed in deg/sec from setpoint x/y speed
annesteenbeek 30:a20f16bf8dda 120
annesteenbeek 48:3cf1eaf34926 121
annesteenbeek 48:3cf1eaf34926 122 if(motorsEnable){ // only run motors if switch is enabled
annesteenbeek 32:2006977785f5 123 // compute new PID parameters using setpoint angle speeds and encoder speed
annesteenbeek 31:8fbee6c92753 124 writeMotors();
annesteenbeek 0:525558a26464 125 }else{
annesteenbeek 0:525558a26464 126 // write 0 to motors
annesteenbeek 2:95ba9f6f0128 127 motor1.write(0);
annesteenbeek 2:95ba9f6f0128 128 motor2.write(0);
annesteenbeek 0:525558a26464 129 }
annesteenbeek 0:525558a26464 130 }
annesteenbeek 0:525558a26464 131
annesteenbeek 31:8fbee6c92753 132 void writeMotors(){
annesteenbeek 54:c14c3bc48b8a 133 motor1PID.Compute(); // calculate PID outputs, output changes automatically
annesteenbeek 76:0aa90e728e4a 134 motor2PID.Compute();
annesteenbeek 31:8fbee6c92753 135 // write new values to motor's
annesteenbeek 82:4cc8f9ad3fec 136 if (motor1SetSpeed > 0 ){ // CCW rotation
annesteenbeek 31:8fbee6c92753 137 direction1 = false;
annesteenbeek 84:df770ec4df61 138 motor1PID.SetOutputLimits(0,1); // change pid output direction
annesteenbeek 31:8fbee6c92753 139 }else{
annesteenbeek 31:8fbee6c92753 140 direction1 = true; // CW rotation
annesteenbeek 83:8fa05f53fc73 141 motor1PID.SetOutputLimits(-1,0);
annesteenbeek 31:8fbee6c92753 142 }
annesteenbeek 82:4cc8f9ad3fec 143 if (motor2SetSpeed > 0 ){ // CCW rotation
annesteenbeek 68:21cb054f1399 144 direction2 = false;
annesteenbeek 83:8fa05f53fc73 145 motor2PID.SetOutputLimits(0,1);
annesteenbeek 68:21cb054f1399 146 }else{
annesteenbeek 68:21cb054f1399 147 direction2 = true; // CW rotation
annesteenbeek 83:8fa05f53fc73 148 motor2PID.SetOutputLimits(-1,0);
annesteenbeek 68:21cb054f1399 149 }
annesteenbeek 31:8fbee6c92753 150 motor1Dir.write(direction1);
annesteenbeek 31:8fbee6c92753 151 motor2Dir.write(direction2);
annesteenbeek 69:37f75a7d36d8 152
annesteenbeek 52:2ac9dee099ce 153 motor1.write(abs(motor1PWM));
annesteenbeek 76:0aa90e728e4a 154 motor2.write(abs(motor2PWM));
annesteenbeek 31:8fbee6c92753 155 }
annesteenbeek 31:8fbee6c92753 156
annesteenbeek 0:525558a26464 157 void servoControl(){
annesteenbeek 0:525558a26464 158 // use potMeter Value to set servo angle
annesteenbeek 74:75be98779124 159 if (motorsEnable){
annesteenbeek 74:75be98779124 160 servo.write(servoPos);
annesteenbeek 74:75be98779124 161 }
annesteenbeek 0:525558a26464 162 // (optionaly calculate xy position to keep balloon in position)
annesteenbeek 0:525558a26464 163 // calculate z position using angle
annesteenbeek 0:525558a26464 164 // calculate x y translation of endpoint
annesteenbeek 0:525558a26464 165 // find new x and y speed.
annesteenbeek 0:525558a26464 166
annesteenbeek 25:874675516927 167 }
annesteenbeek 25:874675516927 168
annesteenbeek 32:2006977785f5 169 void calibrateMotors(){
annesteenbeek 85:03dc24043236 170 bool calibrating1 = true;
annesteenbeek 85:03dc24043236 171 bool calibrating2 = true;
annesteenbeek 85:03dc24043236 172 double motorCalSpeed = 10; // deg/sec
annesteenbeek 85:03dc24043236 173 while (calibrating1 || calibrating2){
annesteenbeek 85:03dc24043236 174 if (calibrating1){
annesteenbeek 85:03dc24043236 175 motor1SetSpeed = motorCalSpeed; // deg/sec
annesteenbeek 85:03dc24043236 176 if(safetyIn.read() !=1){ // check if arm reached safety position
annesteenbeek 85:03dc24043236 177 calibrating1 = false; // done
annesteenbeek 85:03dc24043236 178 motor1SetSpeed = 0; // brake motor
annesteenbeek 86:a5f48ae7096e 179 encoder1.setPosition(0); // set motor 1 cal angle
annesteenbeek 87:7cc7d4d68ef7 180 // move away
annesteenbeek 85:03dc24043236 181 }
annesteenbeek 85:03dc24043236 182 }
annesteenbeek 85:03dc24043236 183 if (calibrating1){
annesteenbeek 85:03dc24043236 184 motor2SetSpeed = motorCalSpeed; // deg/sec
annesteenbeek 85:03dc24043236 185 if(safetyIn.read() !=1){ // check if arm reached safety position
annesteenbeek 85:03dc24043236 186 calibrating2 = false; // done
annesteenbeek 85:03dc24043236 187 motor2SetSpeed = 0; // brake motor
annesteenbeek 86:a5f48ae7096e 188 encoder2.setPosition(0); // set motor 2 cal angle
annesteenbeek 87:7cc7d4d68ef7 189 // move away
annesteenbeek 85:03dc24043236 190 }
annesteenbeek 85:03dc24043236 191 }
annesteenbeek 87:7cc7d4d68ef7 192 writeMotors();
annesteenbeek 88:dd198c58f053 193 wait(0.01f);
annesteenbeek 85:03dc24043236 194 }
annesteenbeek 76:0aa90e728e4a 195 safetyOn = true; // turn safety on after callibration
annesteenbeek 74:75be98779124 196 }
annesteenbeek 74:75be98779124 197
annesteenbeek 74:75be98779124 198
annesteenbeek 74:75be98779124 199 void safety(){
annesteenbeek 74:75be98779124 200 if (safetyOn){
annesteenbeek 81:71e7e98deb2c 201 if (safetyIn.read() != 1){
annesteenbeek 79:cf500b63f349 202 motorsEnable = false;
annesteenbeek 79:cf500b63f349 203 }
annesteenbeek 74:75be98779124 204 }
annesteenbeek 79:cf500b63f349 205 }