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@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?

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 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 }