Biorobotics 2019 - Group 10 - The Poolinator / Mbed 2 deprecated Poolinator

Code for the Poolinator

Dependencies:   mbed QEI HIDScope Pulse biquadFilter MODSERIAL FastPWM

main.cpp@14:6a82804c88d6, 2019-11-01 (annotated)

Committer:
sjoerd1999
Date:
Fri Nov 01 14:25:34 2019 +0000
Revision:
14:6a82804c88d6
Parent:
13:74f2e8d3e04e
Child:
15:47d949e2de1a
WORKINGYAAAAAASSSSS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sjoerd1999 4:8113394bed1b 1 /*
sjoerd1999 4:8113394bed1b 2 The Poolinator - A pool playing robot for people with DMD
sjoerd1999 4:8113394bed1b 3
sjoerd1999 4:8113394bed1b 4 GROUP 10
sjoerd1999 4:8113394bed1b 5 Sjoerd de Jong - s1949950
sjoerd1999 4:8113394bed1b 6 Joost Loohuis - s1969633
sjoerd1999 4:8113394bed1b 7 Viktor Edlund - s2430878
sjoerd1999 4:8113394bed1b 8 Giuseppina Pinky Diatmiko - s1898841
sjoerd1999 4:8113394bed1b 9 Daan v.d Veen - s2003171
sjoerd1999 4:8113394bed1b 10 */
sjoerd1999 4:8113394bed1b 11
RobertoO 0:67c50348f842 12 #include "mbed.h"
sjoerd1999 3:d319bc2b19f1 13 #include "HIDScope.h"
sjoerd1999 2:f8e0a7b5c90a 14 #include "QEI.h"
RobertoO 1:b862262a9d14 15 #include "MODSERIAL.h"
sjoerd1999 2:f8e0a7b5c90a 16 #include "BiQuad.h"
sjoerd1999 2:f8e0a7b5c90a 17 #include "FastPWM.h"
sjoerd1999 9:913a59894698 18 #include <Pulse.h>
sjoerd1999 3:d319bc2b19f1 19
sjoerd1999 8:1733338758d3 20 #define PI 3.14159265358979323846
sjoerd1999 8:1733338758d3 21
sjoerd1999 9:913a59894698 22 // COMMUNICATION
RobertoO 1:b862262a9d14 23 MODSERIAL pc(USBTX, USBRX);
sjoerd1999 13:74f2e8d3e04e 24 PulseInOut pulsePin(D9);
RobertoO 0:67c50348f842 25
sjoerd1999 13:74f2e8d3e04e 26 bool demo = false;
sjoerd1999 14:6a82804c88d6 27 enum State {CALIBRATE, INIT_0, IDLE, INIT_1, PHASE_1, INIT_2, PHASE_2, INIT_3, PHASE_3, PHASE_1EMG};
sjoerd1999 13:74f2e8d3e04e 28 State currentState = CALIBRATE;
sjoerd1999 8:1733338758d3 29
sjoerd1999 8:1733338758d3 30 struct vec {
sjoerd1999 8:1733338758d3 31 double x,y,z;
sjoerd1999 8:1733338758d3 32 };
sjoerd1999 8:1733338758d3 33
sjoerd1999 13:74f2e8d3e04e 34 vec endPos{0,55,20};
sjoerd1999 9:913a59894698 35 vec ballPos{10,50,10};
sjoerd1999 9:913a59894698 36
sjoerd1999 14:6a82804c88d6 37 // EMG CONTROLS
sjoerd1999 14:6a82804c88d6 38 DigitalIn EMG_D(D15);
sjoerd1999 14:6a82804c88d6 39 AnalogIn EMG_A(A5);
sjoerd1999 13:74f2e8d3e04e 40 void moveWithEMG()
sjoerd1999 13:74f2e8d3e04e 41 {
sjoerd1999 14:6a82804c88d6 42 int whichEMG = EMG_D.read();
sjoerd1999 14:6a82804c88d6 43 float EMGvalue = EMG_A.read();
sjoerd1999 14:6a82804c88d6 44 int EMGstate = (EMGvalue < 0.29) ? 0 : (EMGvalue < 0.7) ? 1 : 2;
sjoerd1999 14:6a82804c88d6 45 float delta = 0.08;
sjoerd1999 14:6a82804c88d6 46 pc.printf("whichEMG: %d\r\n",whichEMG);
sjoerd1999 14:6a82804c88d6 47 pc.printf("EMGstate: %d\r\n",EMGstate);
sjoerd1999 14:6a82804c88d6 48 if(whichEMG == 0) {
sjoerd1999 14:6a82804c88d6 49 if(EMGstate == 1) endPos.x -= delta;
sjoerd1999 14:6a82804c88d6 50 if(EMGstate == 2) endPos.x += delta;
sjoerd1999 14:6a82804c88d6 51 } else {
sjoerd1999 14:6a82804c88d6 52 if(EMGstate == 1) endPos.z -= delta;
sjoerd1999 14:6a82804c88d6 53 if(EMGstate == 2) endPos.z += delta;
sjoerd1999 13:74f2e8d3e04e 54 }
sjoerd1999 13:74f2e8d3e04e 55 }
sjoerd1999 9:913a59894698 56
sjoerd1999 9:913a59894698 57 // JOYSTICK CONTROL //
sjoerd1999 9:913a59894698 58 AnalogIn joyX(A2), joyY(A1), slider(A0);
sjoerd1999 9:913a59894698 59 DigitalIn joyButton(PTB20);
sjoerd1999 9:913a59894698 60 void moveWithJoystick()
sjoerd1999 9:913a59894698 61 {
sjoerd1999 9:913a59894698 62 float delta = 0.04;
sjoerd1999 9:913a59894698 63 if(joyX.read() < 0.2) endPos.x += delta;
sjoerd1999 9:913a59894698 64 else if(joyX.read() > 0.8) endPos.x -= delta;
sjoerd1999 9:913a59894698 65 if(joyY.read() < 0.2) endPos.z += delta;
sjoerd1999 9:913a59894698 66 else if(joyY.read() > 0.8) endPos.z -= delta;
sjoerd1999 9:913a59894698 67 if(slider.read() < 0.2) endPos.y += delta;
sjoerd1999 9:913a59894698 68 else if(slider.read() > 0.9) endPos.y -= delta;
sjoerd1999 9:913a59894698 69 }
sjoerd1999 3:d319bc2b19f1 70
sjoerd1999 8:1733338758d3 71 // DC MOTORS //
sjoerd1999 9:913a59894698 72 QEI encoder1(D10,D11,NC,32), encoder2(PTC5,PTC7, NC, 32), encoder3(D12,D13,NC,32);
sjoerd1999 8:1733338758d3 73 PwmOut motor1_pwm(D5), motor3_pwm(D6);
sjoerd1999 8:1733338758d3 74 DigitalOut motor1_dir(D4), motor3_dir(D7), motor2_A(D2), motor2_B(D3);
sjoerd1999 7:bfcb74384f46 75
sjoerd1999 8:1733338758d3 76 float motor1_cur = 0, motor2_cur = 0, motor3_cur = 0;
sjoerd1999 8:1733338758d3 77 float motor1_tar = 0, motor2_tar = 0, motor3_tar = 0;
sjoerd1999 2:f8e0a7b5c90a 78
sjoerd1999 8:1733338758d3 79 void setMotor(int motor, float motor_spd) // Set the motor speed (between -1 and 1)
sjoerd1999 2:f8e0a7b5c90a 80 {
sjoerd1999 2:f8e0a7b5c90a 81 int motor_dir = (motor_spd >= 0) ? 0 : 1;
sjoerd1999 2:f8e0a7b5c90a 82 motor_spd = fabs(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 83
sjoerd1999 2:f8e0a7b5c90a 84 if(motor == 1) {
sjoerd1999 9:913a59894698 85 motor1_dir.write(1 - motor_dir);
sjoerd1999 2:f8e0a7b5c90a 86 motor1_pwm.write(motor_spd);
sjoerd1999 8:1733338758d3 87 } else if(motor == 3) {
sjoerd1999 8:1733338758d3 88 motor3_dir.write(motor_dir);
sjoerd1999 8:1733338758d3 89 motor3_pwm.write(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 90 } else if(motor == 2) {
sjoerd1999 8:1733338758d3 91 motor2_A.write((motor_dir == 0) ? 0 : int(motor_spd)); // Motor 2 has digital pins so no PWM
sjoerd1999 8:1733338758d3 92 motor2_B.write((motor_dir == 0) ? int(motor_spd) : 0);
sjoerd1999 2:f8e0a7b5c90a 93 }
sjoerd1999 2:f8e0a7b5c90a 94 }
sjoerd1999 2:f8e0a7b5c90a 95
sjoerd1999 8:1733338758d3 96 void getMotorPositions() // Calculate current motor positions (M1:angle, M2/M3:length) based on encoder pulses
sjoerd1999 8:1733338758d3 97 {
sjoerd1999 9:913a59894698 98 motor1_cur = 244 - float(encoder1.getPulses()) / 18530.00 * 360;
sjoerd1999 9:913a59894698 99 if(motor1_cur > 360) motor1_cur -= 360;
sjoerd1999 9:913a59894698 100 if(motor1_cur < 0) motor1_cur += 360;
sjoerd1999 9:913a59894698 101 motor2_cur = float(encoder2.getPulses()) / 41920.00 + 14.3;
sjoerd1999 9:913a59894698 102 motor3_cur = float(encoder3.getPulses()) / 41920.00 + 57.8;
sjoerd1999 8:1733338758d3 103 }
sjoerd1999 8:1733338758d3 104
sjoerd1999 8:1733338758d3 105 void moveToTargets() // Move to the target positions. No PID.
sjoerd1999 8:1733338758d3 106 {
sjoerd1999 14:6a82804c88d6 107 setMotor(1, (fabs(motor1_cur - motor1_tar) < 0.5) ? 0 : (motor1_cur < motor1_tar) ? 0.08 : -0.08);
sjoerd1999 14:6a82804c88d6 108 setMotor(2, (fabs(motor2_cur - motor2_tar) < 0.04) ? 0 : (motor2_cur < motor2_tar) ? 1 : -1);
sjoerd1999 14:6a82804c88d6 109 setMotor(3, (fabs(motor3_cur - motor3_tar) < 0.04) ? 0 : (motor3_cur < motor3_tar) ? 1: -1);
sjoerd1999 8:1733338758d3 110 }
sjoerd1999 8:1733338758d3 111
sjoerd1999 8:1733338758d3 112 // KINEMATICS //
sjoerd1999 8:1733338758d3 113 void calculateKinematics(float x, float y, float z) // y is up
sjoerd1999 8:1733338758d3 114 {
sjoerd1999 9:913a59894698 115 float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (2*PI));
sjoerd1999 8:1733338758d3 116 float angle2 = acos(sqrt(x*x + y*y + z*z) / 100.00) + atan2(sqrt(x*x + z*z), y);
sjoerd1999 8:1733338758d3 117 float angle3 = 2 * asin(sqrt(x*x + y*y + z*z) / 100.00);
sjoerd1999 8:1733338758d3 118
sjoerd1999 9:913a59894698 119 motor1_tar = angle1;
sjoerd1999 9:913a59894698 120 motor2_tar = sqrt(200 - 200 * cos(angle2)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = c = 10cm)
sjoerd1999 9:913a59894698 121 motor3_tar = sqrt(2600 - 1000 * cos(PI - angle3)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = 10cm, c = 50cm)
sjoerd1999 9:913a59894698 122 }
sjoerd1999 9:913a59894698 123
sjoerd1999 9:913a59894698 124 void calculateKinematicsHand(float x, float y, float z) // y is up
sjoerd1999 9:913a59894698 125 {
sjoerd1999 9:913a59894698 126 float angle1 = fmod(2*PI - atan2(z, x), 2*PI) * (360 / (2*PI));
sjoerd1999 9:913a59894698 127 float xz = sqrt(x*x + z*z) - 15;
sjoerd1999 9:913a59894698 128 y -= 9;
sjoerd1999 9:913a59894698 129 float angle2 = acos(sqrt(xz*xz + y*y) / 100.00) + atan2(xz, y);
sjoerd1999 9:913a59894698 130 float angle3 = 2 * asin(sqrt(xz*xz +y*y) / 100.00);
sjoerd1999 9:913a59894698 131
sjoerd1999 9:913a59894698 132 motor1_tar = angle1;
sjoerd1999 8:1733338758d3 133 motor2_tar = sqrt(200 - 200 * cos(angle2)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = c = 10cm)
sjoerd1999 8:1733338758d3 134 motor3_tar = sqrt(2600 - 1000 * cos(PI - angle3)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = 10cm, c = 50cm)
sjoerd1999 8:1733338758d3 135 }
sjoerd1999 8:1733338758d3 136
sjoerd1999 8:1733338758d3 137 // STEPPER MOTOR //
sjoerd1999 2:f8e0a7b5c90a 138 DigitalOut STEPPER_IN1(PTB18), STEPPER_IN2(PTB19), STEPPER_IN3(PTC1), STEPPER_IN4(PTC8);
sjoerd1999 2:f8e0a7b5c90a 139
sjoerd1999 2:f8e0a7b5c90a 140 int stepper_steps = 0;
sjoerd1999 8:1733338758d3 141 float stepper_angle = 0, stepper_target = 0;
sjoerd1999 2:f8e0a7b5c90a 142
sjoerd1999 8:1733338758d3 143 void stepper_step(int direction_) // Requires ~1.5ms wait time between each step, 4096 steps is one rotation
sjoerd1999 2:f8e0a7b5c90a 144 {
sjoerd1999 3:d319bc2b19f1 145 STEPPER_IN1 = (stepper_steps == 5 || stepper_steps == 6 || stepper_steps == 7);
sjoerd1999 2:f8e0a7b5c90a 146 STEPPER_IN2 = (stepper_steps == 3 || stepper_steps == 4 || stepper_steps == 5);
sjoerd1999 2:f8e0a7b5c90a 147 STEPPER_IN3 = (stepper_steps == 1 || stepper_steps == 2 || stepper_steps == 3);
sjoerd1999 2:f8e0a7b5c90a 148 STEPPER_IN4 = (stepper_steps == 7 || stepper_steps == 0 || stepper_steps == 1);
sjoerd1999 2:f8e0a7b5c90a 149 stepper_steps += (direction_ == 0) ? - 1 : 1;
sjoerd1999 2:f8e0a7b5c90a 150 stepper_steps = (stepper_steps + 8) % 8;
sjoerd1999 9:913a59894698 151 stepper_angle -= ((direction_ == 0) ? -1 : 1) * (360.00 / 4096.00);
sjoerd1999 2:f8e0a7b5c90a 152 }
sjoerd1999 2:f8e0a7b5c90a 153
sjoerd1999 8:1733338758d3 154 Ticker stepper_moveToAngle;
sjoerd1999 8:1733338758d3 155 void stepper_move() // Move toward desired angle with threshold. In Ticker function because requires wait otherwise
sjoerd1999 8:1733338758d3 156 {
sjoerd1999 9:913a59894698 157 if(fabs(stepper_angle - stepper_target) > 1) stepper_step((stepper_angle < stepper_target) ? 0 : 1);
sjoerd1999 8:1733338758d3 158 }
sjoerd1999 8:1733338758d3 159
sjoerd1999 8:1733338758d3 160 // SERVO //
sjoerd1999 9:913a59894698 161 AnalogOut servo(DAC0_OUT); // Write analog value to the Arduino
sjoerd1999 13:74f2e8d3e04e 162 void setServo(float i) // Set servo to specified angle(0-180 degrees) signal measured by Arduino and decoded there.
sjoerd1999 5:33133ebe37fd 163 {
sjoerd1999 9:913a59894698 164 servo = i / 180.00;
sjoerd1999 5:33133ebe37fd 165 }
sjoerd1999 3:d319bc2b19f1 166
sjoerd1999 8:1733338758d3 167 // AIMING //
sjoerd1999 13:74f2e8d3e04e 168 float aimAngle, aimTilt = 45;
sjoerd1999 12:c3fd0712f43d 169 float cueLength = 15;
sjoerd1999 9:913a59894698 170 void aim(float angle, float tilt) // Moves both stepper and servo so the end affector points towards desired angle
sjoerd1999 8:1733338758d3 171 {
sjoerd1999 9:913a59894698 172 setServo(tilt + 90);
sjoerd1999 9:913a59894698 173
sjoerd1999 9:913a59894698 174 // SPHERICAL TO CARTESIAN:
sjoerd1999 9:913a59894698 175 float handX = ballPos.x + cueLength * sin(tilt / 180 * PI) * cos(angle / 180 * PI);
sjoerd1999 9:913a59894698 176 float handY = ballPos.y - cueLength * cos(tilt / 180 * PI);
sjoerd1999 9:913a59894698 177 float handZ = ballPos.z - cueLength * sin(tilt / 180 * PI) * sin(angle / 180 * PI);
sjoerd1999 9:913a59894698 178 endPos.x = handX;
sjoerd1999 9:913a59894698 179 endPos.y = handY;
sjoerd1999 9:913a59894698 180 endPos.z = handZ;
sjoerd1999 9:913a59894698 181
sjoerd1999 9:913a59894698 182 float stepperAngle = angle - fmod(2*PI - atan2(handZ, handX), 2*PI) * (180 / PI);
sjoerd1999 9:913a59894698 183 if(stepperAngle < 0) stepperAngle += 360;
sjoerd1999 9:913a59894698 184 stepper_target = stepperAngle;
sjoerd1999 8:1733338758d3 185 }
sjoerd1999 8:1733338758d3 186
sjoerd1999 8:1733338758d3 187 // SOLENOID //
sjoerd1999 5:33133ebe37fd 188 DigitalOut solenoidA(PTC0), solenoidB(PTC9);
sjoerd1999 8:1733338758d3 189 void setSolenoid(int dir) // 1 is out, 0 is in
sjoerd1999 3:d319bc2b19f1 190 {
sjoerd1999 9:913a59894698 191 solenoidA = (dir == 0) ? 0 : 1;
sjoerd1999 9:913a59894698 192 solenoidB = (dir == 0) ? 1 : 0;
sjoerd1999 7:bfcb74384f46 193 }
sjoerd1999 7:bfcb74384f46 194
sjoerd1999 8:1733338758d3 195 // LASER //
sjoerd1999 7:bfcb74384f46 196 DigitalOut laserPin(D8);
sjoerd1999 7:bfcb74384f46 197 void setLaser(bool on)
sjoerd1999 7:bfcb74384f46 198 {
sjoerd1999 7:bfcb74384f46 199 if(on) laserPin.write(1);
sjoerd1999 7:bfcb74384f46 200 else laserPin.write(0);
sjoerd1999 7:bfcb74384f46 201 }
sjoerd1999 7:bfcb74384f46 202
sjoerd1999 9:913a59894698 203 // CALIBRATION // a3 a4 A5
sjoerd1999 14:6a82804c88d6 204 AnalogIn switch1(A3), switch2(A4);
sjoerd1999 14:6a82804c88d6 205 DigitalIn switch3(D14);
sjoerd1999 8:1733338758d3 206 void calibrate() // Calibrates all 3 motors simultaniously
sjoerd1999 7:bfcb74384f46 207 {
sjoerd1999 14:6a82804c88d6 208 setMotor(1,0.1); // Set all motors to move towards the switches
sjoerd1999 7:bfcb74384f46 209 setMotor(2,1);
sjoerd1999 7:bfcb74384f46 210 setMotor(3,1);
sjoerd1999 7:bfcb74384f46 211
sjoerd1999 9:913a59894698 212 while(!(switch1.read() < 0.5 && switch2.read() > 0.5 && switch3.read() > 0.5)) { // When all switches have been pushed in, stop
sjoerd1999 9:913a59894698 213 if(switch1.read() < 0.5) setMotor(1,0);
sjoerd1999 7:bfcb74384f46 214 if(switch2.read() > 0.5) setMotor(2,0);
sjoerd1999 7:bfcb74384f46 215 if(switch3.read() > 0.5) setMotor(3,0);
sjoerd1999 9:913a59894698 216 wait_ms(30);
sjoerd1999 7:bfcb74384f46 217 }
sjoerd1999 8:1733338758d3 218 for(int i = 1; i <= 3; i++) setMotor(i,0); // Make sure they've all stopped
sjoerd1999 8:1733338758d3 219
sjoerd1999 8:1733338758d3 220 encoder1.reset(); // Reset encoder positions
sjoerd1999 7:bfcb74384f46 221 encoder2.reset();
sjoerd1999 7:bfcb74384f46 222 encoder3.reset();
sjoerd1999 3:d319bc2b19f1 223 }
sjoerd1999 3:d319bc2b19f1 224
sjoerd1999 3:d319bc2b19f1 225
RobertoO 0:67c50348f842 226 int main()
RobertoO 0:67c50348f842 227 {
RobertoO 0:67c50348f842 228 pc.baud(115200);
RobertoO 1:b862262a9d14 229 pc.printf("\r\nStarting...\r\n\r\n");
sjoerd1999 12:c3fd0712f43d 230 motor1_pwm.period(0.020);
sjoerd1999 12:c3fd0712f43d 231 motor3_pwm.period(0.020);
sjoerd1999 9:913a59894698 232 stepper_moveToAngle.attach(&stepper_move, 0.0015);
sjoerd1999 14:6a82804c88d6 233
sjoerd1999 14:6a82804c88d6 234 // while(true) {
sjoerd1999 14:6a82804c88d6 235 // pc.printf("EMG_D: %d\r\n",EMG_D.read());
sjoerd1999 14:6a82804c88d6 236 // pc.printf("EMG_A: %f\r\n",EMG_A.read());
sjoerd1999 14:6a82804c88d6 237 // wait_ms(100);
sjoerd1999 14:6a82804c88d6 238 // }
sjoerd1999 14:6a82804c88d6 239
sjoerd1999 14:6a82804c88d6 240 // while(true){
sjoerd1999 14:6a82804c88d6 241 // for(int i = 0; i<180; i++){
sjoerd1999 14:6a82804c88d6 242 // setServo(i);
sjoerd1999 14:6a82804c88d6 243 // wait_ms(50);
sjoerd1999 14:6a82804c88d6 244 // }
sjoerd1999 14:6a82804c88d6 245 // }
sjoerd1999 13:74f2e8d3e04e 246 // while(true){
sjoerd1999 13:74f2e8d3e04e 247 // moveWithEMG();
sjoerd1999 13:74f2e8d3e04e 248 // wait_ms(20);
sjoerd1999 13:74f2e8d3e04e 249 // }
sjoerd1999 7:bfcb74384f46 250
RobertoO 0:67c50348f842 251 while (true) {
sjoerd1999 9:913a59894698 252 switch(currentState) {
sjoerd1999 13:74f2e8d3e04e 253 case CALIBRATE: // Calibrate the hand
sjoerd1999 13:74f2e8d3e04e 254 setSolenoid(0);
sjoerd1999 9:913a59894698 255 calibrate();
sjoerd1999 9:913a59894698 256 setServo(90);
sjoerd1999 14:6a82804c88d6 257 currentState = INIT_0;
sjoerd1999 13:74f2e8d3e04e 258 break;
sjoerd1999 13:74f2e8d3e04e 259 case INIT_0: // Go to idle position
sjoerd1999 13:74f2e8d3e04e 260 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
sjoerd1999 13:74f2e8d3e04e 261 getMotorPositions(); // Calculate current positions
sjoerd1999 13:74f2e8d3e04e 262 moveToTargets(); // Set the motors speeds accordingly
sjoerd1999 13:74f2e8d3e04e 263 wait_ms(40);
sjoerd1999 13:74f2e8d3e04e 264 if(fabs(motor1_cur - motor1_tar) < 0.6 && fabs(motor2_cur - motor2_tar) < 0.05 && fabs(motor3_cur - motor3_tar) < 0.05) currentState = IDLE;
sjoerd1999 13:74f2e8d3e04e 265 break;
sjoerd1999 13:74f2e8d3e04e 266 case IDLE:
sjoerd1999 14:6a82804c88d6 267 setMotor(1,0);
sjoerd1999 14:6a82804c88d6 268 setMotor(2,0);
sjoerd1999 14:6a82804c88d6 269 setMotor(3,0);
sjoerd1999 14:6a82804c88d6 270 wait_ms(40);
sjoerd1999 13:74f2e8d3e04e 271 if(joyButton.read() == 0) {
sjoerd1999 14:6a82804c88d6 272 if(demo) currentState = PHASE_1;
sjoerd1999 14:6a82804c88d6 273 else currentState = PHASE_1EMG;
sjoerd1999 13:74f2e8d3e04e 274 setLaser(1);
sjoerd1999 13:74f2e8d3e04e 275 wait_ms(1000);
sjoerd1999 13:74f2e8d3e04e 276 }
sjoerd1999 9:913a59894698 277 break;
sjoerd1999 14:6a82804c88d6 278 case PHASE_1EMG:
sjoerd1999 14:6a82804c88d6 279 moveWithEMG();
sjoerd1999 14:6a82804c88d6 280 setServo(80);
sjoerd1999 14:6a82804c88d6 281 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
sjoerd1999 14:6a82804c88d6 282 getMotorPositions(); // Calculate current positions
sjoerd1999 14:6a82804c88d6 283 moveToTargets(); // Set the motors speeds accordingly
sjoerd1999 14:6a82804c88d6 284 if(joyButton.read() == 0) {
sjoerd1999 14:6a82804c88d6 285 currentState = PHASE_1;
sjoerd1999 14:6a82804c88d6 286 wait_ms(1000);
sjoerd1999 14:6a82804c88d6 287 }
sjoerd1999 14:6a82804c88d6 288 wait_ms(40);
sjoerd1999 14:6a82804c88d6 289 break;
sjoerd1999 9:913a59894698 290 case PHASE_1:
sjoerd1999 14:6a82804c88d6 291 moveWithJoystick();
sjoerd1999 14:6a82804c88d6 292 setServo(80);
sjoerd1999 9:913a59894698 293 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
sjoerd1999 9:913a59894698 294 getMotorPositions(); // Calculate current positions
sjoerd1999 9:913a59894698 295 moveToTargets(); // Set the motors speeds accordingly
sjoerd1999 9:913a59894698 296 if(joyButton.read() == 0) {
sjoerd1999 9:913a59894698 297 currentState = PHASE_2;
sjoerd1999 9:913a59894698 298 wait_ms(1000);
sjoerd1999 9:913a59894698 299 ballPos.x = endPos.x;
sjoerd1999 12:c3fd0712f43d 300 ballPos.y = 75;
sjoerd1999 9:913a59894698 301 ballPos.z = endPos.z;
sjoerd1999 12:c3fd0712f43d 302 setLaser(0);
sjoerd1999 9:913a59894698 303 }
sjoerd1999 9:913a59894698 304 wait_ms(40);
sjoerd1999 9:913a59894698 305 break;
sjoerd1999 9:913a59894698 306
sjoerd1999 9:913a59894698 307 case PHASE_2:
sjoerd1999 13:74f2e8d3e04e 308 if(slider.read() > 0.9) aimTilt += 0.25;
sjoerd1999 13:74f2e8d3e04e 309 else if(slider.read() < 0.1) aimTilt -= 0.25;
sjoerd1999 13:74f2e8d3e04e 310 aimTilt = (aimTilt < 0) ? 0 : (aimTilt > 90) ? 90 : aimTilt;
sjoerd1999 9:913a59894698 311
sjoerd1999 14:6a82804c88d6 312 if(joyX.read() < 0.2) aimAngle -= 0.3;
sjoerd1999 14:6a82804c88d6 313 else if(joyX.read() > 0.8) aimAngle += 0.3;
sjoerd1999 13:74f2e8d3e04e 314 aimAngle = (aimAngle < 0) ? 360 : (aimAngle > 360) ? 0 : aimAngle;
sjoerd1999 9:913a59894698 315
sjoerd1999 9:913a59894698 316 aim(aimAngle, aimTilt);
sjoerd1999 9:913a59894698 317
sjoerd1999 9:913a59894698 318 calculateKinematicsHand(endPos.x, endPos.y, endPos.z); // Calculate target positions
sjoerd1999 9:913a59894698 319 getMotorPositions(); // Calculate current positions
sjoerd1999 9:913a59894698 320 moveToTargets(); // Set the motors speeds accordingly
sjoerd1999 9:913a59894698 321 wait_ms(40);
sjoerd1999 13:74f2e8d3e04e 322
sjoerd1999 13:74f2e8d3e04e 323 if(joyButton.read() == 0) {
sjoerd1999 13:74f2e8d3e04e 324 currentState = PHASE_3;
sjoerd1999 12:c3fd0712f43d 325 }
sjoerd1999 9:913a59894698 326 break;
sjoerd1999 13:74f2e8d3e04e 327 case PHASE_3: // Shoot
sjoerd1999 13:74f2e8d3e04e 328 setSolenoid(1);
sjoerd1999 13:74f2e8d3e04e 329 wait_ms(500);
sjoerd1999 13:74f2e8d3e04e 330 setSolenoid(0);
sjoerd1999 13:74f2e8d3e04e 331 currentState = INIT_0;
sjoerd1999 13:74f2e8d3e04e 332 endPos.x = 0;
sjoerd1999 13:74f2e8d3e04e 333 endPos.y = 55;
sjoerd1999 13:74f2e8d3e04e 334 endPos.z = 20;
sjoerd1999 13:74f2e8d3e04e 335 aimTilt = 45;
sjoerd1999 13:74f2e8d3e04e 336 aimAngle = 0;
sjoerd1999 13:74f2e8d3e04e 337 stepper_target = 0;
sjoerd1999 13:74f2e8d3e04e 338 break;
sjoerd1999 9:913a59894698 339 }
sjoerd1999 4:8113394bed1b 340 /*
sjoerd1999 4:8113394bed1b 341 SOME EXAPLE CODE
sjoerd1999 5:33133ebe37fd 342
sjoerd1999 4:8113394bed1b 343 * MOTOR
sjoerd1999 8:1733338758d3 344 setMotor(..., ...) // which motor (1,2,3), and speed (-1.0, +1.0)
sjoerd1999 5:33133ebe37fd 345
sjoerd1999 8:1733338758d3 346 * KINEMATICS (this should be done every ~30 ms)
sjoerd1999 8:1733338758d3 347 calculateKinematics(x, y, z); // Calculate target positions
sjoerd1999 8:1733338758d3 348 getMotorPositions(); // Calculate current positions
sjoerd1999 8:1733338758d3 349 moveToTargets(); // Set the motors speeds accordingly
sjoerd1999 3:d319bc2b19f1 350
sjoerd1999 4:8113394bed1b 351 * STEPPER
sjoerd1999 8:1733338758d3 352 stepper_target = ...; // angle (between 0.0 and 180.0)
sjoerd1999 4:8113394bed1b 353
sjoerd1999 4:8113394bed1b 354 * SERVO
sjoerd1999 7:bfcb74384f46 355 setServo(...) // value between 0.0 and 180.0 (= 0 and 180 degrees)
sjoerd1999 5:33133ebe37fd 356
sjoerd1999 8:1733338758d3 357 * AIMING
sjoerd1999 8:1733338758d3 358 aim(...) // value between 0 and 360
sjoerd1999 8:1733338758d3 359
sjoerd1999 5:33133ebe37fd 360 * SOLENOID
sjoerd1999 8:1733338758d3 361 setSolenoid(...); // position, 0(in) or 1(out)
sjoerd1999 8:1733338758d3 362
sjoerd1999 8:1733338758d3 363 * LASER
sjoerd1999 8:1733338758d3 364 setLaser(...) // 0(off) or 1(on)
sjoerd1999 5:33133ebe37fd 365
sjoerd1999 4:8113394bed1b 366 */
sjoerd1999 7:bfcb74384f46 367
sjoerd1999 9:913a59894698 368 // pc.printf("jx: %f\r\n",joyX.read());
RobertoO 0:67c50348f842 369 }
RobertoO 0:67c50348f842 370 }