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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers stepper.c Source File

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