This is the firmware for the LaOS - Laser Open Source project. You can use it to drive a laser cutter. For hardware and more information, look at our wiki: http://wiki.laoslaser.org
Dependencies: EthernetNetIf mbed
stepper.c
00001 /* 00002 stepper.c - stepper motor driver: executes motion plans using stepper motors 00003 Part of Grbl 00004 Taken from R2C2 project and modified for Laos by Peter Brier 00005 stripped, included some Marlin changes from Erik van de Zalm 00006 00007 Copyright (c) 2009-2011 Simen Svale Skogsrud 00008 Modifications Copyright (c) 2011 Sungeun K. Jeon 00009 Modifications Copyright (c) 2011 Peter Brier 00010 00011 Grbl is free software: you can redistribute it and/or modify 00012 it under the terms of the GNU General Public License as published by 00013 the Free Software Foundation, either version 3 of the License, orc 00014 (at your option) any later version. 00015 00016 Grbl is distributed in the hope that it will be useful, 00017 but WITHOUT ANY WARRANTY; without even the implied warrlaanty of 00018 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 GNU General Public License for more details. 00020 00021 You should have received a copy of the GNU General Public License 00022 along with Grbl. If not, see <http://www.gnu.org/licenses/>. 00023 */ 00024 00025 /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith 00026 and Philipp Tiefenbacher. */ 00027 00028 /* The acceleration profiles are derived using the techniques descibed in the paper 00029 "Generate stepper-motor speed profiles in real time" David Austin 2004 00030 published at http://www.eetimes.com/design/embedded/4006438/Generate-stepper-motor-speed-profiles-in-real-time and others. 00031 */ 00032 #include <LaosMotion.h> 00033 #include "pins.h" 00034 00035 #include <math.h> 00036 #include <stdlib.h> 00037 #include <string.h> 00038 00039 #include "fixedpt.h" 00040 #include "stepper.h" 00041 #include "config.h" 00042 #include "planner.h" 00043 00044 00045 #define TICKS_PER_MICROSECOND (1) // Ticker uses 1usec units 00046 // #define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND) 00047 #define STEP_TIMER_FREQ 1000000 // 1 MHz 00048 00049 // types: ramp state 00050 typedef enum {RAMP_UP, RAMP_MAX, RAMP_DOWN} tRamp; 00051 00052 // Prototypes 00053 static void st_interrupt (); 00054 static void set_step_timer (uint32_t cycles); 00055 static void st_go_idle(); 00056 00057 // Globals 00058 // volatile uint16_t steptimeout = 0; 00059 volatile unsigned char busy = 0; 00060 volatile double p=0; 00061 00062 // Locals 00063 static block_t *current_block; // A pointer to the block currently being traced 00064 static Ticker timer; // the periodic timer used to step 00065 static tFixedPt pwmofs; // the offset of the PWM value 00066 static tFixedPt pwmscale; // the scaling of the PWM value 00067 static volatile int running = 0; // stepper irq is running 00068 00069 static uint32_t direction_inv; // invert mask for direction bits 00070 static uint32_t direction_bits; // all axes direction (different ports) 00071 static uint32_t step_bits; // all axis step bits 00072 static uint32_t step_inv; // invert mask for the stepper bits 00073 00074 static int32_t counter_x, // Counter variables for the bresenham line tracer 00075 counter_y, 00076 counter_z; 00077 static int32_t counter_e, counter_l, pos_l; // extruder and laser 00078 static uint32_t step_events_completed; // The number of step events executed in the current block 00079 00080 // Variables used by the trapezoid generation 00081 static uint32_t cycles_per_step_event; // The number of machine cycles between each step event 00082 static uint32_t trapezoid_tick_cycle_counter; // The cycles since last trapezoid_tick. Used to generate ticks at a steady 00083 // pace without allocating a separate timer 00084 static uint32_t trapezoid_adjusted_rate; // The current rate of step_events according to the trapezoid generator 00085 00086 static tFixedPt c; // current clock cycle count [1/speed] 00087 static int32_t c_min; // minimal clock cycle count [at vnominal for this block] 00088 static int32_t n; 00089 static int32_t decel_n; 00090 static tRamp ramp; // state of state machine for ramping up/down 00091 static int32_t power; // power [0..10000] 00092 00093 extern unsigned char bitmap_bpp; 00094 extern unsigned long bitmap[], bitmap_width, bitmap_size; 00095 00096 00097 // __________________________ 00098 // /| |\ _________________ ^ 00099 // / | | \ /| |\ | 00100 // / | | \ / | | \ s 00101 // / | | | | | \ p 00102 // / | | | | | \ e 00103 // +-----+------------------------+---+--+---------------+----+ e 00104 // | BLOCK 1 | BLOCK 2 | d 00105 // 00106 // time -----> 00107 // 00108 // The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta 00109 // during the first block->accelerate_until step_events_completed, then keeps going at constant speed until 00110 // step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset. 00111 // The slope of acceleration is always +/- block->rate_delta and is applied at a constant rate following the midpoint rule 00112 // by the trapezoid generator, which is called ACCELERATION_TICKS_PER_SECOND times per second. 00113 00114 00115 00116 // Initialize and start the stepper motor subsystem 00117 void st_init(void) 00118 { 00119 direction_inv = 00120 (cfg->xscale<0 ? (1<<X_DIRECTION_BIT) : 0) | 00121 (cfg->yscale<0 ? (1<<Y_DIRECTION_BIT) : 0) | 00122 (cfg->zscale<0 ? (1<<Z_DIRECTION_BIT) : 0) | 00123 (cfg->escale<0 ? (1<<E_DIRECTION_BIT) : 0); 00124 step_inv = 00125 (cfg->xinv ? (1<<X_STEP_BIT) : 0) | 00126 (cfg->yinv ? (1<<Y_STEP_BIT) : 0) | 00127 (cfg->zinv ? (1<<Z_STEP_BIT) : 0) | 00128 (cfg->einv ? (1<<E_STEP_BIT) : 0); 00129 00130 00131 printf("Direction: %d\n", direction_inv); 00132 pwmofs = to_fixed(cfg->pwmmin) / 100; // offset (0 .. 1.0) 00133 if ( cfg->pwmmin == cfg->pwmmax ) 00134 pwmscale = 0; 00135 else 00136 pwmscale = div_f(to_fixed(cfg->pwmmax - cfg->pwmmin), to_fixed(100) ); 00137 printf("ofs: %d, scale: %d\n", pwmofs, pwmscale); 00138 st_wake_up(); 00139 trapezoid_tick_cycle_counter = 0; 00140 st_go_idle(); // Start in the idle state 00141 } 00142 00143 // output the direction bits to the appropriate output pins 00144 static inline void set_direction_pins (void) 00145 { 00146 xdir = ( (direction_bits & (1<<X_DIRECTION_BIT))? 0 : 1 ); 00147 ydir = ( (direction_bits & (1<<Y_DIRECTION_BIT))? 0 : 1 ); 00148 // zdir = ( (direction_bits & (1<<Z_DIRECTION_BIT))?0:1); 00149 // edir = ( (direction_bits & (1<<E_DIRECTION_BIT))?0:1); 00150 } 00151 00152 // output the step bits on the appropriate output pins 00153 static inline void set_step_pins (uint32_t bits) 00154 { 00155 xstep = ( (bits & (1<<X_STEP_BIT))?1:0 ); 00156 ystep = ( (bits & (1<<Y_STEP_BIT))?1:0 ); 00157 // zstep = ( (bits & (1<<Z_STEP_BIT))?1:0 ); 00158 // estep = ( (bits & (1<<E_STEP_BIT))?1:0 ); 00159 } 00160 00161 // unstep all stepper pins (output low) 00162 static inline void clear_all_step_pins (void) 00163 { 00164 00165 xstep =( (step_inv & (1<<X_STEP_BIT)) ? 1 : 0 ); 00166 ystep =( (step_inv & (1<<Y_STEP_BIT)) ? 1 : 0 ); 00167 // zstep =( (step_inv & (1<<Z_STEP_BIT)) ? 0 : 1 ); 00168 // estep =( (step_inv & (1<<E_STEP_BIT)) ? 0 : 1 ); 00169 } 00170 00171 00172 // check home sensor 00173 int hit_home_stop_x(int axis) 00174 { 00175 return 1; 00176 } 00177 // check home sensor 00178 int hit_home_stop_y(int axis) 00179 { 00180 return 1; 00181 } 00182 // check home sensor 00183 int hit_home_stop_z(int axis) 00184 { 00185 return 1; 00186 } 00187 00188 // Start stepper again from idle state, starts the step timer at a default rate 00189 void st_wake_up() 00190 { 00191 if ( ! running ) 00192 { 00193 running = 1; 00194 set_step_timer(2000); 00195 //printf("wake_up()..\n"); 00196 } 00197 } 00198 00199 // When not stepping, go to idle mode. Steppers can be switched off, or set to reduced current 00200 // (some delay might have to be implemented). Currently no motor switchoff is done. 00201 static void st_go_idle() 00202 { 00203 timer.detach(); 00204 running = 0; 00205 clear_all_step_pins(); 00206 laser = LASEROFF; 00207 pwm = cfg->pwmmax / 100.0; // set pwm to max; 00208 //printf("idle()..\n"); 00209 } 00210 00211 // return number of steps to perform: n = (v^2) / (2*a) 00212 // alpha is a adjustment factor for ?? (alsways 1! in this source) 00213 static inline int32_t calc_n (float speed, float alpha, float accel) 00214 { 00215 return speed * speed / (2.0 * alpha * accel); 00216 } 00217 00218 // Initializes the trapezoid generator from the current block. Called whenever a new 00219 // block begins. Calculates the length ofc the block (in events), step rate, slopes and trigger positions (when to accel, decel, etc.) 00220 static inline void trapezoid_generator_reset() 00221 { 00222 tFixedPt c0; 00223 #define alpha (1.0) 00224 00225 // float alpha = 1.0; 00226 float accel; 00227 int32_t accel_until; 00228 int32_t decel_after; 00229 00230 accel = current_block->rate_delta*ACCELERATION_TICKS_PER_SECOND / 60.0; 00231 00232 c0 = (float)STEP_TIMER_FREQ * sqrt (2.0*alpha/accel); 00233 n = calc_n (current_block->initial_rate/60.0, alpha, accel); 00234 if (n==0) 00235 { 00236 n = 1; 00237 c = c0*0.676; 00238 } 00239 else 00240 { 00241 c = c0 * (sqrt(n+1.0)-sqrt((float)n)); 00242 } 00243 00244 ramp = RAMP_UP; 00245 00246 accel_until = calc_n (current_block->nominal_rate/60.0, alpha, accel); 00247 c_min = c0 * (sqrt(accel_until+1.0)-sqrt((float)accel_until)); 00248 accel_until = accel_until - n; 00249 00250 decel_n = - calc_n (current_block->nominal_rate/60.0, alpha, accel); 00251 decel_after = current_block->step_event_count + decel_n + calc_n (current_block->final_rate/60.0, alpha, accel); 00252 00253 if (decel_after < accel_until) 00254 { 00255 decel_after = (decel_after + accel_until) / 2; 00256 decel_n = decel_after - current_block->step_event_count - calc_n (current_block->final_rate/60.0, alpha, accel); 00257 } 00258 current_block->decelerate_after = decel_after; 00259 00260 c = to_fixed(c); 00261 c_min = to_fixed (c_min); 00262 #undef alpha 00263 } 00264 00265 00266 // get step rate (steps/min) from time cycles 00267 //static inline uint32_t get_step_rate (uint64_t cycles) 00268 //{ 00269 // return (TICKS_PER_MICROSECOND*1000000*6) / cycles * 10; 00270 //} 00271 00272 // Set the step timer. Note: this starts the ticker at an interval of "cycles" 00273 static inline void set_step_timer (uint32_t cycles) 00274 { 00275 timer.attach_us(&st_interrupt,cycles); 00276 p = to_double(pwmofs + mul_f( pwmscale, ((power>>6) * c_min) / ((10000>>6)*cycles) ) ); 00277 // printf("%f\n\r", (float)p); 00278 pwm = p; 00279 } 00280 00281 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with 00282 // set_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. 00283 // It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. 00284 // The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts. 00285 static void st_interrupt (void) 00286 { 00287 // TODO: Check if the busy-flag can be eliminated by just disabeling this interrupt while we are in it 00288 00289 if(busy){ /*printf("busy!\n"); */ return; } // The busy-flag is used to avoid reentering this interrupt 00290 busy = 1; 00291 00292 // Set the direction pins a cuple of nanoseconds before we step the steppers 00293 //STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK); 00294 // set_direction_pins (out_bits); 00295 00296 // Then pulse the stepping pins 00297 //STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits; 00298 // led2 = 1; 00299 set_step_pins (step_bits ^ step_inv); 00300 00301 // If there is no current block, attempt to pop one from the buffer 00302 if (current_block == NULL) 00303 { 00304 // Anything in the buffer? 00305 current_block = plan_get_current_block(); 00306 if (current_block != NULL) { 00307 trapezoid_generator_reset(); 00308 counter_x = -(current_block->step_event_count >> 1); 00309 counter_y = counter_x; 00310 counter_z = counter_x; 00311 counter_e = counter_x; 00312 counter_l = counter_x; 00313 pos_l = 0; 00314 step_events_completed = 0; 00315 direction_bits = current_block->direction_bits ^ direction_inv; 00316 set_direction_pins (); 00317 step_bits = 0; 00318 power = current_block->power; 00319 } 00320 else 00321 { 00322 st_go_idle(); 00323 } 00324 } 00325 00326 // process the current block 00327 if (current_block != NULL) 00328 { 00329 if ( current_block->options & OPT_BITMAP ) 00330 { 00331 laser = ! (bitmap[pos_l / 32] & (1 << (pos_l % 32))); 00332 counter_l += bitmap_width; 00333 // printf("%d %d %d: %d\n\r", bitmap_len, pos_l, counter_l, (bitmap[pos_l / 32] & (pos_l % 32) ? 1 : 0 ) ); 00334 if (counter_l > 0) 00335 { 00336 counter_l -= current_block->step_event_count; 00337 // putchar ( (laser ? '1' : '0' ) ); 00338 pos_l++; 00339 } 00340 } 00341 else 00342 { 00343 laser = ( current_block->options & OPT_LASER_ON ? LASERON : LASEROFF); 00344 } 00345 00346 if (current_block->action_type == AT_MOVE) 00347 { 00348 // Execute step displacement profile by bresenham line algorithm 00349 step_bits = 0; 00350 counter_x += current_block->steps_x; 00351 if (counter_x > 0) { 00352 step_bits |= (1<<X_STEP_BIT); 00353 counter_x -= current_block->step_event_count; 00354 } 00355 counter_y += current_block->steps_y; 00356 if (counter_y > 0) { 00357 step_bits |= (1<<Y_STEP_BIT); 00358 counter_y -= current_block->step_event_count; 00359 } 00360 counter_z += current_block->steps_z; 00361 if (counter_z > 0) { 00362 step_bits |= (1<<Z_STEP_BIT); 00363 counter_z -= current_block->step_event_count; 00364 } 00365 00366 counter_e += current_block->steps_e; 00367 if (counter_e > 0) { 00368 step_bits |= (1<<E_STEP_BIT); 00369 counter_e -= current_block->step_event_count; 00370 } 00371 00372 00373 00374 00375 //clear_step_pins (); // clear the pins, assume that we spend enough CPU cycles in the previous statements for the steppers to react (>1usec) 00376 step_events_completed++; // Iterate step events 00377 00378 // This is a homing block, keep moving until all end-stops are triggered 00379 if (current_block->check_endstops) 00380 { 00381 if ( (current_block->steps_x && hit_home_stop_x (direction_bits & (1<<X_DIRECTION_BIT)) ) || 00382 (current_block->steps_y && hit_home_stop_y (direction_bits & (1<<Y_DIRECTION_BIT)) ) || 00383 (current_block->steps_z && hit_home_stop_z (direction_bits & (1<<Z_DIRECTION_BIT)) ) 00384 ) 00385 { 00386 step_events_completed = current_block->step_event_count; 00387 step_bits = 0; 00388 } 00389 } 00390 00391 00392 // While in block steps, update acceleration profile 00393 if (step_events_completed < current_block->step_event_count) 00394 { 00395 tFixedPt new_c; 00396 00397 switch (ramp) 00398 { 00399 case RAMP_UP: 00400 { 00401 new_c = c - (c<<1) / (4*n+1); 00402 if (step_events_completed >= current_block->decelerate_after) 00403 { 00404 ramp = RAMP_DOWN; 00405 n = decel_n; 00406 } 00407 else if (new_c <= c_min) 00408 { 00409 new_c = c_min; 00410 ramp = RAMP_MAX; 00411 } 00412 00413 if (to_int(new_c) != to_int(c)) 00414 { 00415 set_step_timer (to_int(new_c)); 00416 } 00417 c = new_c; 00418 } 00419 break; 00420 00421 case RAMP_MAX: 00422 if (step_events_completed >= current_block->decelerate_after) 00423 { 00424 ramp = RAMP_DOWN; 00425 n = decel_n; 00426 } 00427 break; 00428 00429 case RAMP_DOWN: 00430 new_c = c - (c<<1) / (4*n+1); 00431 if (to_int(new_c) != to_int(c)) 00432 { 00433 set_step_timer (to_int(c)); 00434 } 00435 c = new_c; 00436 break; 00437 } 00438 00439 n++; 00440 } else { 00441 // If current block is finished, reset pointer 00442 current_block = NULL; 00443 plan_discard_current_block(); 00444 } 00445 } 00446 } 00447 else 00448 { 00449 // Still no block? Set the stepper pins to low before sleeping. 00450 // printf("block == NULL\n"); 00451 step_bits = 0; 00452 } 00453 00454 clear_all_step_pins (); // clear the pins, assume that we spend enough CPU cycles in the previous statements for the steppers to react (>1usec) 00455 busy=0; 00456 00457 } 00458 00459 00460 // Block until all buffered steps are executed 00461 void st_synchronize() 00462 { 00463 while(plan_get_current_block()) { sleep_mode(); } 00464 } 00465 00466
Generated on Tue Jul 12 2022 21:03:04 by 1.7.2