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

Code for the Poolinator

Dependencies:   mbed QEI HIDScope Pulse biquadFilter MODSERIAL FastPWM

main.cpp@12:c3fd0712f43d, 2019-10-31 (annotated)

Committer:
sjoerd1999
Date:
Thu Oct 31 16:58:17 2019 +0000
Revision:
12:c3fd0712f43d
Parent:
9:913a59894698
Child:
13:74f2e8d3e04e
WORKING BIATCHHHHHH

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