interesting article on fast, real-time calculation of acceleration profiles for stepper motors November 09, 2009 01:27AM |
Registered: 16 years ago Posts: 1,094 |
Re: interesting article on fast, real-time calculation of acceleration profiles for stepper motors November 09, 2009 02:25PM |
Registered: 14 years ago Posts: 38 |
Re: interesting article on fast, real-time calculation of acceleration profiles for stepper motors November 09, 2009 02:38PM |
Re: interesting article on fast, real-time calculation of acceleration profiles for stepper motors November 10, 2009 04:13AM |
Registered: 16 years ago Posts: 467 |
ogre
Re: interesting article on fast, real-time calculation of acceleration profiles for stepper motors November 10, 2009 11:41AM |
// Dynamic ramping of a stepper motor // // // This code was mostly stolen from David Austin's article on dynamic // ramp tables (http://www.embedded.com/56800129), with some modifications // // this test code drives a contoller with step/dir input // // pin function direction // --- -------- --------- // PA0 DIR output DIR Signal // PA1 STEP output STEP Signal // // // // ATmega16, 16 MHz quarz #include < util/delay.h> #define STEP_PIN 25 #define DIR_PIN 24 // C0 = first step (must fit in unsigned int) // C_MIN is max speed timer value using 1 MHz timer // related to (1024 * 1024 * 60 / MAX_RPM / STEPS_PER_REV) (see Austins paper) #define C0 40000 #define C_MIN 2000 #if C_MIN >= C0 #error "C_MIN must be less than C0" #endif // ramp state-machine states #define RAMP_IDLE 0 #define RAMP_UP 1 #define RAMP_MAX 2 #define RAMP_DOWN 3 #define RAMP_LAST 4 volatile char ramp_status; // current ramp status volatile long motor_pos; // absolute current position (signed) volatile char run_flag; // true (non-zero) while motor is running volatile unsigned long c32; // 24.8 fixed point delay count volatile unsigned long step_no; // progress of move volatile unsigned int c; // timer delay count volatile unsigned long step_down; // start of ramp-down volatile long denom; // 4 * n + 1 in ramp algo (signed) unsigned long nstep; // total steps to move unsigned long midpoint; // midpoint of current move int pos_inc; // motor_pos increment (1 or -1, signed) void do_wait(void) { unsigned char i; for(i=1;i<=20;i++) _delay_ms(20); } void setup() { //Serial.begin(19200); //Serial.println("Started"); pinMode(STEP_PIN, OUTPUT); pinMode(DIR_PIN, OUTPUT); m_init(); } // // initialization // void m_init(void) { run_flag = 0; // we're idle ramp_status = RAMP_IDLE; // (this proves it) motor_pos = 0; // absolute motor position // timer1 initialization - prescale = 8, normal, 1MHz // use TCCR1B = 0x02; to start timer with prescale = 8 TCCR1B = 0x00; // stop timer1 TCNT1H = 0x00; // timer value TCNT1L = 0x00; OCR1AH = 0x00; // match A value OCR1AL = 0x01; TCCR1A = 0x00; // normal mode MCUCR = 0x00; GICR = 0x00; TIMSK = 0x10; // timer1, output compare A interrupts sei(); } // ------------------------------------------------------- // motor_run() - move motor to new absolute position // // this runs the motor to a new position. the current position is // maintained in the variable "motor_pos" with positive values being // one direction of rotation and negative values being the other (actual // directions depend on the coil polarity - the way you've wired it). // // we set everything up, then start the timer with a 1 ms delay // so all the stepping can be done within the interrupt handler // void move_to(long pos_new) { while (run_flag) ; // paranoid if (pos_new < motor_pos) { // get direction & step count nstep = motor_pos - pos_new; // number of steps is always positive pos_inc = -1; // decrement the position digitalWrite(DIR_PIN, LOW); // set DIR - } else if (pos_new != motor_pos) { nstep = pos_new - motor_pos; // positive number of steps to make pos_inc = 1; // increment the positioin digitalWrite(DIR_PIN, HIGH); // set DIR + } else return; // already there midpoint = (nstep + 1) >> 1; // midpoint, rounded up c = C0; // we're stopped, so this will be first move c32 = ((unsigned long) c) << 8; // c as 24.8 fixed-point fmt for ramp calcs step_no = 0; // step counter within this move denom = 1; // 4 * n + 1, where n = 0 if (nstep > 1) { // single step is special case ramp_status = RAMP_UP; // normal - start ramp state-machine } else { ramp_status = RAMP_DOWN; // shut it down after one step } run_flag = 1; // we're busy... TCCR1B = 0x00; // stop timer1 (should be stopped) TCNT1H = 0x00; // reset tic count TCNT1L = 0x00; OCR1AH = 0x04; // start about a millisecond OCR1AL = 0x00; // after we enable the timer TCCR1B = 0x02; // start timer1, prescale = 8 } // ------------------------------------------------------- // timer1 compare match a interrupt handler // // we come here to start things rolling, and then at the end of each step. // the next timer value will already be in the variable c, and denom has the // previously used denominator for the calculation. // ISR(TIMER1_COMPA_vect) { TCCR1B = 0x00; // stop the timer // we can't get here if we're RAMP_IDLE, so we'll be paranoid. if we're RAMP_LAST, // eveything is done except marking the end of the move (in run_flag) and calling // current_off(). the last step has just just completed. if (ramp_status == RAMP_IDLE) return; if (ramp_status != RAMP_LAST) // if we're not on cleanup duty, { motor_pos += pos_inc; // update the absolute position step_no++; // cur postion within sequence [0,nstep] do_step(STEP_PIN); TCNT1H = 0x00; // reset tic count to zero TCNT1L = 0x00; OCR1AH = c >> 8; // the next match count OCR1AL = c & 0x0ff; TCCR1B = 0x02; // start timer1, prescale = 8 } switch (ramp_status) { case RAMP_UP: // we are accelerating if (step_no >= midpoint) // midpoint: decel { ramp_status = RAMP_DOWN; denom = ((step_no - nstep) << 2) - 3; if (!(nstep & 1)) // even # of moves: repeat last step { denom += 4; break; } } // no break: share code for ramp algo case RAMP_DOWN: // we are decelerating if (step_no >= nstep) // just made last step, next is cleanup { ramp_status = RAMP_LAST; break; } // calculate the step value for the next step denom += 4; c32 -= ((long) (c32 << 1)) / denom; // ramp algorithm c = (c32 + 128) >> 8; // round 24.8 format -> int16 if (c <= C_MIN) // go to constant speed? { ramp_status = RAMP_MAX; // yup step_down = nstep - step_no - 1; c = C_MIN; // next step is this c32 = ((long) c) << 8; // and put it in the 24.8 format break; } break; case RAMP_MAX: // we're holding a constant speed if (step_no == step_down) // are we done yet? { ramp_status = RAMP_DOWN; // yes - start decelerating next time denom = ((step_no - nstep) << 2) + 1; } break; default: // end of last step - cleanup ramp_status = RAMP_IDLE; // we won't be back run_flag = 0; // the move is complete break; } } //ISR(TIMER1_COMPA_vect) void do_step(byte step_pin) { digitalWrite(step_pin, HIGH); delayMicroseconds(2); digitalWrite(step_pin, LOW); } // // main // long pos = 1200; // some position to move to void loop() { move_to(pos); // go to it while (run_flag); // wait until the motor stops do_wait(); move_to(-pos); // go back home while (run_flag); // wait until the motor stops do_wait(); } //main