SweiLz / Mbed 2 deprecated FRA221_Class_XY

XY Plotter

Dependencies:   mbed

main.cpp@0:1c30cb57fc30, 2015-12-05 (annotated)

Committer:
sweilz
Date:
Sat Dec 05 19:17:54 2015 +0000
Revision:
0:1c30cb57fc30
XY Plotter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sweilz 0:1c30cb57fc30 1 #include "mbed.h"
sweilz 0:1c30cb57fc30 2 Serial pc(SERIAL_TX, SERIAL_RX);
sweilz 0:1c30cb57fc30 3
sweilz 0:1c30cb57fc30 4 #define CURVE_SECTION_MM 0.5
sweilz 0:1c30cb57fc30 5 float X_STEPS_PER_MM = 40;
sweilz 0:1c30cb57fc30 6 int X_MOTOR_STEPS = 100;
sweilz 0:1c30cb57fc30 7 float Y_STEPS_PER_MM = 40;
sweilz 0:1c30cb57fc30 8 int Y_MOTOR_STEPS = 100;
sweilz 0:1c30cb57fc30 9 long FAST_XY_FEEDRATE = 2500;
sweilz 0:1c30cb57fc30 10
sweilz 0:1c30cb57fc30 11 PinName X_STEP_PIN = PB_13;
sweilz 0:1c30cb57fc30 12 PinName X_DIR_PIN = PB_14;
sweilz 0:1c30cb57fc30 13 PinName Y_STEP_PIN = PA_12;
sweilz 0:1c30cb57fc30 14 PinName Y_DIR_PIN = PC_5;
sweilz 0:1c30cb57fc30 15 PinName X_MIN_PIN = PC_0;
sweilz 0:1c30cb57fc30 16 PinName X_MAX_PIN = PC_1;
sweilz 0:1c30cb57fc30 17 PinName Y_MIN_PIN = PB_0;
sweilz 0:1c30cb57fc30 18 PinName Y_MAX_PIN = PA_4;
sweilz 0:1c30cb57fc30 19
sweilz 0:1c30cb57fc30 20 struct FloatPoint {
sweilz 0:1c30cb57fc30 21 float x;
sweilz 0:1c30cb57fc30 22 float y;
sweilz 0:1c30cb57fc30 23 };
sweilz 0:1c30cb57fc30 24
sweilz 0:1c30cb57fc30 25 FloatPoint current_units;
sweilz 0:1c30cb57fc30 26 FloatPoint target_units;
sweilz 0:1c30cb57fc30 27 FloatPoint delta_units;
sweilz 0:1c30cb57fc30 28
sweilz 0:1c30cb57fc30 29 FloatPoint current_steps;
sweilz 0:1c30cb57fc30 30 FloatPoint target_steps;
sweilz 0:1c30cb57fc30 31 FloatPoint delta_steps;
sweilz 0:1c30cb57fc30 32
sweilz 0:1c30cb57fc30 33 int8_t x_direction = 1;
sweilz 0:1c30cb57fc30 34 int8_t y_direction = 1;
sweilz 0:1c30cb57fc30 35 long feedrate_micros = 0;
sweilz 0:1c30cb57fc30 36 long max_delta;
sweilz 0:1c30cb57fc30 37 long x_counter;
sweilz 0:1c30cb57fc30 38 long y_counter;
sweilz 0:1c30cb57fc30 39 int milli_delay;
sweilz 0:1c30cb57fc30 40 bool x_can_step;
sweilz 0:1c30cb57fc30 41 bool y_can_step;
sweilz 0:1c30cb57fc30 42
sweilz 0:1c30cb57fc30 43
sweilz 0:1c30cb57fc30 44
sweilz 0:1c30cb57fc30 45 long calculate_feedrate_delay(float feedrate);
sweilz 0:1c30cb57fc30 46 void dda_move(long micro_delay);
sweilz 0:1c30cb57fc30 47 void do_step(PinName pinA, PinName pinB, int8_t dir);
sweilz 0:1c30cb57fc30 48 bool can_step(PinName min_pin, PinName max_pin, long current, long target, int8_t direction);
sweilz 0:1c30cb57fc30 49 void calculate_deltas();
sweilz 0:1c30cb57fc30 50 void set_target(float x, float y);
sweilz 0:1c30cb57fc30 51
sweilz 0:1c30cb57fc30 52
sweilz 0:1c30cb57fc30 53 int main()
sweilz 0:1c30cb57fc30 54 {
sweilz 0:1c30cb57fc30 55 pc.baud(115200);
sweilz 0:1c30cb57fc30 56 pc.printf("Hello World\n");
sweilz 0:1c30cb57fc30 57
sweilz 0:1c30cb57fc30 58 current_units.x = 0.0;
sweilz 0:1c30cb57fc30 59 current_units.y = 0.0;
sweilz 0:1c30cb57fc30 60
sweilz 0:1c30cb57fc30 61 target_units.x = 0.0;
sweilz 0:1c30cb57fc30 62 target_units.y = 0.0;
sweilz 0:1c30cb57fc30 63
sweilz 0:1c30cb57fc30 64 calculate_deltas();
sweilz 0:1c30cb57fc30 65
sweilz 0:1c30cb57fc30 66 set_target(100.0, 200.0);
sweilz 0:1c30cb57fc30 67
sweilz 0:1c30cb57fc30 68 feedrate_micros = calculate_feedrate_delay(FAST_XY_FEEDRATE);
sweilz 0:1c30cb57fc30 69 dda_move(feedrate_micros);
sweilz 0:1c30cb57fc30 70 }
sweilz 0:1c30cb57fc30 71
sweilz 0:1c30cb57fc30 72 long calculate_feedrate_delay(float feedrate)
sweilz 0:1c30cb57fc30 73 {
sweilz 0:1c30cb57fc30 74 //how long is our line length?
sweilz 0:1c30cb57fc30 75 float distance = sqrt(delta_units.x * delta_units.x + delta_units.y * delta_units.y);
sweilz 0:1c30cb57fc30 76 long master_steps = 0;
sweilz 0:1c30cb57fc30 77
sweilz 0:1c30cb57fc30 78 //find the dominant axis.
sweilz 0:1c30cb57fc30 79 if (delta_steps.x > delta_steps.y)
sweilz 0:1c30cb57fc30 80 {
sweilz 0:1c30cb57fc30 81 master_steps = delta_steps.x;
sweilz 0:1c30cb57fc30 82 }
sweilz 0:1c30cb57fc30 83 else
sweilz 0:1c30cb57fc30 84 {
sweilz 0:1c30cb57fc30 85 master_steps = delta_steps.y;
sweilz 0:1c30cb57fc30 86 }
sweilz 0:1c30cb57fc30 87 return ((distance * 60000000.0) / feedrate) / master_steps;
sweilz 0:1c30cb57fc30 88 }
sweilz 0:1c30cb57fc30 89
sweilz 0:1c30cb57fc30 90
sweilz 0:1c30cb57fc30 91 void dda_move(long micro_delay)
sweilz 0:1c30cb57fc30 92 {
sweilz 0:1c30cb57fc30 93 //figure out our deltas
sweilz 0:1c30cb57fc30 94 max_delta = delta_steps.x>delta_steps.y? delta_steps.x:delta_steps.y;
sweilz 0:1c30cb57fc30 95
sweilz 0:1c30cb57fc30 96 //init stuff.
sweilz 0:1c30cb57fc30 97 long x_counter = -max_delta / 2;
sweilz 0:1c30cb57fc30 98 long y_counter = -max_delta / 2;
sweilz 0:1c30cb57fc30 99
sweilz 0:1c30cb57fc30 100 //our step flags
sweilz 0:1c30cb57fc30 101 bool x_can_step = 0;
sweilz 0:1c30cb57fc30 102 bool y_can_step = 0;
sweilz 0:1c30cb57fc30 103
sweilz 0:1c30cb57fc30 104 if (micro_delay >= 16383)
sweilz 0:1c30cb57fc30 105 milli_delay = micro_delay / 1000;
sweilz 0:1c30cb57fc30 106 else
sweilz 0:1c30cb57fc30 107 milli_delay = 0;
sweilz 0:1c30cb57fc30 108
sweilz 0:1c30cb57fc30 109 //do our DDA line!
sweilz 0:1c30cb57fc30 110 do
sweilz 0:1c30cb57fc30 111 {
sweilz 0:1c30cb57fc30 112 x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction);
sweilz 0:1c30cb57fc30 113 y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction);
sweilz 0:1c30cb57fc30 114
sweilz 0:1c30cb57fc30 115 if (x_can_step)
sweilz 0:1c30cb57fc30 116 {
sweilz 0:1c30cb57fc30 117 x_counter += delta_steps.x;
sweilz 0:1c30cb57fc30 118
sweilz 0:1c30cb57fc30 119 if (x_counter > 0)
sweilz 0:1c30cb57fc30 120 {
sweilz 0:1c30cb57fc30 121 do_step(X_STEP_PIN, X_DIR_PIN, x_direction);
sweilz 0:1c30cb57fc30 122 x_counter -= max_delta;
sweilz 0:1c30cb57fc30 123
sweilz 0:1c30cb57fc30 124 if (x_direction)
sweilz 0:1c30cb57fc30 125 current_steps.x++;
sweilz 0:1c30cb57fc30 126 else
sweilz 0:1c30cb57fc30 127 current_steps.x--;
sweilz 0:1c30cb57fc30 128 }
sweilz 0:1c30cb57fc30 129 }
sweilz 0:1c30cb57fc30 130
sweilz 0:1c30cb57fc30 131 if (y_can_step)
sweilz 0:1c30cb57fc30 132 {
sweilz 0:1c30cb57fc30 133 y_counter += delta_steps.y;
sweilz 0:1c30cb57fc30 134
sweilz 0:1c30cb57fc30 135 if (y_counter > 0)
sweilz 0:1c30cb57fc30 136 {
sweilz 0:1c30cb57fc30 137 do_step(Y_STEP_PIN, Y_DIR_PIN, y_direction);
sweilz 0:1c30cb57fc30 138 y_counter -= max_delta;
sweilz 0:1c30cb57fc30 139
sweilz 0:1c30cb57fc30 140 if (y_direction)
sweilz 0:1c30cb57fc30 141 current_steps.y++;
sweilz 0:1c30cb57fc30 142 else
sweilz 0:1c30cb57fc30 143 current_steps.y--;
sweilz 0:1c30cb57fc30 144 }
sweilz 0:1c30cb57fc30 145 }
sweilz 0:1c30cb57fc30 146
sweilz 0:1c30cb57fc30 147
sweilz 0:1c30cb57fc30 148 //wait for next step.
sweilz 0:1c30cb57fc30 149 if (milli_delay > 0)
sweilz 0:1c30cb57fc30 150 wait(milli_delay);
sweilz 0:1c30cb57fc30 151 else
sweilz 0:1c30cb57fc30 152 wait_us(micro_delay);
sweilz 0:1c30cb57fc30 153 }
sweilz 0:1c30cb57fc30 154 while (x_can_step || y_can_step);
sweilz 0:1c30cb57fc30 155
sweilz 0:1c30cb57fc30 156 //set our points to be the same
sweilz 0:1c30cb57fc30 157 current_units.x = target_units.x;
sweilz 0:1c30cb57fc30 158 current_units.y = target_units.y;
sweilz 0:1c30cb57fc30 159 calculate_deltas();
sweilz 0:1c30cb57fc30 160 }
sweilz 0:1c30cb57fc30 161 bool can_step(PinName min_pin, PinName max_pin, long current, long target, int8_t direction)
sweilz 0:1c30cb57fc30 162 {
sweilz 0:1c30cb57fc30 163 DigitalIn Max(max_pin,PullUp);
sweilz 0:1c30cb57fc30 164 DigitalIn Min(min_pin,PullUp);
sweilz 0:1c30cb57fc30 165 //stop us if we're on target
sweilz 0:1c30cb57fc30 166 if (target == current)
sweilz 0:1c30cb57fc30 167 return false;
sweilz 0:1c30cb57fc30 168 //stop us if we're at home and still going
sweilz 0:1c30cb57fc30 169 else if (!Min.read() && !direction)
sweilz 0:1c30cb57fc30 170 return false;
sweilz 0:1c30cb57fc30 171 //stop us if we're at max and still going
sweilz 0:1c30cb57fc30 172 else if (!Max.read() && direction)
sweilz 0:1c30cb57fc30 173 return false;
sweilz 0:1c30cb57fc30 174
sweilz 0:1c30cb57fc30 175 //default to being able to step
sweilz 0:1c30cb57fc30 176 return true;
sweilz 0:1c30cb57fc30 177 }
sweilz 0:1c30cb57fc30 178
sweilz 0:1c30cb57fc30 179 void calculate_deltas()
sweilz 0:1c30cb57fc30 180 {
sweilz 0:1c30cb57fc30 181 //figure our deltas.
sweilz 0:1c30cb57fc30 182 delta_units.x = abs(target_units.x - current_units.x);
sweilz 0:1c30cb57fc30 183 delta_units.y = abs(target_units.y - current_units.y);
sweilz 0:1c30cb57fc30 184
sweilz 0:1c30cb57fc30 185 //set our steps current, target, and delta
sweilz 0:1c30cb57fc30 186 current_steps.x = X_STEPS_PER_MM * current_units.x;
sweilz 0:1c30cb57fc30 187 current_steps.y = Y_STEPS_PER_MM * current_units.y;
sweilz 0:1c30cb57fc30 188
sweilz 0:1c30cb57fc30 189 target_steps.x = X_STEPS_PER_MM * target_units.x;
sweilz 0:1c30cb57fc30 190 target_steps.y = Y_STEPS_PER_MM * target_units.y;
sweilz 0:1c30cb57fc30 191
sweilz 0:1c30cb57fc30 192 delta_steps.x = abs(target_steps.x - current_steps.x);
sweilz 0:1c30cb57fc30 193 delta_steps.y = abs(target_steps.y - current_steps.y);
sweilz 0:1c30cb57fc30 194
sweilz 0:1c30cb57fc30 195 //what is our direction
sweilz 0:1c30cb57fc30 196 x_direction = (target_units.x >= current_units.x);
sweilz 0:1c30cb57fc30 197 y_direction = (target_units.y >= current_units.y);
sweilz 0:1c30cb57fc30 198
sweilz 0:1c30cb57fc30 199 //set our direction pins as well
sweilz 0:1c30cb57fc30 200 DigitalOut X_DIR(X_DIR_PIN,x_direction);
sweilz 0:1c30cb57fc30 201 DigitalOut Y_DIR(Y_DIR_PIN,y_direction);
sweilz 0:1c30cb57fc30 202 }
sweilz 0:1c30cb57fc30 203
sweilz 0:1c30cb57fc30 204
sweilz 0:1c30cb57fc30 205 void set_target(float x, float y)
sweilz 0:1c30cb57fc30 206 {
sweilz 0:1c30cb57fc30 207 target_units.x = x;
sweilz 0:1c30cb57fc30 208 target_units.y = y;
sweilz 0:1c30cb57fc30 209
sweilz 0:1c30cb57fc30 210 calculate_deltas();
sweilz 0:1c30cb57fc30 211 }
sweilz 0:1c30cb57fc30 212
sweilz 0:1c30cb57fc30 213 void do_step(PinName pinA, PinName pinB, int8_t dir)
sweilz 0:1c30cb57fc30 214 {
sweilz 0:1c30cb57fc30 215 DigitalOut A(pinA);
sweilz 0:1c30cb57fc30 216 DigitalOut B(pinB);
sweilz 0:1c30cb57fc30 217 switch (dir << 2 | A.read() << 1 | B.read()) {
sweilz 0:1c30cb57fc30 218 case 0: /* 0 00 -> 10 */
sweilz 0:1c30cb57fc30 219 case 5: /* 1 01 -> 11 */
sweilz 0:1c30cb57fc30 220 A.write(1);
sweilz 0:1c30cb57fc30 221 break;
sweilz 0:1c30cb57fc30 222 case 1: /* 0 01 -> 00 */
sweilz 0:1c30cb57fc30 223 case 7: /* 1 11 -> 10 */
sweilz 0:1c30cb57fc30 224 B.write(0);
sweilz 0:1c30cb57fc30 225 break;
sweilz 0:1c30cb57fc30 226 case 2: /* 0 10 -> 11 */
sweilz 0:1c30cb57fc30 227 case 4: /* 1 00 -> 01 */
sweilz 0:1c30cb57fc30 228 B.write(1);
sweilz 0:1c30cb57fc30 229 break;
sweilz 0:1c30cb57fc30 230 case 3: /* 0 11 -> 01 */
sweilz 0:1c30cb57fc30 231 case 6: /* 1 10 -> 00 */
sweilz 0:1c30cb57fc30 232 A.write(0);
sweilz 0:1c30cb57fc30 233 break;
sweilz 0:1c30cb57fc30 234 }
sweilz 0:1c30cb57fc30 235 wait_us(5);
sweilz 0:1c30cb57fc30 236 }