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

Code for the Poolinator

Dependencies:   mbed QEI HIDScope Pulse biquadFilter MODSERIAL FastPWM

main.cpp@8:1733338758d3, 2019-10-28 (annotated)

Committer:
sjoerd1999
Date:
Mon Oct 28 11:59:32 2019 +0000
Revision:
8:1733338758d3
Parent:
7:bfcb74384f46
Child:
9:913a59894698
Child:
11:5c5bd574c01a
updated;

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 3:d319bc2b19f1 18
sjoerd1999 8:1733338758d3 19 #define PI 3.14159265358979323846
sjoerd1999 8:1733338758d3 20
RobertoO 1:b862262a9d14 21 MODSERIAL pc(USBTX, USBRX);
RobertoO 0:67c50348f842 22
sjoerd1999 8:1733338758d3 23 AnalogIn pot(A1); // Potentiometer on BioRobotics shield
sjoerd1999 8:1733338758d3 24
sjoerd1999 8:1733338758d3 25 struct vec {
sjoerd1999 8:1733338758d3 26 double x,y,z;
sjoerd1999 8:1733338758d3 27 };
sjoerd1999 8:1733338758d3 28
sjoerd1999 8:1733338758d3 29 vec endPos{0,0,0};
sjoerd1999 3:d319bc2b19f1 30
sjoerd1999 8:1733338758d3 31 // DC MOTORS //
sjoerd1999 8:1733338758d3 32 QEI encoder1(D10,D11,NC,32), encoder3(D12,D13,NC,32), encoder2(PTC5,PTC7, NC, 32);
sjoerd1999 8:1733338758d3 33 PwmOut motor1_pwm(D5), motor3_pwm(D6);
sjoerd1999 8:1733338758d3 34 DigitalOut motor1_dir(D4), motor3_dir(D7), motor2_A(D2), motor2_B(D3);
sjoerd1999 7:bfcb74384f46 35
sjoerd1999 8:1733338758d3 36 float motor1_cur = 0, motor2_cur = 0, motor3_cur = 0;
sjoerd1999 8:1733338758d3 37 float motor1_tar = 0, motor2_tar = 0, motor3_tar = 0;
sjoerd1999 8:1733338758d3 38 float motor1_zero = 37.5, motor2_zero = 14.3, motor3_zero = 57.8; // in degrees(M1) / cm(M2/M3)
sjoerd1999 2:f8e0a7b5c90a 39
sjoerd1999 8:1733338758d3 40 void setMotor(int motor, float motor_spd) // Set the motor speed (between -1 and 1)
sjoerd1999 2:f8e0a7b5c90a 41 {
sjoerd1999 2:f8e0a7b5c90a 42 int motor_dir = (motor_spd >= 0) ? 0 : 1;
sjoerd1999 2:f8e0a7b5c90a 43 motor_spd = fabs(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 44
sjoerd1999 2:f8e0a7b5c90a 45 if(motor == 1) {
sjoerd1999 2:f8e0a7b5c90a 46 motor1_dir.write(motor_dir);
sjoerd1999 2:f8e0a7b5c90a 47 motor1_pwm.write(motor_spd);
sjoerd1999 8:1733338758d3 48 } else if(motor == 3) {
sjoerd1999 8:1733338758d3 49 motor3_dir.write(motor_dir);
sjoerd1999 8:1733338758d3 50 motor3_pwm.write(motor_spd);
sjoerd1999 2:f8e0a7b5c90a 51 } else if(motor == 2) {
sjoerd1999 8:1733338758d3 52 motor2_A.write((motor_dir == 0) ? 0 : int(motor_spd)); // Motor 2 has digital pins so no PWM
sjoerd1999 8:1733338758d3 53 motor2_B.write((motor_dir == 0) ? int(motor_spd) : 0);
sjoerd1999 2:f8e0a7b5c90a 54 }
sjoerd1999 2:f8e0a7b5c90a 55 }
sjoerd1999 2:f8e0a7b5c90a 56
sjoerd1999 8:1733338758d3 57 void getMotorPositions() // Calculate current motor positions (M1:angle, M2/M3:length) based on encoder pulses
sjoerd1999 8:1733338758d3 58 {
sjoerd1999 8:1733338758d3 59 // SOME SCALING NEEDED HERE //
sjoerd1999 8:1733338758d3 60 motor1_cur = encoder1.getPulses();
sjoerd1999 8:1733338758d3 61 motor2_cur = encoder2.getPulses();
sjoerd1999 8:1733338758d3 62 motor3_cur = encoder3.getPulses();
sjoerd1999 8:1733338758d3 63 }
sjoerd1999 8:1733338758d3 64
sjoerd1999 8:1733338758d3 65 void moveToTargets() // Move to the target positions. No PID.
sjoerd1999 8:1733338758d3 66 {
sjoerd1999 8:1733338758d3 67 float threshold = 100; // If close to the target pos, don't move
sjoerd1999 8:1733338758d3 68 setMotor(1, (fabs(motor1_cur - motor1_tar) < threshold) ? 0 : (motor1_cur < motor1_tar) ? 0.2 : -0.2);
sjoerd1999 8:1733338758d3 69 setMotor(2, (fabs(motor2_cur - motor2_tar) < threshold) ? 0 : (motor2_cur < motor2_tar) ? 1 : -1);
sjoerd1999 8:1733338758d3 70 setMotor(3, (fabs(motor3_cur - motor3_tar) < threshold) ? 0 : (motor3_cur < motor3_tar) ? 1: -1);
sjoerd1999 8:1733338758d3 71 }
sjoerd1999 8:1733338758d3 72
sjoerd1999 8:1733338758d3 73 // KINEMATICS //
sjoerd1999 8:1733338758d3 74 void calculateKinematics(float x, float y, float z) // y is up
sjoerd1999 8:1733338758d3 75 {
sjoerd1999 8:1733338758d3 76 float angle1 = fmod(2*PI - atan2(z, x), 2*PI);
sjoerd1999 8:1733338758d3 77 float angle2 = acos(sqrt(x*x + y*y + z*z) / 100.00) + atan2(sqrt(x*x + z*z), y);
sjoerd1999 8:1733338758d3 78 float angle3 = 2 * asin(sqrt(x*x + y*y + z*z) / 100.00);
sjoerd1999 8:1733338758d3 79
sjoerd1999 8:1733338758d3 80 motor1_tar = (angle1 < 30) ? 30 : (angle1 > 330) ? 330 : angle1; // constrain between 30 - 330
sjoerd1999 8:1733338758d3 81 motor2_tar = sqrt(200 - 200 * cos(angle2)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = c = 10cm)
sjoerd1999 8:1733338758d3 82 motor3_tar = sqrt(2600 - 1000 * cos(PI - angle3)); // a^2 = b^2 + c^2 - 2bc * cos(angle) (b = 10cm, c = 50cm)
sjoerd1999 8:1733338758d3 83 }
sjoerd1999 8:1733338758d3 84
sjoerd1999 8:1733338758d3 85 // STEPPER MOTOR //
sjoerd1999 2:f8e0a7b5c90a 86 DigitalOut STEPPER_IN1(PTB18), STEPPER_IN2(PTB19), STEPPER_IN3(PTC1), STEPPER_IN4(PTC8);
sjoerd1999 2:f8e0a7b5c90a 87
sjoerd1999 2:f8e0a7b5c90a 88 int stepper_steps = 0;
sjoerd1999 8:1733338758d3 89 float stepper_angle = 0, stepper_target = 0;
sjoerd1999 2:f8e0a7b5c90a 90
sjoerd1999 8:1733338758d3 91 void stepper_step(int direction_) // Requires ~1.5ms wait time between each step, 4096 steps is one rotation
sjoerd1999 2:f8e0a7b5c90a 92 {
sjoerd1999 3:d319bc2b19f1 93 STEPPER_IN1 = (stepper_steps == 5 || stepper_steps == 6 || stepper_steps == 7);
sjoerd1999 2:f8e0a7b5c90a 94 STEPPER_IN2 = (stepper_steps == 3 || stepper_steps == 4 || stepper_steps == 5);
sjoerd1999 2:f8e0a7b5c90a 95 STEPPER_IN3 = (stepper_steps == 1 || stepper_steps == 2 || stepper_steps == 3);
sjoerd1999 2:f8e0a7b5c90a 96 STEPPER_IN4 = (stepper_steps == 7 || stepper_steps == 0 || stepper_steps == 1);
sjoerd1999 2:f8e0a7b5c90a 97 stepper_steps += (direction_ == 0) ? - 1 : 1;
sjoerd1999 2:f8e0a7b5c90a 98 stepper_steps = (stepper_steps + 8) % 8;
sjoerd1999 3:d319bc2b19f1 99 stepper_angle += ((direction_ == 0) ? -1 : 1) * (360.00 / 4096.00);
sjoerd1999 2:f8e0a7b5c90a 100 }
sjoerd1999 2:f8e0a7b5c90a 101
sjoerd1999 8:1733338758d3 102 Ticker stepper_moveToAngle;
sjoerd1999 8:1733338758d3 103 void stepper_move() // Move toward desired angle with threshold. In Ticker function because requires wait otherwise
sjoerd1999 8:1733338758d3 104 {
sjoerd1999 8:1733338758d3 105 if(fabs(stepper_angle - stepper_target) > 1) stepper_step((stepper_angle < stepper_target) ? 1 : 0);
sjoerd1999 8:1733338758d3 106 }
sjoerd1999 8:1733338758d3 107
sjoerd1999 8:1733338758d3 108 // SERVO //
sjoerd1999 5:33133ebe37fd 109 PwmOut servo(D9);
sjoerd1999 8:1733338758d3 110 void setServo(float angle) // Set servo to specified angle(0-180 degrees)
sjoerd1999 5:33133ebe37fd 111 {
sjoerd1999 7:bfcb74384f46 112 float i = angle / 1.800;
sjoerd1999 5:33133ebe37fd 113 float duty = (i + 20) / 1000.00;
sjoerd1999 5:33133ebe37fd 114 servo.write(duty);
sjoerd1999 5:33133ebe37fd 115 }
sjoerd1999 3:d319bc2b19f1 116
sjoerd1999 8:1733338758d3 117 // AIMING //
sjoerd1999 8:1733338758d3 118 void aim(float angle) // Moves both stepper and servo so the end affector points towards desired angle
sjoerd1999 8:1733338758d3 119 {
sjoerd1999 8:1733338758d3 120 if(angle < 180) {
sjoerd1999 8:1733338758d3 121 servo.write(0);
sjoerd1999 8:1733338758d3 122 stepper_target = angle;
sjoerd1999 8:1733338758d3 123 } else {
sjoerd1999 8:1733338758d3 124 servo.write(180);
sjoerd1999 8:1733338758d3 125 stepper_target = angle - 180;
sjoerd1999 8:1733338758d3 126 }
sjoerd1999 8:1733338758d3 127 }
sjoerd1999 8:1733338758d3 128
sjoerd1999 8:1733338758d3 129 // SOLENOID //
sjoerd1999 5:33133ebe37fd 130 DigitalOut solenoidA(PTC0), solenoidB(PTC9);
sjoerd1999 8:1733338758d3 131 void setSolenoid(int dir) // 1 is out, 0 is in
sjoerd1999 3:d319bc2b19f1 132 {
sjoerd1999 7:bfcb74384f46 133 solenoidA = (dir == 0) ? 1 : 0;
sjoerd1999 7:bfcb74384f46 134 solenoidB = (dir == 0) ? 0 : 1;
sjoerd1999 7:bfcb74384f46 135 }
sjoerd1999 7:bfcb74384f46 136
sjoerd1999 8:1733338758d3 137 // LASER //
sjoerd1999 7:bfcb74384f46 138 DigitalOut laserPin(D8);
sjoerd1999 7:bfcb74384f46 139 void setLaser(bool on)
sjoerd1999 7:bfcb74384f46 140 {
sjoerd1999 7:bfcb74384f46 141 if(on) laserPin.write(1);
sjoerd1999 7:bfcb74384f46 142 else laserPin.write(0);
sjoerd1999 7:bfcb74384f46 143 }
sjoerd1999 7:bfcb74384f46 144
sjoerd1999 7:bfcb74384f46 145 // CALIBRATION //
sjoerd1999 7:bfcb74384f46 146 AnalogIn switch1(A2), switch2(A3), switch3(A4);
sjoerd1999 8:1733338758d3 147 void calibrate() // Calibrates all 3 motors simultaniously
sjoerd1999 7:bfcb74384f46 148 {
sjoerd1999 8:1733338758d3 149 setMotor(1,0.2); // Set all motors to move towards the switches
sjoerd1999 7:bfcb74384f46 150 setMotor(2,1);
sjoerd1999 7:bfcb74384f46 151 setMotor(3,1);
sjoerd1999 7:bfcb74384f46 152
sjoerd1999 8:1733338758d3 153 while(!(switch1.read() > 0.5 && switch2.read() > 0.5 && switch3.read() > 0.5)) { // When all switches have been pushed in, stop
sjoerd1999 7:bfcb74384f46 154 if(switch1.read() > 0.5) setMotor(1,0);
sjoerd1999 7:bfcb74384f46 155 if(switch2.read() > 0.5) setMotor(2,0);
sjoerd1999 7:bfcb74384f46 156 if(switch3.read() > 0.5) setMotor(3,0);
sjoerd1999 8:1733338758d3 157 wait_ms(40);
sjoerd1999 7:bfcb74384f46 158 }
sjoerd1999 8:1733338758d3 159 for(int i = 1; i <= 3; i++) setMotor(i,0); // Make sure they've all stopped
sjoerd1999 8:1733338758d3 160
sjoerd1999 8:1733338758d3 161 encoder1.reset(); // Reset encoder positions
sjoerd1999 7:bfcb74384f46 162 encoder2.reset();
sjoerd1999 7:bfcb74384f46 163 encoder3.reset();
sjoerd1999 8:1733338758d3 164
sjoerd1999 8:1733338758d3 165 //setServo(0);
sjoerd1999 3:d319bc2b19f1 166 }
sjoerd1999 3:d319bc2b19f1 167
sjoerd1999 3:d319bc2b19f1 168
RobertoO 0:67c50348f842 169 int main()
RobertoO 0:67c50348f842 170 {
RobertoO 0:67c50348f842 171 pc.baud(115200);
RobertoO 1:b862262a9d14 172 pc.printf("\r\nStarting...\r\n\r\n");
sjoerd1999 8:1733338758d3 173 motor1_pwm.period(0.0001);
sjoerd1999 8:1733338758d3 174 motor3_pwm.period(0.0001);
sjoerd1999 5:33133ebe37fd 175 servo.period(0.020);
sjoerd1999 8:1733338758d3 176 //stepper_moveToAngle.attach(&stepper_move, 0.0015);
sjoerd1999 2:f8e0a7b5c90a 177
sjoerd1999 8:1733338758d3 178 calibrate();
sjoerd1999 7:bfcb74384f46 179
RobertoO 0:67c50348f842 180 while (true) {
sjoerd1999 4:8113394bed1b 181 /*
sjoerd1999 4:8113394bed1b 182 SOME EXAPLE CODE
sjoerd1999 5:33133ebe37fd 183
sjoerd1999 4:8113394bed1b 184 * MOTOR
sjoerd1999 8:1733338758d3 185 setMotor(..., ...) // which motor (1,2,3), and speed (-1.0, +1.0)
sjoerd1999 5:33133ebe37fd 186
sjoerd1999 8:1733338758d3 187 * KINEMATICS (this should be done every ~30 ms)
sjoerd1999 8:1733338758d3 188 calculateKinematics(x, y, z); // Calculate target positions
sjoerd1999 8:1733338758d3 189 getMotorPositions(); // Calculate current positions
sjoerd1999 8:1733338758d3 190 moveToTargets(); // Set the motors speeds accordingly
sjoerd1999 3:d319bc2b19f1 191
sjoerd1999 4:8113394bed1b 192 * STEPPER
sjoerd1999 8:1733338758d3 193 stepper_target = ...; // angle (between 0.0 and 180.0)
sjoerd1999 4:8113394bed1b 194
sjoerd1999 4:8113394bed1b 195 * SERVO
sjoerd1999 7:bfcb74384f46 196 setServo(...) // value between 0.0 and 180.0 (= 0 and 180 degrees)
sjoerd1999 5:33133ebe37fd 197
sjoerd1999 8:1733338758d3 198 * AIMING
sjoerd1999 8:1733338758d3 199 aim(...) // value between 0 and 360
sjoerd1999 8:1733338758d3 200
sjoerd1999 5:33133ebe37fd 201 * SOLENOID
sjoerd1999 8:1733338758d3 202 setSolenoid(...); // position, 0(in) or 1(out)
sjoerd1999 8:1733338758d3 203
sjoerd1999 8:1733338758d3 204 * LASER
sjoerd1999 8:1733338758d3 205 setLaser(...) // 0(off) or 1(on)
sjoerd1999 5:33133ebe37fd 206
sjoerd1999 4:8113394bed1b 207 */
sjoerd1999 7:bfcb74384f46 208
sjoerd1999 8:1733338758d3 209 // 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 210 // endPos.z += (n > 30) ? 1 : (n > 20) ? 0 : -1;
sjoerd1999 8:1733338758d3 211 // endPos.x += (n % 10 == 1) ? 1 : (n % 10 == 2) ? 0 : -1;
sjoerd1999 8:1733338758d3 212
sjoerd1999 7:bfcb74384f46 213
sjoerd1999 7:bfcb74384f46 214 float value = pot.read();
sjoerd1999 8:1733338758d3 215 if(value < 0.4) setMotor(3, -1);
sjoerd1999 8:1733338758d3 216 else if (value > 0.6) setMotor(3, 1);
sjoerd1999 7:bfcb74384f46 217 else setMotor(3,0);
sjoerd1999 7:bfcb74384f46 218
sjoerd1999 7:bfcb74384f46 219 wait_ms(30);
sjoerd1999 7:bfcb74384f46 220
sjoerd1999 8:1733338758d3 221 //getMotorPositions();
sjoerd1999 8:1733338758d3 222 motor1_cur = encoder1.getPulses();
sjoerd1999 8:1733338758d3 223 motor2_cur = encoder2.getPulses();
sjoerd1999 8:1733338758d3 224 motor3_cur = encoder3.getPulses();
sjoerd1999 8:1733338758d3 225
sjoerd1999 8:1733338758d3 226
sjoerd1999 8:1733338758d3 227 pc.printf("m1: %d\r\n",motor1_cur);
sjoerd1999 8:1733338758d3 228 pc.printf("m2: %d\r\n",motor2_cur);
sjoerd1999 8:1733338758d3 229 pc.printf("m3: %d\r\n",motor3_cur);
RobertoO 0:67c50348f842 230 }
RobertoO 0:67c50348f842 231 }