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 ;