control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
actuators.cpp@130:2542f844ba1e, 2015-10-30 (annotated)
- Committer:
- annesteenbeek
- Date:
- Fri Oct 30 10:32:49 2015 +0100
- Revision:
- 130:2542f844ba1e
- Parent:
- 127:831f03471efb
changes
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 | 107:de47331612d9 | 7 | #include "buttons.h" |
annesteenbeek | 107:de47331612d9 | 8 | #include "Servo.h" |
annesteenbeek | 98:25528494287d | 9 | |
annesteenbeek | 109:026abd708dce | 10 | // Motor control constants |
annesteenbeek | 109:026abd708dce | 11 | #define pwm_frequency 50000 // still High, could be lowered |
annesteenbeek | 109:026abd708dce | 12 | #define PI 3.14159265 |
annesteenbeek | 106:1773bf7b95c5 | 13 | |
annesteenbeek | 109:026abd708dce | 14 | // functions for controlling the motors |
annesteenbeek | 109:026abd708dce | 15 | bool motorsEnable = false; |
annesteenbeek | 109:026abd708dce | 16 | bool safetyOn = true; |
annesteenbeek | 76:0aa90e728e4a | 17 | |
annesteenbeek | 6:b957d8809e7c | 18 | |
annesteenbeek | 109:026abd708dce | 19 | double encoder1Counts = 0; |
annesteenbeek | 109:026abd708dce | 20 | double encoder2Counts = 0; |
annesteenbeek | 68:21cb054f1399 | 21 | |
annesteenbeek | 109:026abd708dce | 22 | bool direction1 = false; // CCW is false(positive rotation), CW is true (neg rotation) |
annesteenbeek | 109:026abd708dce | 23 | bool direction2 = false; |
annesteenbeek | 6:b957d8809e7c | 24 | |
annesteenbeek | 109:026abd708dce | 25 | double motor1Pos = 0; |
annesteenbeek | 109:026abd708dce | 26 | double motor2Pos = 0; |
annesteenbeek | 6:b957d8809e7c | 27 | |
annesteenbeek | 109:026abd708dce | 28 | double motor1Speed = 0; |
annesteenbeek | 109:026abd708dce | 29 | double motor2Speed = 0; |
annesteenbeek | 6:b957d8809e7c | 30 | |
annesteenbeek | 109:026abd708dce | 31 | double motor1SetSpeed = 0; |
annesteenbeek | 109:026abd708dce | 32 | double motor2SetSpeed = 0; |
annesteenbeek | 6:b957d8809e7c | 33 | |
annesteenbeek | 118:49605b5bd802 | 34 | double setXSpeed = 0; |
annesteenbeek | 118:49605b5bd802 | 35 | double setYSpeed = 0; |
annesteenbeek | 118:49605b5bd802 | 36 | |
annesteenbeek | 109:026abd708dce | 37 | double motor1PWM = 0; |
annesteenbeek | 109:026abd708dce | 38 | double motor2PWM = 0; |
annesteenbeek | 6:b957d8809e7c | 39 | |
annesteenbeek | 109:026abd708dce | 40 | // Set PID values |
annesteenbeek | 124:f67ce69557db | 41 | double Kp1 = 0.008; |
annesteenbeek | 124:f67ce69557db | 42 | double Ki1 = 0.08; |
annesteenbeek | 109:026abd708dce | 43 | double Kd1 = 0; |
annesteenbeek | 0:525558a26464 | 44 | |
annesteenbeek | 109:026abd708dce | 45 | double Kp2 = 0.008; |
annesteenbeek | 109:026abd708dce | 46 | double Ki2 = 0.08; |
annesteenbeek | 109:026abd708dce | 47 | double Kd2 = 0; |
annesteenbeek | 20:3cba803cd771 | 48 | |
annesteenbeek | 109:026abd708dce | 49 | double motor1PrevCounts = 0; |
annesteenbeek | 109:026abd708dce | 50 | double motor2PrevCounts = 0; |
annesteenbeek | 109:026abd708dce | 51 | double prevTime = 0; |
annesteenbeek | 109:026abd708dce | 52 | double now = 0; |
annesteenbeek | 109:026abd708dce | 53 | double timechange; |
annesteenbeek | 109:026abd708dce | 54 | |
annesteenbeek | 109:026abd708dce | 55 | // Set servo values |
annesteenbeek | 110:a6439e13be8b | 56 | double servoSpeed = 0; |
annesteenbeek | 109:026abd708dce | 57 | const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg |
annesteenbeek | 26:0a9e4147a31a | 58 | |
annesteenbeek | 109:026abd708dce | 59 | // Create object instances |
annesteenbeek | 109:026abd708dce | 60 | // Safety Pin |
annesteenbeek | 109:026abd708dce | 61 | DigitalIn safetyIn(safetyPin); |
annesteenbeek |
32:2006977785f5 | 62 | |
annesteenbeek | 109:026abd708dce | 63 | // Initialze motors |
annesteenbeek | 109:026abd708dce | 64 | PwmOut motor1(motor1PWMPin); |
annesteenbeek | 109:026abd708dce | 65 | PwmOut motor2(motor2PWMPin); |
annesteenbeek | 109:026abd708dce | 66 | |
annesteenbeek | 109:026abd708dce | 67 | // initialize Servo |
annesteenbeek | 109:026abd708dce | 68 | Servo servo(servoPin); |
annesteenbeek | 98:25528494287d | 69 | |
annesteenbeek | 98:25528494287d | 70 | |
annesteenbeek | 109:026abd708dce | 71 | // Initialize encoders |
annesteenbeek | 109:026abd708dce | 72 | Encoder encoder1(enc1A, enc1B); |
annesteenbeek | 109:026abd708dce | 73 | Encoder encoder2(enc2A, enc2B); |
annesteenbeek | 25:874675516927 | 74 | |
annesteenbeek | 109:026abd708dce | 75 | // Set direction pins |
annesteenbeek | 109:026abd708dce | 76 | DigitalOut motor1Dir(motor1DirPin); |
annesteenbeek | 109:026abd708dce | 77 | DigitalOut motor2Dir(motor2DirPin); |
annesteenbeek | 26:0a9e4147a31a | 78 | |
annesteenbeek | 109:026abd708dce | 79 | // create PID instances |
annesteenbeek | 109:026abd708dce | 80 | PID motor1PID(&motor1Speed, &motor1PWM, &motor1SetSpeed, Kp1, Ki1, Kd1); |
annesteenbeek | 109:026abd708dce | 81 | PID motor2PID(&motor2Speed, &motor2PWM, &motor2SetSpeed, Kp2, Ki2, Kd2); |
annesteenbeek | 31:8fbee6c92753 | 82 | |
annesteenbeek | 127:831f03471efb | 83 | // set pumpswitch |
annesteenbeek | 127:831f03471efb | 84 | DigitalOut pumpSwitch(pumpPin); |
annesteenbeek | 127:831f03471efb | 85 | |
annesteenbeek | 127:831f03471efb | 86 | |
annesteenbeek | 109:026abd708dce | 87 | Timer t; |
annesteenbeek | 25:874675516927 | 88 | |
annesteenbeek | 25:874675516927 | 89 | void motorInit(){ |
annesteenbeek | 25:874675516927 | 90 | |
annesteenbeek | 3:47c76be6d402 | 91 | motor1Dir.write(direction1); |
annesteenbeek | 3:47c76be6d402 | 92 | motor2Dir.write(direction2); |
annesteenbeek | 3:47c76be6d402 | 93 | |
annesteenbeek | 3:47c76be6d402 | 94 | // Set motor PWM period |
annesteenbeek | 3:47c76be6d402 | 95 | motor1.period(1/pwm_frequency); |
annesteenbeek | 3:47c76be6d402 | 96 | motor2.period(1/pwm_frequency); |
annesteenbeek | 55:ee5257fb73df | 97 | |
annesteenbeek | 55:ee5257fb73df | 98 | motor1PID.SetSampleTime(motorCall); |
annesteenbeek | 55:ee5257fb73df | 99 | motor2PID.SetSampleTime(motorCall); |
annesteenbeek | 55:ee5257fb73df | 100 | |
annesteenbeek | 69:37f75a7d36d8 | 101 | motor1PID.SetOutputLimits(0,1); |
annesteenbeek | 69:37f75a7d36d8 | 102 | motor2PID.SetOutputLimits(0,1); |
annesteenbeek | 69:37f75a7d36d8 | 103 | |
annesteenbeek | 30:a20f16bf8dda | 104 | // Turn PID on |
annesteenbeek | 55:ee5257fb73df | 105 | motor1PID.SetMode(AUTOMATIC); |
annesteenbeek | 55:ee5257fb73df | 106 | motor2PID.SetMode(AUTOMATIC); |
annesteenbeek | 98:25528494287d | 107 | |
annesteenbeek | 98:25528494287d | 108 | // set servo period |
annesteenbeek | 107:de47331612d9 | 109 | servo.Enable(servoStartPos, 20000); |
annesteenbeek | 98:25528494287d | 110 | |
annesteenbeek | 30:a20f16bf8dda | 111 | // start the timer |
annesteenbeek | 29:e4f3455aaa0b | 112 | t.start(); |
annesteenbeek | 4:80e2280058ed | 113 | } |
annesteenbeek | 0:525558a26464 | 114 | |
annesteenbeek | 26:0a9e4147a31a | 115 | |
annesteenbeek | 0:525558a26464 | 116 | void motorControl(){ |
annesteenbeek | 127:831f03471efb | 117 | // EMG signals to motor speeds |
annesteenbeek | 124:f67ce69557db | 118 | if(!usePotmeters && controlAngle){ |
annesteenbeek | 124:f67ce69557db | 119 | double scaleVel = 20; |
annesteenbeek | 124:f67ce69557db | 120 | motor1SetSpeed = x_velocity*scaleVel; |
annesteenbeek | 124:f67ce69557db | 121 | motor2SetSpeed = y_velocity*scaleVel; |
annesteenbeek | 124:f67ce69557db | 122 | servoSpeed = z_velocity*scaleVel; |
annesteenbeek | 124:f67ce69557db | 123 | } |
annesteenbeek | 124:f67ce69557db | 124 | if(!usePotmeters && controlDirection){ |
annesteenbeek | 124:f67ce69557db | 125 | double scaleVel = 0.1; |
annesteenbeek | 124:f67ce69557db | 126 | setXSpeed = x_velocity*scaleVel; |
annesteenbeek | 124:f67ce69557db | 127 | setYSpeed = y_velocity*scaleVel; |
annesteenbeek | 124:f67ce69557db | 128 | servoSpeed = z_velocity*scaleVel; |
annesteenbeek | 124:f67ce69557db | 129 | } |
annesteenbeek | 30:a20f16bf8dda | 130 | // get encoder positions in degrees |
annesteenbeek | 30:a20f16bf8dda | 131 | // 131.25:1 gear ratio |
annesteenbeek | 30:a20f16bf8dda | 132 | // getPosition uses X2 configuration, so 32 counts per revolution |
annesteenbeek | 30:a20f16bf8dda | 133 | // encoder reads CCW negative, and CW positive, so multiply by -1 to make CCW positive |
annesteenbeek | 68:21cb054f1399 | 134 | |
annesteenbeek | 68:21cb054f1399 | 135 | encoder1Counts = encoder1.getPosition(); |
annesteenbeek | 68:21cb054f1399 | 136 | encoder2Counts = encoder2.getPosition(); |
annesteenbeek | 68:21cb054f1399 | 137 | |
annesteenbeek | 111:43c0881fe7e7 | 138 | motor1Pos = ((encoder1Counts/32)/131.25)*360; |
annesteenbeek | 111:43c0881fe7e7 | 139 | motor2Pos = ((encoder2Counts/32)/131.25)*360; |
annesteenbeek | 3:47c76be6d402 | 140 | |
annesteenbeek | 0:525558a26464 | 141 | // check if motor's are within rotational boundarys |
annesteenbeek | 30:a20f16bf8dda | 142 | // get encoder speeds in deg/sec |
annesteenbeek | 66:d1ab5904f8e5 | 143 | now = t.read(); |
annesteenbeek | 50:b0cf07ca53cf | 144 | timechange = (now - prevTime); |
annesteenbeek | 111:43c0881fe7e7 | 145 | motor1Speed = ((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange; |
annesteenbeek | 111:43c0881fe7e7 | 146 | motor2Speed = ((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange; |
annesteenbeek | 50:b0cf07ca53cf | 147 | prevTime = now; |
annesteenbeek | 68:21cb054f1399 | 148 | motor1PrevCounts = encoder1Counts; |
annesteenbeek | 68:21cb054f1399 | 149 | motor2PrevCounts = encoder2Counts; |
annesteenbeek | 29:e4f3455aaa0b | 150 | |
annesteenbeek | 31:8fbee6c92753 | 151 | // calculate motor setpoint speed in deg/sec from setpoint x/y speed |
annesteenbeek | 30:a20f16bf8dda | 152 | |
annesteenbeek | 112:7b964afb97b4 | 153 | // exclude kinematics when still calibrating |
annesteenbeek | 124:f67ce69557db | 154 | if (calReady && !controlAngle){ |
annesteenbeek | 112:7b964afb97b4 | 155 | kinematics(); |
annesteenbeek | 112:7b964afb97b4 | 156 | } |
annesteenbeek | 112:7b964afb97b4 | 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 | 127:831f03471efb | 172 | // write new values to motor's |
annesteenbeek | 124:f67ce69557db | 173 | if (motor1SetSpeed ==0 ){ |
annesteenbeek | 124:f67ce69557db | 174 | motor1PID.SetOutputLimits(0,0); |
annesteenbeek | 124:f67ce69557db | 175 | } |
annesteenbeek | 82:4cc8f9ad3fec | 176 | if (motor1SetSpeed > 0 ){ // CCW rotation |
annesteenbeek | 111:43c0881fe7e7 | 177 | direction1 = true; |
annesteenbeek | 84:df770ec4df61 | 178 | motor1PID.SetOutputLimits(0,1); // change pid output direction |
annesteenbeek | 31:8fbee6c92753 | 179 | }else{ |
annesteenbeek | 111:43c0881fe7e7 | 180 | direction1 = false; // CW rotation |
annesteenbeek | 83:8fa05f53fc73 | 181 | motor1PID.SetOutputLimits(-1,0); |
annesteenbeek | 31:8fbee6c92753 | 182 | } |
annesteenbeek | 124:f67ce69557db | 183 | if (motor2SetSpeed ==0 ){ |
annesteenbeek | 124:f67ce69557db | 184 | motor2PID.SetOutputLimits(0,0); |
annesteenbeek | 124:f67ce69557db | 185 | } |
annesteenbeek | 82:4cc8f9ad3fec | 186 | if (motor2SetSpeed > 0 ){ // CCW rotation |
annesteenbeek | 111:43c0881fe7e7 | 187 | direction2 = true; |
annesteenbeek | 83:8fa05f53fc73 | 188 | motor2PID.SetOutputLimits(0,1); |
annesteenbeek | 68:21cb054f1399 | 189 | }else{ |
annesteenbeek | 111:43c0881fe7e7 | 190 | direction2 = false; // CW rotation |
annesteenbeek | 83:8fa05f53fc73 | 191 | motor2PID.SetOutputLimits(-1,0); |
annesteenbeek | 68:21cb054f1399 | 192 | } |
annesteenbeek | 31:8fbee6c92753 | 193 | motor1Dir.write(direction1); |
annesteenbeek | 31:8fbee6c92753 | 194 | motor2Dir.write(direction2); |
annesteenbeek | 69:37f75a7d36d8 | 195 | |
annesteenbeek | 52:2ac9dee099ce | 196 | motor1.write(abs(motor1PWM)); |
annesteenbeek | 76:0aa90e728e4a | 197 | motor2.write(abs(motor2PWM)); |
annesteenbeek | 31:8fbee6c92753 | 198 | } |
annesteenbeek | 31:8fbee6c92753 | 199 | |
annesteenbeek | 0:525558a26464 | 200 | void servoControl(){ |
annesteenbeek | 107:de47331612d9 | 201 | servo.SetPosition(1300 + 700*servoSpeed); |
annesteenbeek | 25:874675516927 | 202 | } |
annesteenbeek | 25:874675516927 | 203 | |
annesteenbeek | 112:7b964afb97b4 | 204 | const double motor1StartPos = (-10*32*131.25)/360; // angle to encoder counts |
annesteenbeek | 124:f67ce69557db | 205 | const double motor2StartPos = (120*32*131.25)/360; |
annesteenbeek | 124:f67ce69557db | 206 | |
annesteenbeek | 124:f67ce69557db | 207 | // Set calibration values |
annesteenbeek | 124:f67ce69557db | 208 | double motor1CalSpeed = -10; // deg/sec |
annesteenbeek | 124:f67ce69557db | 209 | double motor2CalSpeed = 5; |
annesteenbeek | 124:f67ce69557db | 210 | bool springHit = false; |
annesteenbeek | 124:f67ce69557db | 211 | float lastCall = 0; |
annesteenbeek | 124:f67ce69557db | 212 | bool calibrating1 = true; |
annesteenbeek | 124:f67ce69557db | 213 | bool calibrating2 = false; |
annesteenbeek | 124:f67ce69557db | 214 | double looseTime = 0; |
annesteenbeek | 112:7b964afb97b4 | 215 | |
annesteenbeek | 106:1773bf7b95c5 | 216 | bool calibrateMotors(){ |
annesteenbeek | 98:25528494287d | 217 | safetyOn = false; // safety springs off |
annesteenbeek | 98:25528494287d | 218 | motorsEnable = true; // motors on |
annesteenbeek | 98:25528494287d | 219 | while (calibrating1 || calibrating2){ |
annesteenbeek | 98:25528494287d | 220 | if (calibrating1){ |
annesteenbeek | 111:43c0881fe7e7 | 221 | motor1SetSpeed = motor1CalSpeed; |
annesteenbeek | 98:25528494287d | 222 | redLed.write(1); greenLed.write(0); blueLed.write(1); |
annesteenbeek | 111:43c0881fe7e7 | 223 | if(safetyIn.read() !=1 && !springHit){ // check if arm reached safety position |
annesteenbeek | 112:7b964afb97b4 | 224 | encoder1.setPosition(motor1StartPos); // set motor 1 cal angle |
annesteenbeek | 112:7b964afb97b4 | 225 | motor1PrevCounts = motor1StartPos; // set previous count to prevent speed spike |
annesteenbeek | 111:43c0881fe7e7 | 226 | motor1CalSpeed = -motor1CalSpeed; // move away |
annesteenbeek | 98:25528494287d | 227 | springHit = true; |
annesteenbeek | 111:43c0881fe7e7 | 228 | looseTime = t.read(); // timer to compensate spring movement |
annesteenbeek | 98:25528494287d | 229 | }else{ |
annesteenbeek | 111:43c0881fe7e7 | 230 | // if hit and after is no longer touching spring and 0.5seconds passed |
annesteenbeek | 111:43c0881fe7e7 | 231 | if(springHit && ((t.read() - looseTime) > 2)){ |
annesteenbeek | 98:25528494287d | 232 | motor1SetSpeed = 0; |
annesteenbeek | 98:25528494287d | 233 | springHit = false; |
annesteenbeek | 111:43c0881fe7e7 | 234 | calibrating2 = true; // start calibrating 2 |
annesteenbeek | 98:25528494287d | 235 | calibrating1 = false; |
annesteenbeek | 98:25528494287d | 236 | } |
annesteenbeek | 98:25528494287d | 237 | } |
annesteenbeek | 98:25528494287d | 238 | } |
annesteenbeek | 98:25528494287d | 239 | if (calibrating2){ |
annesteenbeek | 111:43c0881fe7e7 | 240 | motor2SetSpeed = motor2CalSpeed; |
annesteenbeek | 98:25528494287d | 241 | redLed.write(1); greenLed.write(1); blueLed.write(0); |
annesteenbeek | 111:43c0881fe7e7 | 242 | if(safetyIn.read() !=1 && !springHit){ // check if arm reached safety position |
annesteenbeek | 112:7b964afb97b4 | 243 | encoder2.setPosition(motor2StartPos); // set motor 2 cal angle |
annesteenbeek | 112:7b964afb97b4 | 244 | motor2PrevCounts = motor2StartPos; // set previous cunt to prevent speed spike |
annesteenbeek | 111:43c0881fe7e7 | 245 | motor2CalSpeed = -motor2CalSpeed; // move away |
annesteenbeek | 98:25528494287d | 246 | springHit = true; |
annesteenbeek | 111:43c0881fe7e7 | 247 | looseTime = t.read(); |
annesteenbeek | 98:25528494287d | 248 | }else{ |
annesteenbeek | 111:43c0881fe7e7 | 249 | if(springHit && ((t.read() - looseTime) > 2)){ |
annesteenbeek | 98:25528494287d | 250 | motor2SetSpeed = 0; |
annesteenbeek | 98:25528494287d | 251 | springHit = false; |
annesteenbeek | 98:25528494287d | 252 | calibrating2 = false; // stop calibrating 2 |
annesteenbeek | 98:25528494287d | 253 | } |
annesteenbeek | 98:25528494287d | 254 | } |
annesteenbeek | 98:25528494287d | 255 | } |
annesteenbeek | 111:43c0881fe7e7 | 256 | // now = t.read(); // call motor using timer instead of wait |
annesteenbeek | 111:43c0881fe7e7 | 257 | // if(now - lastCall > motorCall){ |
annesteenbeek | 98:25528494287d | 258 | motorControl(); |
annesteenbeek | 111:43c0881fe7e7 | 259 | wait(motorCall); // keep calling PID's with motorCall frequency |
annesteenbeek | 111:43c0881fe7e7 | 260 | // lastCall = now; |
annesteenbeek | 111:43c0881fe7e7 | 261 | // } |
annesteenbeek | 98:25528494287d | 262 | |
annesteenbeek | 98:25528494287d | 263 | } |
annesteenbeek | 111:43c0881fe7e7 | 264 | redLed.write(0); greenLed.write(1); blueLed.write(1); |
annesteenbeek | 99:7030e9790b1d | 265 | motorsEnable = false; // turn motor's off again |
annesteenbeek | 76:0aa90e728e4a | 266 | safetyOn = true; // turn safety on after callibration |
annesteenbeek | 110:a6439e13be8b | 267 | return true; // return true when finished |
annesteenbeek | 74:75be98779124 | 268 | } |
annesteenbeek | 74:75be98779124 | 269 | |
annesteenbeek | 74:75be98779124 | 270 | |
annesteenbeek | 74:75be98779124 | 271 | void safety(){ |
annesteenbeek | 74:75be98779124 | 272 | if (safetyOn){ |
annesteenbeek | 81:71e7e98deb2c | 273 | if (safetyIn.read() != 1){ |
annesteenbeek | 79:cf500b63f349 | 274 | motorsEnable = false; |
annesteenbeek | 79:cf500b63f349 | 275 | } |
annesteenbeek | 74:75be98779124 | 276 | } |
annesteenbeek | 79:cf500b63f349 | 277 | } |
annesteenbeek | 106:1773bf7b95c5 | 278 | |
annesteenbeek | 112:7b964afb97b4 | 279 | const double L1 = 0.436; // first arm in m |
annesteenbeek | 112:7b964afb97b4 | 280 | const double L2 = 0.120; // first arm in m |
annesteenbeek | 112:7b964afb97b4 | 281 | const double L3 = 0.255; // servo arm in m |
annesteenbeek | 112:7b964afb97b4 | 282 | const double Xmax = 0.3; |
annesteenbeek | 120:440f1516101b | 283 | const double Xmin = -0.3; |
annesteenbeek | 112:7b964afb97b4 | 284 | const double Ymax = 0.645; |
annesteenbeek | 112:7b964afb97b4 | 285 | const double Ymin = 0.33; |
annesteenbeek | 121:6d8f1bdcda05 | 286 | double Xpos = 0; // set values |
annesteenbeek | 124:f67ce69557db | 287 | double Ypos = 0; |
annesteenbeek | 106:1773bf7b95c5 | 288 | |
annesteenbeek | 112:7b964afb97b4 | 289 | |
annesteenbeek | 106:1773bf7b95c5 | 290 | |
annesteenbeek | 112:7b964afb97b4 | 291 | bool kinematics(){ |
annesteenbeek | 127:831f03471efb | 292 | // calculate current x and Y |
annesteenbeek | 127:831f03471efb | 293 | Xpos = (L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180); |
annesteenbeek | 127:831f03471efb | 294 | Ypos = (L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180); |
annesteenbeek | 127:831f03471efb | 295 | // check if x and y are within limits |
annesteenbeek | 127:831f03471efb | 296 | // else Store the constraint line |
annesteenbeek | 127:831f03471efb | 297 | // check if movement is in direction of constraint |
annesteenbeek | 127:831f03471efb | 298 | // else return false no movement (anglespeed = 0) |
annesteenbeek | 127:831f03471efb | 299 | // calculate required angle speeds |
annesteenbeek | 127:831f03471efb | 300 | if( (Xpos>Xmax && setXSpeed > 0 )|| \ |
annesteenbeek | 127:831f03471efb | 301 | (Xpos<Xmin && setXSpeed < 0 )|| \ |
annesteenbeek | 127:831f03471efb | 302 | (Ypos>Ymax && setYSpeed > 0 )|| \ |
annesteenbeek | 127:831f03471efb | 303 | (Ypos<Ymin && setYSpeed < 0 ) \ |
annesteenbeek | 127:831f03471efb | 304 | ){ |
annesteenbeek | 127:831f03471efb | 305 | motor1SetSpeed = 0; |
annesteenbeek | 127:831f03471efb | 306 | motor2SetSpeed = 0; |
annesteenbeek | 127:831f03471efb | 307 | return false; |
annesteenbeek | 127:831f03471efb | 308 | } |
annesteenbeek | 127:831f03471efb | 309 | motor1SetSpeed = (180/PI)*((setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \ |
annesteenbeek | 127:831f03471efb | 310 | setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180))); |
annesteenbeek | 127:831f03471efb | 311 | motor2SetSpeed = (180/PI)*(-(setXSpeed*(L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + \ |
annesteenbeek | 127:831f03471efb | 312 | setYSpeed*(L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + \ |
annesteenbeek | 127:831f03471efb | 313 | setXSpeed*L1*cos(motor1Pos*PI/180) + \ |
annesteenbeek | 127:831f03471efb | 314 | setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*(L2+L3)*sin(motor2Pos*PI/180))); |
annesteenbeek | 120:440f1516101b | 315 | return true; |
annesteenbeek | 127:831f03471efb | 316 | } |
annesteenbeek | 112:7b964afb97b4 | 317 | |
annesteenbeek | 127:831f03471efb | 318 | void runPump(){ |
annesteenbeek | 127:831f03471efb | 319 | pumpSwitch.write(enablePump); |
annesteenbeek | 112:7b964afb97b4 | 320 | } |