Example Stepper Motor Control Programs

Control Program in the C Language

// C Language demo for Stepper Motors using PowerIO Wildcard, V6 (for PDQ platform)
// DATE:        11/6/2009
 
// This file provides a demo program to control one or
// more stepper motors using a PDQ controller with one or two Power I/O Wildcards.
// Two steppers per Wildcard can be supported.
// The default implementation is two steppers, though 4 can be supported.
// At 1000 steps/sec, four steppers consume about 8% of the CPU time on a PDQ controller.
 
// 1000 TICKS/SECOND allows a maximum speed of 1000 steps per second
// if full stepping a motor (which may be too fast for the default motor shipped
// by Mosaic), or 1000 half steps per second if half stepping a motor.
// Driving 4 motors with TICKS_PER_SECOND = 1000 makes a worst-case demand
// of about 8% of the PDQ processor's time.
 
// This demo uses OC3 to make a 1 msec clock.  You may use any of the 8 output compares
// that are available; see the PDQ user guide for details.
 
// NOTE: make sure that MOTOR_0_1_MODULENUM and MOTOR_2_3_MODULENUM
// match the hardware jumpers on your PowerIO Wildcard(s)!
 
#include  <mosaic\allqed.h>    // include the standard library functions
#include "steppers.h"          // Include prebuilt driver
 
 
#define TCNTS_PER_MSEC 625
// 625 is the default number of tcounts per millisecond on the PDQ platforms;
// this corresponds to 1.6 microsec per tcount.
// NOTE: if your application calls ECTPrescaler to change the default system prescaler,
// change the value of TCNTS_PER_MSEC to reflect the change.
// See the glossary entry for ECTPrescaler for more details.
 
#define MSEC_PER_TICK 1    // for accuracy, this should divide evenly into 1000
 
#define TCNTS_PER_TICK ((int) (TCNTS_PER_MSEC * MSEC_PER_TICK))
    // must fit in 16 bits (<65535)
 
#define TICKS_PER_SECOND ((int) (1000 / MSEC_PER_TICK))
    // must agree with TCNTS_PER_TICK
 
// The following speeds are in steps/sec; the Init_Steppers function adjusts
// the values to be correct whether full stepping or half stepping.
// Similarly, the following accelerations/decelerations are in steps/sec/sec.
// But if you pass these parameters to a function directly, make sure
// to multiply by two if you are half stepping;
// see the call to Steps_At_Speed in the Revolutions function for an example.
// Note that you are free to specify different speeds and accelerations
// for each motor. You can mix and match by customizing InitSteppers().
 
#define MAX_STEPPERS 4     // NOTE: this can NOT exceed 4!
    // values in excess of 4 exceed the allocated data structures of the stepper driver.
 
// the following are in (half)steps/sec:
#define DEFAULT_JOG_START_SPEED 100    // no ramp-up or ramp-down needed
#define DEFAULT_STEADY_SPEED    500    // destination speed of a ramp-up
 
// the following are in (half)steps/sec/sec:
#define DEFAULT_ACCELERATION  2500        // used during ramp up
#define DEFAULT_DECELERATION  2500        // used during ramp down
 
 
// motor0 and motor1 are interfaced via one Power I/O Wildcard:
#define MOTOR_0_1_ADDRESS    0x00 // module offset 0 is the high-current output byte
#define MOTOR_0_1_MODULENUM 0x01 // modulenum = 1; MAKE SURE THIS MATCHES HARDWARE JUMPERS!
#define MOTOR_0_MASK        0x0F // motor0 is at bits 0-3
#define MOTOR_1_MASK        0xF0 // motor1 at bits 4-7
 
 
// motor2 and motor3 are interfaced via a second Power I/O Wildcard:
#define MOTOR_2_3_ADDRESS    0x00 // module offset 0 is the high-current output byte
#define MOTOR_2_3_MODULENUM 0x02 // modulenum = 2; MAKE SURE THIS MATCHES HARDWARE JUMPERS!
#define MOTOR_2_MASK        0x0F // motor2 is at bits 0-3
#define MOTOR_3_MASK        0xF0 // motor3 at bits 4-7
 
 
void OC3_Motor_Service(void)
// use OC3 as a periodic interrupt, based on TCNTS_PER_TICK
{     uint oc3_value;
     Call_Step_Manager();                            // invoke the stepper motor handler
     oc3_value = OCICRegRead(3);                    // use OC3, auto-clears irq flag
     OCRegWrite(oc3_value+TCNTS_PER_TICK, 3); // setup next interrupt; irq flag autocleared
}
 
MAKE_ISR(OC3_Motor_Service); // needed for Attach() to work
 
 
void Init_Stepper_IRQ(void)
{    ATTACH(OC3_Motor_Service, ECT3_ID);     // post the interrupt handler
    ECTClearInterruptFlag(3);    // clear irq flag
    OutputCompare(3);                // configure ect channel 3 as an output compare
    OCAction(OC_NO_ACTION, 3); // timer channel OC3 has no pin action on timer match
    ECTFastClear();                // auto-clear of the interrupt bit when register is accessed
    ECTInterruptEnable(3);        // enable local interrupt
    ENABLE_INTERRUPTS();            // globally enable interrupts
}
 
void Disable_Stepper_IRQ(void)
{    ECTInterruptDisable(3);        // locally disable
    ECTClearInterruptFlag(3);    // clear irq flag
}
 
void Init_Motor_0(int use_half_steps)
// use_half_steps is TRUE (nonzero) for half stepping, FALSE (=0) for full stepping.
// Note that you should customize  this routine for your own application,
// setting the appropriate steady speed (typically the maximum speed
// at which no steps are lost), acceleration, and half/full step
// specification for each motor.
{    int speedfactor = 1;
    if(use_half_steps)
        speedfactor = 2;    // if halfstepping, multiply speeds & accel * 2
   Init_Stepper_Status(DEFAULT_JOG_START_SPEED * speedfactor,
        DEFAULT_STEADY_SPEED * speedfactor, DEFAULT_ACCELERATION * speedfactor,
        DEFAULT_DECELERATION * speedfactor,
        MOTOR_0_1_ADDRESS, MOTOR_0_1_MODULENUM, MOTOR_0_MASK, use_half_steps, 0);    // motor0
}
 
void Init_Motor_1(int use_half_steps)
// use_half_steps is TRUE (nonzero) for half stepping, FALSE (=0) for full stepping.
// Note that you should customize  this routine for your own application,
// setting the appropriate steady speed (typically the maximum speed
// at which no steps are lost), acceleration, and half/full step
// specification for each motor.
{    int speedfactor = 1;
    if(use_half_steps)
        speedfactor = 2;    // if halfstepping, multiply speeds & accel * 2
   Init_Stepper_Status(DEFAULT_JOG_START_SPEED * speedfactor,
        DEFAULT_STEADY_SPEED * speedfactor, DEFAULT_ACCELERATION * speedfactor,
        DEFAULT_DECELERATION * speedfactor,
        MOTOR_0_1_ADDRESS, MOTOR_0_1_MODULENUM, MOTOR_1_MASK, use_half_steps, 1);    // motor1
}
 
void Init_Motor_2(int use_half_steps)
// use_half_steps is TRUE (nonzero) for half stepping, FALSE (=0) for full stepping.
// Note that you should customize  this routine for your own application,
// setting the appropriate steady speed (typically the maximum speed
// at which no steps are lost), acceleration, and half/full step
// specification for each motor.
{    int speedfactor = 1;
    if(use_half_steps)
        speedfactor = 2;    // if halfstepping, multiply speeds & accel * 2
   Init_Stepper_Status(DEFAULT_JOG_START_SPEED * speedfactor,
        DEFAULT_STEADY_SPEED * speedfactor, DEFAULT_ACCELERATION * speedfactor,
        DEFAULT_DECELERATION * speedfactor,
        MOTOR_2_3_ADDRESS, MOTOR_2_3_MODULENUM, MOTOR_2_MASK, use_half_steps, 2);    // motor2
}
 
 
void Init_Motor_3(int use_half_steps)
// use_half_steps is TRUE (nonzero) for half stepping, FALSE (=0) for full stepping.
// Note that you should customize  this routine for your own application,
// setting the appropriate steady speed (typically the maximum speed
// at which no steps are lost), acceleration, and half/full step
// specification for each motor.
{    int speedfactor = 1;
    if(use_half_steps)
        speedfactor = 2;    // if halfstepping, multiply speeds & accel * 2
   Init_Stepper_Status(DEFAULT_JOG_START_SPEED * speedfactor,
        DEFAULT_STEADY_SPEED * speedfactor, DEFAULT_ACCELERATION * speedfactor,
        DEFAULT_DECELERATION * speedfactor,
        MOTOR_2_3_ADDRESS, MOTOR_2_3_MODULENUM, MOTOR_3_MASK, use_half_steps, 3);    // motor3
}
 
_Q void Init_Steppers( int use_half_steps )
// use_half_steps is TRUE (nonzero) for half stepping, FALSE (=0) for full stepping.
// This routine can be customized for your own application,
// setting the appropriate steady speed (typically the maximum speed
// at which no steps are lost), acceleration, and half/full step
// specification for each motor.  While this is coded so that all motors
// either halfstep or full step, you can halfstep some motors while
// full stepping others; simply modify the code to
// set &speedfactor to 1 or 2 and set the use_half_steps flag true or false as required.
{   int i;
     var_ticks_per_sec = TICKS_PER_SECOND;     // must init this global variable
    Disable_Stepper_IRQ();
    Zero_Stepper_Arrays();
    // initialize only the motors that are in use...
     Init_Motor_0(use_half_steps);
//     Init_Motor_1(use_half_steps);    // comment out this line if motor1 is not used
//     Init_Motor_2(use_half_steps);    // comment out this line if motor2 is not used
//     Init_Motor_3(use_half_steps);    // comment out this line if motor3 is not used
     for(i=0; i < MAX_STEPPERS; i++)
         Clear_Motor_Port(i);
             // must clear ALL ports before energizing any to handle port overlaps
     for(i=0; i < MAX_STEPPERS; i++)
         Energize_Stepper(i);
    Init_Stepper_IRQ();  // globally enables interrupts
}
 
#define HALFSTEPS_PER_REV ((uint) 400)
#define MAX_REV ((uint) 100)
// HALFSTEPS_PER_REV * MAX_REV must fit in a 16-bit value (< 65535)
 
 
_Q void Revolutions( int direction, uint num_revolutions )
// rotates stepper motor 0 in specified direction for the specified number of revolutions,
// up to a max of MAX_REV revolutions
{    uint numsteps = (MIN(num_revolutions,MAX_REV) * HALFSTEPS_PER_REV);
    *Step_Count(0) = (long) 0;
    Steps_At_Speed(direction, numsteps, DEFAULT_STEADY_SPEED*2, 0);
                // configure the interrupt routine to do specified action in background
    while(Motor_State(0)!= MOTOR_STOPPED)    // wait until motor stops
        Pause();            // give other tasks a chance to run while waiting
    MicrosecDelay(1000);  // wait one more tick to be sure step count is updated
    numsteps = ABS((int)*Step_Count(0));  // convert to unsigned (removes direction info)
    printf("\nMotor took %6u steps\n",numsteps);
}
 
 
_Q void Demo_Stepper( void ) // this is the stepper demo, spins motor ten revolutions
{    Init_Steppers(TRUE);    // init specified motors for half stepping
    Revolutions(CW,5);    // five clockwise revolutions
    Revolutions(CCW,5);    // five counter-clockwise revolutions
}
 
 
int main( void ) // calls the Stepper_Demo, spins motor ten revolutions
{    Demo_Stepper();
    return 0;
}

Control Program in the Forth Language

\ Forth demo for Stepper Motors using PowerIO Wildcard, V6 (for PDQ platform)
\ DATE:        11/6/2009
 
\ This file provides a demo program to control one or
\ more stepper motors using a PDQ controller with one or two Power I/O Wildcards.
\ Two steppers per Wildcard can be supported.
\ The default implementation is two steppers, though 4 can be supported.
\ At 1000 steps/sec, four steppers consume about 8% of the CPU time on a PDQ controller.
 
\ 1000 TICKS/SECOND allows a maximum speed of 1000 steps per second
\ if full stepping a motor (which may be too fast for the default motor shipped
\ by Mosaic), or 1000 half steps per second if half stepping a motor.
\ Driving 4 motors with TICKS_PER_SECOND = 1000 makes a worst-case demand
\ of about 8% of the PDQ processor's time.
 
\ This demo uses OC3 to make a 1 msec clock.  You may use any of the 8 output compares
\ that are available; see the PDQ user guide for details.
 
\ NOTE: make sure that MOTOR_0_1_MODULENUM and MOTOR_2_3_MODULENUM
\ match the hardware jumpers on your PowerIO Wildcard(s)!
 
 
ANEW stepdemo    \ marker for easy reloading
 
DECIMAL    \ interpret these numbers as decimal...
 
625  CONSTANT  TCNTS_PER_MSEC
\ 625 is the default number of tcounts per millisecond on the PDQ platforms;
\ this corresponds to 1.6 microsec per tcount.
\ NOTE: if your application calls ECT.Prescaler to change the default system prescaler,
\ change the value of TCNTS_PER_MSEC to reflect the change.
\ See the glossary entry for ECT.Prescaler for more details.
 
1 CONSTANT MSEC_PER_TICK    \ for accuracy, this should divide evenly into 1000
 
TCNTS_PER_MSEC MSEC_PER_TICK * CONSTANT  TCNTS_PER_TICK  \ must fit in 16 bits (<65535)
 
1000 MSEC_PER_TICK / CONSTANT TICKS_PER_SECOND \ must agree with TCNTS_PER_TICK
 
\ The following speeds are in steps/sec; the Init_Steppers function adjusts
\ the values to be correct whether full stepping or half stepping.
\ Similarly, the following accelerations/decelerations are in steps/sec/sec.
\ But if you pass these parameters to a function directly, make sure
\ to multiply by two if you are half stepping;
\ see the call to Steps_At_Speed in the Revolutions function for an example.
\ Note that you are free to specify different speeds and accelerations
\ for each motor. You can mix and match by customizing InitSteppers().
 
4 CONSTANT MAX_STEPPERS    \ NOTE: this can NOT exceed 4!
    \ values in excess of 4 exceed the allocated data structures of the stepper driver.
 
\ the following are in (half)steps/sec:
100    CONSTANT DEFAULT_JOG_START_SPEED    \ no ramp-up or ramp-down needed
500    CONSTANT DEFAULT_STEADY_SPEED       \ destination speed of a ramp-up
 
\ the following are in (half)steps/sec/sec:
2500 CONSTANT DEFAULT_ACCELERATION         \ used during ramp up
2500 CONSTANT DEFAULT_DECELERATION         \ used during ramp down
 
 
\ motor0 and motor1 are interfaced via one Power I/O Wildcard:
0x00 CONSTANT  MOTOR_0_1_ADDRESS    \ module offset 0 is the high-current output byte
0x01 CONSTANT  MOTOR_0_1_MODULENUM \ modulenum = 1; MAKE SURE THIS MATCHES HARDWARE JUMPERS!
0x0F CONSTANT  MOTOR_0_MASK        \ motor0 is at bits 0-3
0xF0 CONSTANT  MOTOR_1_MASK        \ motor1 at bits 4-7
 
 
\ motor2 and motor3 are interfaced via a second Power I/O Wildcard:
0x00 CONSTANT  MOTOR_2_3_ADDRESS    \ module offset 0 is the high-current output byte
0x02 CONSTANT  MOTOR_2_3_MODULENUM \ modulenum = 2; MAKE SURE THIS MATCHES HARDWARE JUMPERS!
0x0F CONSTANT  MOTOR_2_MASK    \ motor2 is at bits 0-3
0xF0 CONSTANT  MOTOR_3_MASK    \ motor3 at bits 4-7
 
: OC3_Motor_Service    ( -- )
\ use OC3 as a periodic interrupt, based on TCNTS_PER_TICK.
     Call_Step_Manager        \ invoke the stepper motor handler, treats Y nicely
     3 OC.IC.REG.READ                ( oc3.register -- ) \ auto-clears irq flag
     TCNTS_PER_TICK +                ( updated.oc3.reg.contents -- )
     3 OC.REG.WRITE                ( -- ) \ setup next interrupt; irq flag autocleared
;
 
: Init_Stepper_IRQ    ( -- )
    CFA.FOR OC3_Motor_Service ECT3.ID ATTACH
    3 ECT.CLEAR.INTERRUPT.FLAG          \ clear irq flag
    3 OUTPUT.COMPARE            \ configure ect channel 3 as an output compare
    OC.NO.ACTION 3             ( oc_action_id\channel_id -- )
    OC.ACTION                    ( -- ) \ timer channel OC3 has no pin action on timer match
    ECT.FAST.CLEAR                \ auto-clear of the interrupt bit when register is accessed
    3 ECT.INTERRUPT.ENABLE    \ locally enable
    ENABLE.INTERRUPTS
;
 
: Disable_Stepper_IRQ    ( -- )
     3 ECT.INTERRUPT.DISABLE        \ locally disable
    3 ECT.CLEAR.INTERRUPT.FLAG        \ clear irq flag
;
 
: Init_Motor_0    ( Half_Steps? -- )
\ Half_Steps? is TRUE (nonzero) for half stepping, FALSE (=0) for full stepping.
\ Note that you should customize  this routine for your own application,
\ setting the appropriate steady speed (typically the maximum speed
\ at which no steps are lost), acceleration, and half/full step
\ specification for each motor.
     1 NEEDED
    DUP IF 2 ELSE 1 ENDIF         \ if halfstepping, multiply speeds & accel * 2
    LOCALS{ &speedfactor &half? }
    DEFAULT_JOG_START_SPEED &speedfactor *
    DEFAULT_STEADY_SPEED    &speedfactor *
    DEFAULT_ACCELERATION    &speedfactor *
    DEFAULT_DECELERATION    &speedfactor *
    MOTOR_0_1_ADDRESS MOTOR_0_1_MODULENUM MOTOR_0_MASK &half? 0    \ motor0
    Init_Stepper_Status
;
 
: Init_Motor_1    ( Half_Steps? -- )
\ Half_Steps? is TRUE (nonzero) for half stepping, FALSE (=0) for full stepping.
\ Note that you should customize  this routine for your own application,
\ setting the appropriate steady speed (typically the maximum speed
\ at which no steps are lost), acceleration, and half/full step
\ specification for each motor.
     1 NEEDED
    DUP IF 2 ELSE 1 ENDIF         \ if halfstepping, multiply speeds & accel * 2
    LOCALS{ &speedfactor &half? }
    DEFAULT_JOG_START_SPEED &speedfactor *
    DEFAULT_STEADY_SPEED    &speedfactor *
    DEFAULT_ACCELERATION    &speedfactor *
    DEFAULT_DECELERATION    &speedfactor *
    MOTOR_0_1_ADDRESS MOTOR_0_1_MODULENUM MOTOR_1_MASK &half? 1    \ motor1
    Init_Stepper_Status
;
 
: Init_Motor_2    ( Half_Steps? -- )
\ Half_Steps? is TRUE (nonzero) for half stepping, FALSE (=0) for full stepping.
\ Note that you should customize  this routine for your own application,
\ setting the appropriate steady speed (typically the maximum speed
\ at which no steps are lost), acceleration, and half/full step
\ specification for each motor.
     1 NEEDED
    DUP IF 2 ELSE 1 ENDIF         \ if halfstepping, multiply speeds & accel * 2
    LOCALS{ &speedfactor &half? }
    DEFAULT_JOG_START_SPEED &speedfactor *
    DEFAULT_STEADY_SPEED    &speedfactor *
    DEFAULT_ACCELERATION    &speedfactor *
    DEFAULT_DECELERATION    &speedfactor *
    MOTOR_2_3_ADDRESS MOTOR_2_3_MODULENUM MOTOR_2_MASK &half? 2    \ motor2
    Init_Stepper_Status
;
 
: Init_Motor_3    ( Half_Steps? -- )
\ Half_Steps? is TRUE (nonzero) for half stepping, FALSE (=0) for full stepping.
\ Note that you should customize  this routine for your own application,
\ setting the appropriate steady speed (typically the maximum speed
\ at which no steps are lost), acceleration, and half/full step
\ specification for each motor.
     1 NEEDED
    DUP IF 2 ELSE 1 ENDIF         \ if halfstepping, multiply speeds & accel * 2
    LOCALS{ &speedfactor &half? }
    DEFAULT_JOG_START_SPEED &speedfactor *
    DEFAULT_STEADY_SPEED    &speedfactor *
    DEFAULT_ACCELERATION    &speedfactor *
    DEFAULT_DECELERATION    &speedfactor *
    MOTOR_2_3_ADDRESS MOTOR_2_3_MODULENUM MOTOR_3_MASK &half? 3    \ motor3
    Init_Stepper_Status
;
 
: Init_Steppers    ( Half_Steps? -- )
\ Half_Steps? is TRUE (nonzero) for half stepping, FALSE (=0) for full stepping.
\ This routine can be customized for your own application,
\ setting the appropriate steady speed (typically the maximum speed
\ at which no steps are lost), acceleration, and half/full step
\ specification for each motor.  While this is coded so that all motors
\ either halfstep or full step, you can halfstep some motors while
\ full stepping others; simply modify the code to
\ set &speedfactor to 1 or 2 and set the &half? flag true or false as required.
     1 NEEDED
    LOCALS{ &half? }
    TICKS_PER_SECOND var_ticks_per_sec !    \ must init this global variable
    Disable_Stepper_IRQ
    Zero_Stepper_Arrays
    \ initialize only the motors that are in use...
     &half? Init_Motor_0
\     &half? Init_Motor_1    \ comment out this line if motor1 is not used
\     &half? Init_Motor_2    \ comment out this line if motor2 is not used
\     &half? Init_Motor_3    \ comment out this line if motor3 is not used
 
    MAX_STEPPERS 4 UMIN 0
    DO    I Clear_Motor_Port
    LOOP      \ must clear ALL ports before energizing any to handle port overlaps!
    MAX_STEPPERS 4 UMIN 0
    DO    I Energize_Stepper
    LOOP
    Init_Stepper_IRQ    ( -- )  \ globally enables interrupts
;
 
400 CONSTANT HALFSTEPS_PER_REV
100 CONSTANT MAX_REV    \ HALFSTEPS_PER_REV * MAX_REV must fit in a 16-bit value (< 65535)
 
 
: Revolutions ( direction\num_revolutions -- )
\ rotates stepper motor 0 in specified direction for the specified number of revolutions,
\ up to a max of MAX_REV
    MAX_REV UMIN HALFSTEPS_PER_REV *
    LOCALS{ &rev_steps &direction }
    DIN 0 0 Step_Count 2!    \ store 32-bit zero into count
    &direction &rev_steps DEFAULT_STEADY_SPEED 2* 0 ( direction\numsteps\speed\motor_num -- )
    Steps_At_Speed \ configure the interrupt routine to do specified action in background
    BEGIN 0 Motor_State MOTOR_STOPPED =    \ wait until motor stops
            PAUSE                                    \ give other tasks a chance to run while waiting
    UNTIL
    1000 MICROSEC.DELAY    \ wait one more tick to ensure Step_Count is updated
    CR ." Motor took " 0 Step_Count 2@ DABS D. ."  steps."
;
 
: Demo_Stepper ( -- )
    TRUE Init_Steppers    \ init specified motors for half stepping
    CW 5 Revolutions        \ five clockwise revolutions
    CCW 5 Revolutions        \ five counter-clockwise revolutions
;