Michael Spencer / Mbed 2 deprecated LaOS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

LaosMotion/grbl/stepper.cpp@1:f5ac63519541, 2014-03-05 (annotated)

Committer:
Michael J. Spencer
Date:
Wed Mar 05 06:14:02 2014 -0800
Revision:
1:f5ac63519541
Initial commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Michael J. Spencer 1:f5ac63519541 1 /*
Michael J. Spencer 1:f5ac63519541 2 stepper.c - stepper motor driver: executes motion plans using stepper motors
Michael J. Spencer 1:f5ac63519541 3 Part of Grbl
Michael J. Spencer 1:f5ac63519541 4 Taken from R2C2 project and modified for Laos by Peter Brier
Michael J. Spencer 1:f5ac63519541 5 stripped, included some Marlin changes from Erik van de Zalm
Michael J. Spencer 1:f5ac63519541 6
Michael J. Spencer 1:f5ac63519541 7 Copyright (c) 2009-2011 Simen Svale Skogsrud
Michael J. Spencer 1:f5ac63519541 8 Modifications Copyright (c) 2011 Sungeun K. Jeon
Michael J. Spencer 1:f5ac63519541 9 Modifications Copyright (c) 2011 Peter Brier
Michael J. Spencer 1:f5ac63519541 10
Michael J. Spencer 1:f5ac63519541 11 Grbl is free software: you can redistribute it and/or modify
Michael J. Spencer 1:f5ac63519541 12 it under the terms of the GNU General Public License as published by
Michael J. Spencer 1:f5ac63519541 13 the Free Software Foundation, either version 3 of the License, orc
Michael J. Spencer 1:f5ac63519541 14 (at your option) any later version.
Michael J. Spencer 1:f5ac63519541 15
Michael J. Spencer 1:f5ac63519541 16 Grbl is distributed in the hope that it will be useful,
Michael J. Spencer 1:f5ac63519541 17 but WITHOUT ANY WARRANTY; without even the implied warrlaanty of
Michael J. Spencer 1:f5ac63519541 18 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
Michael J. Spencer 1:f5ac63519541 19 GNU General Public License for more details.
Michael J. Spencer 1:f5ac63519541 20
Michael J. Spencer 1:f5ac63519541 21 You should have received a copy of the GNU General Public License
Michael J. Spencer 1:f5ac63519541 22 along with Grbl. If not, see <http://www.gnu.org/licenses/>.
Michael J. Spencer 1:f5ac63519541 23 */
Michael J. Spencer 1:f5ac63519541 24
Michael J. Spencer 1:f5ac63519541 25 /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
Michael J. Spencer 1:f5ac63519541 26 and Philipp Tiefenbacher. */
Michael J. Spencer 1:f5ac63519541 27
Michael J. Spencer 1:f5ac63519541 28 /* The acceleration profiles are derived using the techniques descibed in the paper
Michael J. Spencer 1:f5ac63519541 29 "Generate stepper-motor speed profiles in real time" David Austin 2004
Michael J. Spencer 1:f5ac63519541 30 published at http://www.eetimes.com/design/embedded/4006438/Generate-stepper-motor-speed-profiles-in-real-time and others.
Michael J. Spencer 1:f5ac63519541 31 */
Michael J. Spencer 1:f5ac63519541 32 #include <LaosMotion.h>
Michael J. Spencer 1:f5ac63519541 33 #include "pins.h"
Michael J. Spencer 1:f5ac63519541 34
Michael J. Spencer 1:f5ac63519541 35 #include <math.h>
Michael J. Spencer 1:f5ac63519541 36 #include <stdlib.h>
Michael J. Spencer 1:f5ac63519541 37 #include <string.h>
Michael J. Spencer 1:f5ac63519541 38
Michael J. Spencer 1:f5ac63519541 39 #include "fixedpt.h"
Michael J. Spencer 1:f5ac63519541 40 #include "stepper.h"
Michael J. Spencer 1:f5ac63519541 41 #include "config.h"
Michael J. Spencer 1:f5ac63519541 42 #include "planner.h"
Michael J. Spencer 1:f5ac63519541 43
Michael J. Spencer 1:f5ac63519541 44
Michael J. Spencer 1:f5ac63519541 45 #define TICKS_PER_MICROSECOND (1) // Ticker uses 1usec units
Michael J. Spencer 1:f5ac63519541 46 // #define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
Michael J. Spencer 1:f5ac63519541 47 #define STEP_TIMER_FREQ 1000000 // 1 MHz
Michael J. Spencer 1:f5ac63519541 48
Michael J. Spencer 1:f5ac63519541 49 // types: ramp state
Michael J. Spencer 1:f5ac63519541 50 typedef enum {RAMP_UP, RAMP_MAX, RAMP_DOWN} tRamp;
Michael J. Spencer 1:f5ac63519541 51
Michael J. Spencer 1:f5ac63519541 52 // Prototypes
Michael J. Spencer 1:f5ac63519541 53 static void st_interrupt ();
Michael J. Spencer 1:f5ac63519541 54 static void set_step_timer (uint32_t cycles);
Michael J. Spencer 1:f5ac63519541 55 static void st_go_idle();
Michael J. Spencer 1:f5ac63519541 56
Michael J. Spencer 1:f5ac63519541 57 // Globals
Michael J. Spencer 1:f5ac63519541 58 volatile unsigned char busy = 0;
Michael J. Spencer 1:f5ac63519541 59 volatile int32_t actpos_x, actpos_y, actpos_z, actpos_e; // actual position
Michael J. Spencer 1:f5ac63519541 60
Michael J. Spencer 1:f5ac63519541 61 // Locals
Michael J. Spencer 1:f5ac63519541 62 static block_t *current_block; // A pointer to the block currently being traced
Michael J. Spencer 1:f5ac63519541 63 static Ticker timer; // the periodic timer used to step
Michael J. Spencer 1:f5ac63519541 64 static tFixedPt pwmofs; // the offset of the PWM value
Michael J. Spencer 1:f5ac63519541 65 static tFixedPt pwmscale; // the scaling of the PWM value
Michael J. Spencer 1:f5ac63519541 66 static volatile int running = 0; // stepper irq is running
Michael J. Spencer 1:f5ac63519541 67
Michael J. Spencer 1:f5ac63519541 68 static uint32_t direction_inv; // invert mask for direction bits
Michael J. Spencer 1:f5ac63519541 69 static uint32_t direction_bits; // all axes direction (different ports)
Michael J. Spencer 1:f5ac63519541 70 static uint32_t step_bits; // all axis step bits
Michael J. Spencer 1:f5ac63519541 71 static uint32_t step_inv; // invert mask for the stepper bits
Michael J. Spencer 1:f5ac63519541 72 static uint32_t nominal_rate; // [steps/min]
Michael J. Spencer 1:f5ac63519541 73 static int32_t counter_x, // Counter variables for the bresenham line tracer
Michael J. Spencer 1:f5ac63519541 74 counter_y,
Michael J. Spencer 1:f5ac63519541 75 counter_z;
Michael J. Spencer 1:f5ac63519541 76 static int32_t counter_e, counter_l, pos_l; // extruder and laser
Michael J. Spencer 1:f5ac63519541 77 static uint32_t step_events_completed; // The number of step events executed in the current block
Michael J. Spencer 1:f5ac63519541 78
Michael J. Spencer 1:f5ac63519541 79 // Variables used by the trapezoid generation
Michael J. Spencer 1:f5ac63519541 80 //static uint32_t cycles_per_step_event; // The number of machine cycles between each step event
Michael J. Spencer 1:f5ac63519541 81 static uint32_t trapezoid_tick_cycle_counter; // The cycles since last trapezoid_tick. Used to generate ticks at a steady
Michael J. Spencer 1:f5ac63519541 82 // pace without allocating a separate timer
Michael J. Spencer 1:f5ac63519541 83 static uint32_t trapezoid_adjusted_rate; // The current rate of step_events according to the trapezoid generator
Michael J. Spencer 1:f5ac63519541 84
Michael J. Spencer 1:f5ac63519541 85 static tFixedPt c; // current clock cycle count [1/speed]
Michael J. Spencer 1:f5ac63519541 86 static int32_t c_min; // minimal clock cycle count [at vnominal for this block]
Michael J. Spencer 1:f5ac63519541 87 static int32_t n;
Michael J. Spencer 1:f5ac63519541 88 static int32_t decel_n;
Michael J. Spencer 1:f5ac63519541 89 static tRamp ramp; // state of state machine for ramping up/down
Michael J. Spencer 1:f5ac63519541 90
Michael J. Spencer 1:f5ac63519541 91 extern unsigned char bitmap_bpp;
Michael J. Spencer 1:f5ac63519541 92 extern unsigned long bitmap[], bitmap_width, bitmap_size;
Michael J. Spencer 1:f5ac63519541 93
Michael J. Spencer 1:f5ac63519541 94
Michael J. Spencer 1:f5ac63519541 95 // __________________________
Michael J. Spencer 1:f5ac63519541 96 // /| |\ _________________ ^
Michael J. Spencer 1:f5ac63519541 97 // / | | \ /| |\ |
Michael J. Spencer 1:f5ac63519541 98 // / | | \ / | | \ s
Michael J. Spencer 1:f5ac63519541 99 // / | | | | | \ p
Michael J. Spencer 1:f5ac63519541 100 // / | | | | | \ e
Michael J. Spencer 1:f5ac63519541 101 // +-----+------------------------+---+--+---------------+----+ e
Michael J. Spencer 1:f5ac63519541 102 // | BLOCK 1 | BLOCK 2 | d
Michael J. Spencer 1:f5ac63519541 103 //
Michael J. Spencer 1:f5ac63519541 104 // time ----->
Michael J. Spencer 1:f5ac63519541 105 //
Michael J. Spencer 1:f5ac63519541 106 // The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
Michael J. Spencer 1:f5ac63519541 107 // during the first block->accelerate_until step_events_completed, then keeps going at constant speed until
Michael J. Spencer 1:f5ac63519541 108 // step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
Michael J. Spencer 1:f5ac63519541 109 // The slope of acceleration is always +/- block->rate_delta and is applied at a constant rate following the midpoint rule
Michael J. Spencer 1:f5ac63519541 110 // by the trapezoid generator, which is called ACCELERATION_TICKS_PER_SECOND times per second.
Michael J. Spencer 1:f5ac63519541 111
Michael J. Spencer 1:f5ac63519541 112
Michael J. Spencer 1:f5ac63519541 113
Michael J. Spencer 1:f5ac63519541 114 // Initialize and start the stepper motor subsystem
Michael J. Spencer 1:f5ac63519541 115 void st_init(void)
Michael J. Spencer 1:f5ac63519541 116 {
Michael J. Spencer 1:f5ac63519541 117 direction_inv =
Michael J. Spencer 1:f5ac63519541 118 (cfg->xscale<0 ? (1<<X_DIRECTION_BIT) : 0) |
Michael J. Spencer 1:f5ac63519541 119 (cfg->yscale<0 ? (1<<Y_DIRECTION_BIT) : 0) |
Michael J. Spencer 1:f5ac63519541 120 (cfg->zscale<0 ? (1<<Z_DIRECTION_BIT) : 0) |
Michael J. Spencer 1:f5ac63519541 121 (cfg->escale<0 ? (1<<E_DIRECTION_BIT) : 0);
Michael J. Spencer 1:f5ac63519541 122 step_inv =
Michael J. Spencer 1:f5ac63519541 123 (cfg->xinv ? (1<<X_STEP_BIT) : 0) |
Michael J. Spencer 1:f5ac63519541 124 (cfg->yinv ? (1<<Y_STEP_BIT) : 0) |
Michael J. Spencer 1:f5ac63519541 125 (cfg->zinv ? (1<<Z_STEP_BIT) : 0) |
Michael J. Spencer 1:f5ac63519541 126 (cfg->einv ? (1<<E_STEP_BIT) : 0);
Michael J. Spencer 1:f5ac63519541 127
Michael J. Spencer 1:f5ac63519541 128 printf("Direction: %d\n", direction_inv);
Michael J. Spencer 1:f5ac63519541 129 pwmofs = to_fixed(cfg->pwmmin) / 100; // offset (0 .. 1.0)
Michael J. Spencer 1:f5ac63519541 130 if ( cfg->pwmmin == cfg->pwmmax )
Michael J. Spencer 1:f5ac63519541 131 pwmscale = 0;
Michael J. Spencer 1:f5ac63519541 132 else
Michael J. Spencer 1:f5ac63519541 133 pwmscale = div_f(to_fixed(cfg->pwmmax - cfg->pwmmin), to_fixed(100) );
Michael J. Spencer 1:f5ac63519541 134 printf("ofs: %d, scale: %d\n", pwmofs, pwmscale);
Michael J. Spencer 1:f5ac63519541 135 actpos_x = actpos_y = actpos_z = actpos_e = 0;
Michael J. Spencer 1:f5ac63519541 136 st_wake_up();
Michael J. Spencer 1:f5ac63519541 137 trapezoid_tick_cycle_counter = 0;
Michael J. Spencer 1:f5ac63519541 138 st_go_idle(); // Start in the idle state
Michael J. Spencer 1:f5ac63519541 139 }
Michael J. Spencer 1:f5ac63519541 140
Michael J. Spencer 1:f5ac63519541 141 // output the direction bits to the appropriate output pins
Michael J. Spencer 1:f5ac63519541 142 static inline void set_direction_pins (void)
Michael J. Spencer 1:f5ac63519541 143 {
Michael J. Spencer 1:f5ac63519541 144 xdir = ( (direction_bits & (1<<X_DIRECTION_BIT))? 0 : 1 );
Michael J. Spencer 1:f5ac63519541 145 ydir = ( (direction_bits & (1<<Y_DIRECTION_BIT))? 0 : 1 );
Michael J. Spencer 1:f5ac63519541 146 zdir = ( (direction_bits & (1<<Z_DIRECTION_BIT))? 0 : 1 );
Michael J. Spencer 1:f5ac63519541 147 // edir = ( (direction_bits & (1<<E_DIRECTION_BIT))?0:1);
Michael J. Spencer 1:f5ac63519541 148 }
Michael J. Spencer 1:f5ac63519541 149
Michael J. Spencer 1:f5ac63519541 150 // output the step bits on the appropriate output pins
Michael J. Spencer 1:f5ac63519541 151 static inline void set_step_pins (uint32_t bits)
Michael J. Spencer 1:f5ac63519541 152 {
Michael J. Spencer 1:f5ac63519541 153 xstep = ( (bits & (1<<X_STEP_BIT))?1:0 );
Michael J. Spencer 1:f5ac63519541 154 ystep = ( (bits & (1<<Y_STEP_BIT))?1:0 );
Michael J. Spencer 1:f5ac63519541 155 zstep = ( (bits & (1<<Z_STEP_BIT))?1:0 );
Michael J. Spencer 1:f5ac63519541 156 // estep = ( (bits & (1<<E_STEP_BIT))?1:0 );
Michael J. Spencer 1:f5ac63519541 157 }
Michael J. Spencer 1:f5ac63519541 158
Michael J. Spencer 1:f5ac63519541 159 // unstep all stepper pins (output low)
Michael J. Spencer 1:f5ac63519541 160 static inline void clear_all_step_pins (void)
Michael J. Spencer 1:f5ac63519541 161 {
Michael J. Spencer 1:f5ac63519541 162
Michael J. Spencer 1:f5ac63519541 163 xstep =( (step_inv & (1<<X_STEP_BIT)) ? 1 : 0 );
Michael J. Spencer 1:f5ac63519541 164 ystep =( (step_inv & (1<<Y_STEP_BIT)) ? 1 : 0 );
Michael J. Spencer 1:f5ac63519541 165 zstep =( (step_inv & (1<<Z_STEP_BIT)) ? 1 : 0 );
Michael J. Spencer 1:f5ac63519541 166 // estep =( (step_inv & (1<<E_STEP_BIT)) ? 0 : 1 );
Michael J. Spencer 1:f5ac63519541 167 }
Michael J. Spencer 1:f5ac63519541 168
Michael J. Spencer 1:f5ac63519541 169
Michael J. Spencer 1:f5ac63519541 170 // check home sensor
Michael J. Spencer 1:f5ac63519541 171 int hit_home_stop_x(int axis)
Michael J. Spencer 1:f5ac63519541 172 {
Michael J. Spencer 1:f5ac63519541 173 return 1;
Michael J. Spencer 1:f5ac63519541 174 }
Michael J. Spencer 1:f5ac63519541 175 // check home sensor
Michael J. Spencer 1:f5ac63519541 176 int hit_home_stop_y(int axis)
Michael J. Spencer 1:f5ac63519541 177 {
Michael J. Spencer 1:f5ac63519541 178 return 1;
Michael J. Spencer 1:f5ac63519541 179 }
Michael J. Spencer 1:f5ac63519541 180 // check home sensor
Michael J. Spencer 1:f5ac63519541 181 int hit_home_stop_z(int axis)
Michael J. Spencer 1:f5ac63519541 182 {
Michael J. Spencer 1:f5ac63519541 183 return 1;
Michael J. Spencer 1:f5ac63519541 184 }
Michael J. Spencer 1:f5ac63519541 185
Michael J. Spencer 1:f5ac63519541 186 // Start stepper again from idle state, starts the step timer at a default rate
Michael J. Spencer 1:f5ac63519541 187 void st_wake_up()
Michael J. Spencer 1:f5ac63519541 188 {
Michael J. Spencer 1:f5ac63519541 189 if ( ! running )
Michael J. Spencer 1:f5ac63519541 190 {
Michael J. Spencer 1:f5ac63519541 191 running = 1;
Michael J. Spencer 1:f5ac63519541 192 set_step_timer(2000);
Michael J. Spencer 1:f5ac63519541 193 // printf("wake_up()..\n");
Michael J. Spencer 1:f5ac63519541 194 }
Michael J. Spencer 1:f5ac63519541 195 }
Michael J. Spencer 1:f5ac63519541 196
Michael J. Spencer 1:f5ac63519541 197 // When not stepping, go to idle mode. Steppers can be switched off, or set to reduced current
Michael J. Spencer 1:f5ac63519541 198 // (some delay might have to be implemented). Currently no motor switchoff is done.
Michael J. Spencer 1:f5ac63519541 199 static void st_go_idle()
Michael J. Spencer 1:f5ac63519541 200 {
Michael J. Spencer 1:f5ac63519541 201 timer.detach();
Michael J. Spencer 1:f5ac63519541 202 running = 0;
Michael J. Spencer 1:f5ac63519541 203 clear_all_step_pins();
Michael J. Spencer 1:f5ac63519541 204 *laser = LASEROFF;
Michael J. Spencer 1:f5ac63519541 205 pwm = cfg->pwmmax / 100.0; // set pwm to max;
Michael J. Spencer 1:f5ac63519541 206 // printf("idle()..\n");
Michael J. Spencer 1:f5ac63519541 207 }
Michael J. Spencer 1:f5ac63519541 208
Michael J. Spencer 1:f5ac63519541 209 // return number of steps to perform: n = (v^2) / (2*a)
Michael J. Spencer 1:f5ac63519541 210 // alpha is a adjustment factor for ?? (alsways 1! in this source)
Michael J. Spencer 1:f5ac63519541 211 static inline int32_t calc_n (float speed, float alpha, float accel)
Michael J. Spencer 1:f5ac63519541 212 {
Michael J. Spencer 1:f5ac63519541 213 return speed * speed / (2.0 * alpha * accel);
Michael J. Spencer 1:f5ac63519541 214 }
Michael J. Spencer 1:f5ac63519541 215
Michael J. Spencer 1:f5ac63519541 216 // Initializes the trapezoid generator from the current block. Called whenever a new
Michael J. Spencer 1:f5ac63519541 217 // block begins. Calculates the length ofc the block (in events), step rate, slopes and trigger positions (when to accel, decel, etc.)
Michael J. Spencer 1:f5ac63519541 218 static inline void trapezoid_generator_reset()
Michael J. Spencer 1:f5ac63519541 219 {
Michael J. Spencer 1:f5ac63519541 220 tFixedPt c0;
Michael J. Spencer 1:f5ac63519541 221 #define alpha (1.0)
Michael J. Spencer 1:f5ac63519541 222
Michael J. Spencer 1:f5ac63519541 223 // float alpha = 1.0;
Michael J. Spencer 1:f5ac63519541 224 float accel;
Michael J. Spencer 1:f5ac63519541 225 int32_t accel_until;
Michael J. Spencer 1:f5ac63519541 226 int32_t decel_after;
Michael J. Spencer 1:f5ac63519541 227
Michael J. Spencer 1:f5ac63519541 228 accel = current_block->rate_delta*ACCELERATION_TICKS_PER_SECOND / 60.0;
Michael J. Spencer 1:f5ac63519541 229
Michael J. Spencer 1:f5ac63519541 230 c0 = (float)STEP_TIMER_FREQ * sqrt (2.0*alpha/accel);
Michael J. Spencer 1:f5ac63519541 231 n = calc_n (current_block->initial_rate/60.0, alpha, accel);
Michael J. Spencer 1:f5ac63519541 232 if (n==0)
Michael J. Spencer 1:f5ac63519541 233 {
Michael J. Spencer 1:f5ac63519541 234 n = 1;
Michael J. Spencer 1:f5ac63519541 235 c = c0*0.676;
Michael J. Spencer 1:f5ac63519541 236 }
Michael J. Spencer 1:f5ac63519541 237 else
Michael J. Spencer 1:f5ac63519541 238 {
Michael J. Spencer 1:f5ac63519541 239 c = c0 * (sqrt(n+1.0)-sqrt((float)n));
Michael J. Spencer 1:f5ac63519541 240 }
Michael J. Spencer 1:f5ac63519541 241
Michael J. Spencer 1:f5ac63519541 242 ramp = RAMP_UP;
Michael J. Spencer 1:f5ac63519541 243
Michael J. Spencer 1:f5ac63519541 244 accel_until = calc_n (current_block->nominal_rate/60.0, alpha, accel);
Michael J. Spencer 1:f5ac63519541 245 c_min = c0 * (sqrt(accel_until+1.0)-sqrt((float)accel_until));
Michael J. Spencer 1:f5ac63519541 246 accel_until = accel_until - n;
Michael J. Spencer 1:f5ac63519541 247
Michael J. Spencer 1:f5ac63519541 248 decel_n = - calc_n (current_block->nominal_rate/60.0, alpha, accel);
Michael J. Spencer 1:f5ac63519541 249 decel_after = current_block->step_event_count + decel_n + calc_n (current_block->final_rate/60.0, alpha, accel);
Michael J. Spencer 1:f5ac63519541 250
Michael J. Spencer 1:f5ac63519541 251 if (decel_after < accel_until)
Michael J. Spencer 1:f5ac63519541 252 {
Michael J. Spencer 1:f5ac63519541 253 decel_after = (decel_after + accel_until) / 2;
Michael J. Spencer 1:f5ac63519541 254 decel_n = decel_after - current_block->step_event_count - calc_n (current_block->final_rate/60.0, alpha, accel);
Michael J. Spencer 1:f5ac63519541 255 }
Michael J. Spencer 1:f5ac63519541 256 current_block->decelerate_after = decel_after;
Michael J. Spencer 1:f5ac63519541 257
Michael J. Spencer 1:f5ac63519541 258 c = to_fixed(c);
Michael J. Spencer 1:f5ac63519541 259 c_min = to_fixed (c_min);
Michael J. Spencer 1:f5ac63519541 260 #undef alpha
Michael J. Spencer 1:f5ac63519541 261 }
Michael J. Spencer 1:f5ac63519541 262
Michael J. Spencer 1:f5ac63519541 263
Michael J. Spencer 1:f5ac63519541 264 // get step rate (steps/min) from time cycles
Michael J. Spencer 1:f5ac63519541 265 //static inline uint32_t get_step_rate (uint64_t cycles)
Michael J. Spencer 1:f5ac63519541 266 //{
Michael J. Spencer 1:f5ac63519541 267 // return (TICKS_PER_MICROSECOND*1000000*6) / cycles * 10;
Michael J. Spencer 1:f5ac63519541 268 //}
Michael J. Spencer 1:f5ac63519541 269
Michael J. Spencer 1:f5ac63519541 270 // Set the step timer. Note: this starts the ticker at an interval of "cycles"
Michael J. Spencer 1:f5ac63519541 271 static inline void set_step_timer (uint32_t cycles)
Michael J. Spencer 1:f5ac63519541 272 {
Michael J. Spencer 1:f5ac63519541 273 volatile static double p;
Michael J. Spencer 1:f5ac63519541 274 timer.attach_us(&st_interrupt,cycles);
Michael J. Spencer 1:f5ac63519541 275 // p = to_double(pwmofs + mul_f( pwmscale, ((power>>6) * c_min) / ((10000>>6)*cycles) ) );
Michael J. Spencer 1:f5ac63519541 276 // p = ( to_double(c_min) * current_block->power) / ( 10000.0 * (double)cycles);
Michael J. Spencer 1:f5ac63519541 277 // p = (60E6/nominal_rate) / cycles; // nom_rate is steps/minute,
Michael J. Spencer 1:f5ac63519541 278 //printf("%f,%f,%f\n\r", (float)(60E6/nominal_rate), (float)cycles, (float)p);
Michael J. Spencer 1:f5ac63519541 279 // printf("%d: %f %f\n\r", (int)current_block->power, (float)p, (float)c_min/(float(c) ));
Michael J. Spencer 1:f5ac63519541 280 p = (double)(cfg->pwmmin/100.0 + ((current_block->power/10000.0)*((cfg->pwmmax - cfg->pwmmin)/100.0)));
Michael J. Spencer 1:f5ac63519541 281 pwm = p;
Michael J. Spencer 1:f5ac63519541 282 }
Michael J. Spencer 1:f5ac63519541 283
Michael J. Spencer 1:f5ac63519541 284 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with
Michael J. Spencer 1:f5ac63519541 285 // set_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
Michael J. Spencer 1:f5ac63519541 286 // It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse.
Michael J. Spencer 1:f5ac63519541 287 // The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts.
Michael J. Spencer 1:f5ac63519541 288 static void st_interrupt (void)
Michael J. Spencer 1:f5ac63519541 289 {
Michael J. Spencer 1:f5ac63519541 290 // TODO: Check if the busy-flag can be eliminated by just disabeling this interrupt while we are in it
Michael J. Spencer 1:f5ac63519541 291
Michael J. Spencer 1:f5ac63519541 292 if(busy){ /*printf("busy!\n"); */ return; } // The busy-flag is used to avoid reentering this interrupt
Michael J. Spencer 1:f5ac63519541 293 busy = 1;
Michael J. Spencer 1:f5ac63519541 294
Michael J. Spencer 1:f5ac63519541 295 // Set the direction pins a cuple of nanoseconds before we step the steppers
Michael J. Spencer 1:f5ac63519541 296 //STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
Michael J. Spencer 1:f5ac63519541 297 // set_direction_pins (out_bits);
Michael J. Spencer 1:f5ac63519541 298
Michael J. Spencer 1:f5ac63519541 299 // Then pulse the stepping pins
Michael J. Spencer 1:f5ac63519541 300 //STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits;
Michael J. Spencer 1:f5ac63519541 301 // led2 = 1;
Michael J. Spencer 1:f5ac63519541 302 set_step_pins (step_bits ^ step_inv);
Michael J. Spencer 1:f5ac63519541 303
Michael J. Spencer 1:f5ac63519541 304 // If there is no current block, attempt to pop one from the buffer
Michael J. Spencer 1:f5ac63519541 305 if (current_block == NULL)
Michael J. Spencer 1:f5ac63519541 306 {
Michael J. Spencer 1:f5ac63519541 307 // Anything in the buffer?
Michael J. Spencer 1:f5ac63519541 308 current_block = plan_get_current_block();
Michael J. Spencer 1:f5ac63519541 309 if (current_block != NULL) {
Michael J. Spencer 1:f5ac63519541 310 trapezoid_generator_reset();
Michael J. Spencer 1:f5ac63519541 311 counter_x = -(current_block->step_event_count >> 1);
Michael J. Spencer 1:f5ac63519541 312 counter_y = counter_x;
Michael J. Spencer 1:f5ac63519541 313 counter_z = counter_x;
Michael J. Spencer 1:f5ac63519541 314 counter_e = counter_x;
Michael J. Spencer 1:f5ac63519541 315 counter_l = counter_x;
Michael J. Spencer 1:f5ac63519541 316 pos_l = 0; // reset laser bitmap counter
Michael J. Spencer 1:f5ac63519541 317 step_events_completed = 0;
Michael J. Spencer 1:f5ac63519541 318 direction_bits = current_block->direction_bits ^ direction_inv;
Michael J. Spencer 1:f5ac63519541 319 set_direction_pins ();
Michael J. Spencer 1:f5ac63519541 320 step_bits = 0;
Michael J. Spencer 1:f5ac63519541 321 }
Michael J. Spencer 1:f5ac63519541 322 else
Michael J. Spencer 1:f5ac63519541 323 {
Michael J. Spencer 1:f5ac63519541 324 st_go_idle();
Michael J. Spencer 1:f5ac63519541 325 }
Michael J. Spencer 1:f5ac63519541 326 }
Michael J. Spencer 1:f5ac63519541 327
Michael J. Spencer 1:f5ac63519541 328 // process the current block
Michael J. Spencer 1:f5ac63519541 329 if (current_block != NULL)
Michael J. Spencer 1:f5ac63519541 330 {
Michael J. Spencer 1:f5ac63519541 331
Michael J. Spencer 1:f5ac63519541 332 // this block is a bitmap engraving line, read laser on/off status from buffer
Michael J. Spencer 1:f5ac63519541 333 if ( current_block->options & OPT_BITMAP )
Michael J. Spencer 1:f5ac63519541 334 {
Michael J. Spencer 1:f5ac63519541 335 *laser = ! (bitmap[pos_l / 32] & (1 << (pos_l % 32)));
Michael J. Spencer 1:f5ac63519541 336 counter_l += bitmap_width;
Michael J. Spencer 1:f5ac63519541 337 // printf("%d %d %d: %d %d %c\n\r", bitmap_width, pos_l, counter_l, pos_l / 32, pos_l % 32, (*laser ? '1' : '0' ));
Michael J. Spencer 1:f5ac63519541 338 if (counter_l > 0)
Michael J. Spencer 1:f5ac63519541 339 {
Michael J. Spencer 1:f5ac63519541 340 counter_l -= current_block->step_event_count;
Michael J. Spencer 1:f5ac63519541 341 // putchar ( (*laser ? '1' : '0' ) );
Michael J. Spencer 1:f5ac63519541 342 pos_l++;
Michael J. Spencer 1:f5ac63519541 343 }
Michael J. Spencer 1:f5ac63519541 344 }
Michael J. Spencer 1:f5ac63519541 345 else
Michael J. Spencer 1:f5ac63519541 346 {
Michael J. Spencer 1:f5ac63519541 347 *laser = ( current_block->options & OPT_LASER_ON ? LASERON : LASEROFF);
Michael J. Spencer 1:f5ac63519541 348 }
Michael J. Spencer 1:f5ac63519541 349
Michael J. Spencer 1:f5ac63519541 350 if (current_block->action_type == AT_MOVE)
Michael J. Spencer 1:f5ac63519541 351 {
Michael J. Spencer 1:f5ac63519541 352 // Execute step displacement profile by bresenham line algorithm
Michael J. Spencer 1:f5ac63519541 353 step_bits = 0;
Michael J. Spencer 1:f5ac63519541 354 counter_x += current_block->steps_x;
Michael J. Spencer 1:f5ac63519541 355 if (counter_x > 0) {
Michael J. Spencer 1:f5ac63519541 356 actpos_x += ( (current_block->direction_bits & (1<<X_DIRECTION_BIT))? -1 : 1 );
Michael J. Spencer 1:f5ac63519541 357 step_bits |= (1<<X_STEP_BIT);
Michael J. Spencer 1:f5ac63519541 358 counter_x -= current_block->step_event_count;
Michael J. Spencer 1:f5ac63519541 359 }
Michael J. Spencer 1:f5ac63519541 360 counter_y += current_block->steps_y;
Michael J. Spencer 1:f5ac63519541 361 if (counter_y > 0) {
Michael J. Spencer 1:f5ac63519541 362 actpos_y += ( (current_block->direction_bits & (1<<Y_DIRECTION_BIT))? -1 : 1 );
Michael J. Spencer 1:f5ac63519541 363 step_bits |= (1<<Y_STEP_BIT);
Michael J. Spencer 1:f5ac63519541 364 counter_y -= current_block->step_event_count;
Michael J. Spencer 1:f5ac63519541 365 }
Michael J. Spencer 1:f5ac63519541 366 counter_z += current_block->steps_z;
Michael J. Spencer 1:f5ac63519541 367 if (counter_z > 0) {
Michael J. Spencer 1:f5ac63519541 368 actpos_z += ( (current_block->direction_bits & (1<<Z_DIRECTION_BIT))? -1 : 1 );
Michael J. Spencer 1:f5ac63519541 369 step_bits |= (1<<Z_STEP_BIT);
Michael J. Spencer 1:f5ac63519541 370 counter_z -= current_block->step_event_count;
Michael J. Spencer 1:f5ac63519541 371 }
Michael J. Spencer 1:f5ac63519541 372
Michael J. Spencer 1:f5ac63519541 373 counter_e += current_block->steps_e;
Michael J. Spencer 1:f5ac63519541 374 if (counter_e > 0) {
Michael J. Spencer 1:f5ac63519541 375 actpos_e += ( (current_block->direction_bits & (1<<E_DIRECTION_BIT))? -1 : 1 );
Michael J. Spencer 1:f5ac63519541 376 step_bits |= (1<<E_STEP_BIT);
Michael J. Spencer 1:f5ac63519541 377 counter_e -= current_block->step_event_count;
Michael J. Spencer 1:f5ac63519541 378 }
Michael J. Spencer 1:f5ac63519541 379
Michael J. Spencer 1:f5ac63519541 380 //clear_step_pins (); // clear the pins, assume that we spend enough CPU cycles in the previous statements for the steppers to react (>1usec)
Michael J. Spencer 1:f5ac63519541 381 step_events_completed++; // Iterate step events
Michael J. Spencer 1:f5ac63519541 382
Michael J. Spencer 1:f5ac63519541 383 // This is a homing block, keep moving until all end-stops are triggered
Michael J. Spencer 1:f5ac63519541 384 if (current_block->check_endstops)
Michael J. Spencer 1:f5ac63519541 385 {
Michael J. Spencer 1:f5ac63519541 386 if ( (current_block->steps_x && hit_home_stop_x (direction_bits & (1<<X_DIRECTION_BIT)) ) ||
Michael J. Spencer 1:f5ac63519541 387 (current_block->steps_y && hit_home_stop_y (direction_bits & (1<<Y_DIRECTION_BIT)) ) ||
Michael J. Spencer 1:f5ac63519541 388 (current_block->steps_z && hit_home_stop_z (direction_bits & (1<<Z_DIRECTION_BIT)) )
Michael J. Spencer 1:f5ac63519541 389 )
Michael J. Spencer 1:f5ac63519541 390 {
Michael J. Spencer 1:f5ac63519541 391 step_events_completed = current_block->step_event_count;
Michael J. Spencer 1:f5ac63519541 392 step_bits = 0;
Michael J. Spencer 1:f5ac63519541 393 }
Michael J. Spencer 1:f5ac63519541 394 }
Michael J. Spencer 1:f5ac63519541 395
Michael J. Spencer 1:f5ac63519541 396
Michael J. Spencer 1:f5ac63519541 397 // While in block steps, update acceleration profile
Michael J. Spencer 1:f5ac63519541 398 if (step_events_completed < current_block->step_event_count)
Michael J. Spencer 1:f5ac63519541 399 {
Michael J. Spencer 1:f5ac63519541 400 tFixedPt new_c;
Michael J. Spencer 1:f5ac63519541 401
Michael J. Spencer 1:f5ac63519541 402 switch (ramp)
Michael J. Spencer 1:f5ac63519541 403 {
Michael J. Spencer 1:f5ac63519541 404 case RAMP_UP:
Michael J. Spencer 1:f5ac63519541 405 {
Michael J. Spencer 1:f5ac63519541 406 new_c = c - (c<<1) / (4*n+1);
Michael J. Spencer 1:f5ac63519541 407 if (step_events_completed >= current_block->decelerate_after)
Michael J. Spencer 1:f5ac63519541 408 {
Michael J. Spencer 1:f5ac63519541 409 ramp = RAMP_DOWN;
Michael J. Spencer 1:f5ac63519541 410 n = decel_n;
Michael J. Spencer 1:f5ac63519541 411 }
Michael J. Spencer 1:f5ac63519541 412 else if (new_c <= c_min)
Michael J. Spencer 1:f5ac63519541 413 {
Michael J. Spencer 1:f5ac63519541 414 new_c = c_min;
Michael J. Spencer 1:f5ac63519541 415 ramp = RAMP_MAX;
Michael J. Spencer 1:f5ac63519541 416 }
Michael J. Spencer 1:f5ac63519541 417
Michael J. Spencer 1:f5ac63519541 418 if (to_int(new_c) != to_int(c))
Michael J. Spencer 1:f5ac63519541 419 {
Michael J. Spencer 1:f5ac63519541 420 set_step_timer (to_int(new_c));
Michael J. Spencer 1:f5ac63519541 421 }
Michael J. Spencer 1:f5ac63519541 422 c = new_c;
Michael J. Spencer 1:f5ac63519541 423 }
Michael J. Spencer 1:f5ac63519541 424 break;
Michael J. Spencer 1:f5ac63519541 425
Michael J. Spencer 1:f5ac63519541 426 case RAMP_MAX:
Michael J. Spencer 1:f5ac63519541 427 if (step_events_completed >= current_block->decelerate_after)
Michael J. Spencer 1:f5ac63519541 428 {
Michael J. Spencer 1:f5ac63519541 429 ramp = RAMP_DOWN;
Michael J. Spencer 1:f5ac63519541 430 n = decel_n;
Michael J. Spencer 1:f5ac63519541 431 }
Michael J. Spencer 1:f5ac63519541 432 break;
Michael J. Spencer 1:f5ac63519541 433
Michael J. Spencer 1:f5ac63519541 434 case RAMP_DOWN:
Michael J. Spencer 1:f5ac63519541 435 new_c = c - (c<<1) / (4*n+1);
Michael J. Spencer 1:f5ac63519541 436 if (to_int(new_c) != to_int(c))
Michael J. Spencer 1:f5ac63519541 437 {
Michael J. Spencer 1:f5ac63519541 438 set_step_timer (to_int(c));
Michael J. Spencer 1:f5ac63519541 439 }
Michael J. Spencer 1:f5ac63519541 440 c = new_c;
Michael J. Spencer 1:f5ac63519541 441 break;
Michael J. Spencer 1:f5ac63519541 442 }
Michael J. Spencer 1:f5ac63519541 443
Michael J. Spencer 1:f5ac63519541 444 n++;
Michael J. Spencer 1:f5ac63519541 445 } else {
Michael J. Spencer 1:f5ac63519541 446 // If current block is finished, reset pointer
Michael J. Spencer 1:f5ac63519541 447 current_block = NULL;
Michael J. Spencer 1:f5ac63519541 448 plan_discard_current_block();
Michael J. Spencer 1:f5ac63519541 449 }
Michael J. Spencer 1:f5ac63519541 450 }
Michael J. Spencer 1:f5ac63519541 451 }
Michael J. Spencer 1:f5ac63519541 452 else
Michael J. Spencer 1:f5ac63519541 453 {
Michael J. Spencer 1:f5ac63519541 454 // Still no block? Set the stepper pins to low before sleeping.
Michael J. Spencer 1:f5ac63519541 455 // printf("block == NULL\n");
Michael J. Spencer 1:f5ac63519541 456 step_bits = 0;
Michael J. Spencer 1:f5ac63519541 457 }
Michael J. Spencer 1:f5ac63519541 458
Michael J. Spencer 1:f5ac63519541 459 clear_all_step_pins (); // clear the pins, assume that we spend enough CPU cycles in the previous statements for the steppers to react (>1usec)
Michael J. Spencer 1:f5ac63519541 460 busy=0;
Michael J. Spencer 1:f5ac63519541 461
Michael J. Spencer 1:f5ac63519541 462 }
Michael J. Spencer 1:f5ac63519541 463
Michael J. Spencer 1:f5ac63519541 464
Michael J. Spencer 1:f5ac63519541 465 // Block until all buffered steps are executed
Michael J. Spencer 1:f5ac63519541 466 void st_synchronize()
Michael J. Spencer 1:f5ac63519541 467 {
Michael J. Spencer 1:f5ac63519541 468 while(plan_get_current_block()) { sleep_mode(); }
Michael J. Spencer 1:f5ac63519541 469 }
Michael J. Spencer 1:f5ac63519541 470