Suspended plotter for the skaperfest
Dependencies: mbed HTTPServer EthernetNetIf FatFileSystemCpp
SuspendedPlotter.cpp@0:602ff2b2d41c, 2022-08-22 (annotated)
- Committer:
- rengro01
- Date:
- Mon Aug 22 10:24:23 2022 +0000
- Revision:
- 0:602ff2b2d41c
skaperfest
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rengro01 | 0:602ff2b2d41c | 1 | #include "SuspendedPlotter.h" |
rengro01 | 0:602ff2b2d41c | 2 | |
rengro01 | 0:602ff2b2d41c | 3 | // Keep speeds low to increase torque |
rengro01 | 0:602ff2b2d41c | 4 | #define SERVO_SPEED 8 |
rengro01 | 0:602ff2b2d41c | 5 | #define STEPPER_SPEED 4 |
rengro01 | 0:602ff2b2d41c | 6 | |
rengro01 | 0:602ff2b2d41c | 7 | // 1000, 1500, 2000 |
rengro01 | 0:602ff2b2d41c | 8 | |
rengro01 | 0:602ff2b2d41c | 9 | SuspendedPlotter::Servo::Servo(PinName pn) |
rengro01 | 0:602ff2b2d41c | 10 | : m_pwm(pn) |
rengro01 | 0:602ff2b2d41c | 11 | , m_pulse_width(0.0015) |
rengro01 | 0:602ff2b2d41c | 12 | { |
rengro01 | 0:602ff2b2d41c | 13 | m_pwm.period(0.020); |
rengro01 | 0:602ff2b2d41c | 14 | m_pwm.pulsewidth(m_pulse_width); |
rengro01 | 0:602ff2b2d41c | 15 | } |
rengro01 | 0:602ff2b2d41c | 16 | |
rengro01 | 0:602ff2b2d41c | 17 | void SuspendedPlotter::Servo::open() |
rengro01 | 0:602ff2b2d41c | 18 | { |
rengro01 | 0:602ff2b2d41c | 19 | while(m_pulse_width > 0.0015) |
rengro01 | 0:602ff2b2d41c | 20 | { |
rengro01 | 0:602ff2b2d41c | 21 | m_pwm.pulsewidth(m_pulse_width); |
rengro01 | 0:602ff2b2d41c | 22 | wait_ms(SERVO_SPEED); |
rengro01 | 0:602ff2b2d41c | 23 | m_pulse_width -= 0.000005; |
rengro01 | 0:602ff2b2d41c | 24 | } |
rengro01 | 0:602ff2b2d41c | 25 | } |
rengro01 | 0:602ff2b2d41c | 26 | |
rengro01 | 0:602ff2b2d41c | 27 | void SuspendedPlotter::Servo::close() |
rengro01 | 0:602ff2b2d41c | 28 | { |
rengro01 | 0:602ff2b2d41c | 29 | while(m_pulse_width < 0.002) |
rengro01 | 0:602ff2b2d41c | 30 | { |
rengro01 | 0:602ff2b2d41c | 31 | m_pwm.pulsewidth(m_pulse_width); |
rengro01 | 0:602ff2b2d41c | 32 | wait_ms(SERVO_SPEED); |
rengro01 | 0:602ff2b2d41c | 33 | m_pulse_width += 0.000005; |
rengro01 | 0:602ff2b2d41c | 34 | } |
rengro01 | 0:602ff2b2d41c | 35 | } |
rengro01 | 0:602ff2b2d41c | 36 | |
rengro01 | 0:602ff2b2d41c | 37 | SuspendedPlotter::Stepper::Stepper(PinName pn1, PinName pn2, PinName pn3, PinName pn4) |
rengro01 | 0:602ff2b2d41c | 38 | : m_step(0) |
rengro01 | 0:602ff2b2d41c | 39 | , m_in1(pn1) |
rengro01 | 0:602ff2b2d41c | 40 | , m_in2(pn2) |
rengro01 | 0:602ff2b2d41c | 41 | , m_in3(pn3) |
rengro01 | 0:602ff2b2d41c | 42 | , m_in4(pn4) |
rengro01 | 0:602ff2b2d41c | 43 | { |
rengro01 | 0:602ff2b2d41c | 44 | for (int i = 0; i < 8; i++) |
rengro01 | 0:602ff2b2d41c | 45 | stepTo(-1); |
rengro01 | 0:602ff2b2d41c | 46 | for (int i = 0; i < 8; i++) |
rengro01 | 0:602ff2b2d41c | 47 | stepTo(1); |
rengro01 | 0:602ff2b2d41c | 48 | } |
rengro01 | 0:602ff2b2d41c | 49 | |
rengro01 | 0:602ff2b2d41c | 50 | void SuspendedPlotter::Stepper::stepTo(int dir) |
rengro01 | 0:602ff2b2d41c | 51 | { |
rengro01 | 0:602ff2b2d41c | 52 | static const int steps[8][4] = { |
rengro01 | 0:602ff2b2d41c | 53 | {1, 0, 0, 0}, |
rengro01 | 0:602ff2b2d41c | 54 | {1, 1, 0, 0}, |
rengro01 | 0:602ff2b2d41c | 55 | {0, 1, 0, 0}, |
rengro01 | 0:602ff2b2d41c | 56 | {0, 1, 1, 0}, |
rengro01 | 0:602ff2b2d41c | 57 | {0, 0, 1, 0}, |
rengro01 | 0:602ff2b2d41c | 58 | {0, 0, 1, 1}, |
rengro01 | 0:602ff2b2d41c | 59 | {0, 0, 0, 1}, |
rengro01 | 0:602ff2b2d41c | 60 | {1, 0, 0, 1} |
rengro01 | 0:602ff2b2d41c | 61 | }; |
rengro01 | 0:602ff2b2d41c | 62 | m_step = (m_step+dir+8) % 8; |
rengro01 | 0:602ff2b2d41c | 63 | |
rengro01 | 0:602ff2b2d41c | 64 | m_in1 = steps[m_step][0]; |
rengro01 | 0:602ff2b2d41c | 65 | m_in2 = steps[m_step][1]; |
rengro01 | 0:602ff2b2d41c | 66 | m_in3 = steps[m_step][2]; |
rengro01 | 0:602ff2b2d41c | 67 | m_in4 = steps[m_step][3]; |
rengro01 | 0:602ff2b2d41c | 68 | } |
rengro01 | 0:602ff2b2d41c | 69 | |
rengro01 | 0:602ff2b2d41c | 70 | SuspendedPlotter::SuspendedPlotter() |
rengro01 | 0:602ff2b2d41c | 71 | : m_left(p17, p18, p19, p20) |
rengro01 | 0:602ff2b2d41c | 72 | , m_right(p13, p14, p15, p16) |
rengro01 | 0:602ff2b2d41c | 73 | , m_servo(p21) |
rengro01 | 0:602ff2b2d41c | 74 | , m_x(0.5) |
rengro01 | 0:602ff2b2d41c | 75 | , m_y(0.5) |
rengro01 | 0:602ff2b2d41c | 76 | { |
rengro01 | 0:602ff2b2d41c | 77 | } |
rengro01 | 0:602ff2b2d41c | 78 | |
rengro01 | 0:602ff2b2d41c | 79 | void SuspendedPlotter::reset() |
rengro01 | 0:602ff2b2d41c | 80 | { |
rengro01 | 0:602ff2b2d41c | 81 | m_x = 0.5; |
rengro01 | 0:602ff2b2d41c | 82 | m_y = 0.5; |
rengro01 | 0:602ff2b2d41c | 83 | } |
rengro01 | 0:602ff2b2d41c | 84 | |
rengro01 | 0:602ff2b2d41c | 85 | void SuspendedPlotter::startDraw() |
rengro01 | 0:602ff2b2d41c | 86 | { |
rengro01 | 0:602ff2b2d41c | 87 | m_servo.close(); |
rengro01 | 0:602ff2b2d41c | 88 | } |
rengro01 | 0:602ff2b2d41c | 89 | |
rengro01 | 0:602ff2b2d41c | 90 | void SuspendedPlotter::stopDraw() |
rengro01 | 0:602ff2b2d41c | 91 | { |
rengro01 | 0:602ff2b2d41c | 92 | m_servo.open(); |
rengro01 | 0:602ff2b2d41c | 93 | } |
rengro01 | 0:602ff2b2d41c | 94 | |
rengro01 | 0:602ff2b2d41c | 95 | void SuspendedPlotter::moveTo(float x, float y) |
rengro01 | 0:602ff2b2d41c | 96 | { |
rengro01 | 0:602ff2b2d41c | 97 | static const float MIN_Y = 0.2; |
rengro01 | 0:602ff2b2d41c | 98 | static const float MAX_Y = 2.0; |
rengro01 | 0:602ff2b2d41c | 99 | if (x < 0.0) |
rengro01 | 0:602ff2b2d41c | 100 | x = 0.0; |
rengro01 | 0:602ff2b2d41c | 101 | if (x > 1.0) |
rengro01 | 0:602ff2b2d41c | 102 | x = 1.0; |
rengro01 | 0:602ff2b2d41c | 103 | if (y < MIN_Y) |
rengro01 | 0:602ff2b2d41c | 104 | y = MIN_Y; |
rengro01 | 0:602ff2b2d41c | 105 | if (y > MAX_Y) |
rengro01 | 0:602ff2b2d41c | 106 | y = MAX_Y; |
rengro01 | 0:602ff2b2d41c | 107 | |
rengro01 | 0:602ff2b2d41c | 108 | /* Adaptation of Bresenham's line algorithm */ |
rengro01 | 0:602ff2b2d41c | 109 | /* 280 mm per 10000 steps */ |
rengro01 | 0:602ff2b2d41c | 110 | /* Base is 815 mm or 29107 steps */ |
rengro01 | 0:602ff2b2d41c | 111 | /* Diagonals at 0.5 are 576mm */ /*280 left , 280 right */ |
rengro01 | 0:602ff2b2d41c | 112 | static const float BASE_STEPS_L = 29107; |
rengro01 | 0:602ff2b2d41c | 113 | static const float BASE_STEPS_R = 29107; |
rengro01 | 0:602ff2b2d41c | 114 | int left_0 = hypot(m_x * BASE_STEPS_L, m_y * BASE_STEPS_L); |
rengro01 | 0:602ff2b2d41c | 115 | int right_0 = hypot(BASE_STEPS_R - m_x * BASE_STEPS_R, m_y * BASE_STEPS_R); |
rengro01 | 0:602ff2b2d41c | 116 | int left_1 = hypot(x * BASE_STEPS_L, y * BASE_STEPS_L); |
rengro01 | 0:602ff2b2d41c | 117 | int right_1 = hypot(BASE_STEPS_R - x * BASE_STEPS_R, y * BASE_STEPS_R); |
rengro01 | 0:602ff2b2d41c | 118 | |
rengro01 | 0:602ff2b2d41c | 119 | int d_right = abs(left_1 - left_0); |
rengro01 | 0:602ff2b2d41c | 120 | int s_right = left_0 < left_1 ? 1 : -1; |
rengro01 | 0:602ff2b2d41c | 121 | int d_left = abs(right_1 - right_0); |
rengro01 | 0:602ff2b2d41c | 122 | int s_left = right_0 < right_1 ? 1 : -1; |
rengro01 | 0:602ff2b2d41c | 123 | int err = (d_right > d_left ? d_right : -d_left) / 2, e2; |
rengro01 | 0:602ff2b2d41c | 124 | |
rengro01 | 0:602ff2b2d41c | 125 | while ((left_0 != left_1) || (right_0 != right_1)) |
rengro01 | 0:602ff2b2d41c | 126 | { |
rengro01 | 0:602ff2b2d41c | 127 | if (left_0 == left_1 && right_0 == right_1) |
rengro01 | 0:602ff2b2d41c | 128 | break; |
rengro01 | 0:602ff2b2d41c | 129 | e2 = err; |
rengro01 | 0:602ff2b2d41c | 130 | if (e2 > -d_right) |
rengro01 | 0:602ff2b2d41c | 131 | { |
rengro01 | 0:602ff2b2d41c | 132 | err -= d_left; |
rengro01 | 0:602ff2b2d41c | 133 | left_0 += s_right; |
rengro01 | 0:602ff2b2d41c | 134 | m_left.stepTo(-s_right); /* Should be +, but the left motor is reversed */ |
rengro01 | 0:602ff2b2d41c | 135 | //printf("LEFT: current=%d, left_0=%d, left_1=%d, s_right=%d\n\r", m_left.getPosition(), left_0, left_1, s_right); |
rengro01 | 0:602ff2b2d41c | 136 | } |
rengro01 | 0:602ff2b2d41c | 137 | if (e2 < d_left) |
rengro01 | 0:602ff2b2d41c | 138 | { |
rengro01 | 0:602ff2b2d41c | 139 | err += d_right; |
rengro01 | 0:602ff2b2d41c | 140 | right_0 += s_left; |
rengro01 | 0:602ff2b2d41c | 141 | m_right.stepTo(-s_left); |
rengro01 | 0:602ff2b2d41c | 142 | //printf("RIGHT: current=%d, right_0=%d, right1=%d, s_left=%d\n\r", m_right.getPosition(), right_0, right_1, s_left); |
rengro01 | 0:602ff2b2d41c | 143 | } |
rengro01 | 0:602ff2b2d41c | 144 | wait_ms(STEPPER_SPEED); |
rengro01 | 0:602ff2b2d41c | 145 | } |
rengro01 | 0:602ff2b2d41c | 146 | |
rengro01 | 0:602ff2b2d41c | 147 | m_x = x; |
rengro01 | 0:602ff2b2d41c | 148 | m_y = y; |
rengro01 | 0:602ff2b2d41c | 149 | } |
rengro01 | 0:602ff2b2d41c | 150 | |
rengro01 | 0:602ff2b2d41c | 151 | void SuspendedPlotter::unrollLeft(int steps) |
rengro01 | 0:602ff2b2d41c | 152 | { |
rengro01 | 0:602ff2b2d41c | 153 | for (int i = 0; i < steps; i++) |
rengro01 | 0:602ff2b2d41c | 154 | { |
rengro01 | 0:602ff2b2d41c | 155 | m_left.stepTo(-1); /* Should be +, but the left motor is reversed */ |
rengro01 | 0:602ff2b2d41c | 156 | wait_ms(STEPPER_SPEED); |
rengro01 | 0:602ff2b2d41c | 157 | } |
rengro01 | 0:602ff2b2d41c | 158 | } |
rengro01 | 0:602ff2b2d41c | 159 | |
rengro01 | 0:602ff2b2d41c | 160 | void SuspendedPlotter::unrollRight(int steps) |
rengro01 | 0:602ff2b2d41c | 161 | { |
rengro01 | 0:602ff2b2d41c | 162 | for (int i = 0; i < steps; i++) |
rengro01 | 0:602ff2b2d41c | 163 | { |
rengro01 | 0:602ff2b2d41c | 164 | m_right.stepTo(-1); |
rengro01 | 0:602ff2b2d41c | 165 | wait_ms(STEPPER_SPEED); |
rengro01 | 0:602ff2b2d41c | 166 | } |
rengro01 | 0:602ff2b2d41c | 167 | } |
rengro01 | 0:602ff2b2d41c | 168 | |
rengro01 | 0:602ff2b2d41c | 169 | void SuspendedPlotter::rollLeft(int steps) |
rengro01 | 0:602ff2b2d41c | 170 | { |
rengro01 | 0:602ff2b2d41c | 171 | for (int i = 0; i < steps; i++) |
rengro01 | 0:602ff2b2d41c | 172 | { |
rengro01 | 0:602ff2b2d41c | 173 | m_left.stepTo(1); /* Should be -, but the left motor is reversed */ |
rengro01 | 0:602ff2b2d41c | 174 | wait_ms(STEPPER_SPEED); |
rengro01 | 0:602ff2b2d41c | 175 | } |
rengro01 | 0:602ff2b2d41c | 176 | } |
rengro01 | 0:602ff2b2d41c | 177 | |
rengro01 | 0:602ff2b2d41c | 178 | void SuspendedPlotter::rollRight(int steps) |
rengro01 | 0:602ff2b2d41c | 179 | { |
rengro01 | 0:602ff2b2d41c | 180 | for (int i = 0; i < steps; i++) |
rengro01 | 0:602ff2b2d41c | 181 | { |
rengro01 | 0:602ff2b2d41c | 182 | m_right.stepTo(1); |
rengro01 | 0:602ff2b2d41c | 183 | wait_ms(STEPPER_SPEED); |
rengro01 | 0:602ff2b2d41c | 184 | } |
rengro01 | 0:602ff2b2d41c | 185 | } |