Renato Grottesi / Mbed 2 deprecated SuspendedPlotterIoT

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?

UserRevisionLine numberNew 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 }