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@74:75be98779124, 2015-10-20 (annotated)

Committer:
annesteenbeek
Date:
Tue Oct 20 12:14:18 2015 +0200
Revision:
74:75be98779124
Parent:
69:37f75a7d36d8
Child:
76:0aa90e728e4a
connected safety to interrupt pin

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