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?
User | Revision | Line number | New 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 | } |