📄 step.c
字号:
/*****************************************************************************/
/* */
/* File: step.c - Functions for stepper motor control */
/* */
/* Routines: */
/* init_step - Sets up ports and timers */
/* go_position - Go to a target position (relative) */
/* go_velocity - Run at a target velocity */
/* stop - Stop the pulses in position or velocity mode */
/* */
/* Counter control routines: */
/* set_frequency - Set the frequency for the steps (timer 2) */
/* set_position - Set the number of steps to go */
/* disable_ctrs - Inhibit counter activity */
/* enable_ctrs - Allow counter activity */
/* pause_ctr - Pause counting */
/* continue_ctr - Continue counting */
/* tigger_ctr - Start a counter running */
/* count_in_prog - Indicates whether a counter is running */
/* clear_IP - Clear the interrupt flag (set when ctr finishes)*/
/* */
/* Digital I/O routines: */
/* set_RUN - Set driver to "RUN" */
/* set_HOLD - Set driver to "HOLD" */
/* set_CW - Set clockwise direction */
/* set_CCW - Set counter-clockwise direction */
/* set_HALF - Step by half-steps */
/* set_FULL - Step by full-steps */
/* */
/* Remarks: */
/* Implents a controller for a stepper motor for both position and */
/* and velocity control. */
/* */
/* updates: */
/* 31 March 98 - Make work with standard menu */
/* 18 March 98 - Structured with Higher/Lower functions */
/* */
/*****************************************************************************/
/***********************************************************************
I M P O R T S
***********************************************************************/
#include "windows.h"
#include "rtapi.h"
#include "stdio.h"
#include "math.h"
#include "int32pci.h" /* Declarations for Int32 timer - I/O bd */
#include "step.h" /* exported declarations for this module */
/**********************************************************************/
/***********************************************************************
F O R W A R D D E C L A R A T I O N S
***********************************************************************/
/***********************************************************************
P R I V A T E D A T A V A R I A B L E S
***********************************************************************/
/***********************************************************************
E N T R Y R O U T I N E S
***********************************************************************/
/*****************************************************************************/
/* */
/* INIT_STEP */
/* Configure timer board for stepper control */
/* */
/*****************************************************************************/
void init_step(int chip) {
int tmp;
// Int32_Reset(chip); took this out so that servo code worked easily with stepper
/* Set Port A for Half step, not run, CW rotation */
tmp = Int32_Read_PortA(chip);
Int32_Write_PortA(chip,(tmp & ~(RUN)) | HALF | CW);
/* Set counter 1's external input line (PortB-5) */
/* All other lines are designated as outputs */
// added the Read 2/16/99 dblock
tmp = Int32_Read_Register(chip,PORT_B_DATA_DIR);
Int32_Write_Register(chip,PORT_B_DATA_DIR,tmp | BIT5);
// removed dblock 2/16/99 no longer using SWITCH
// Int32_Write_Register(chip,PORT_A_DATA_DIR,SWITCH);
/* Enadle digital I/O for ports A & B */
// added the Read 2/16/99 dblock
tmp = Int32_Read_Register(chip,MASTER_CONF_CNTR);
Int32_Write_Register(chip,MASTER_CONF_CNTR,tmp | PORT_A_EN_BIT | PORT_B_EN_BIT);
/* Counter/Timer Set-up: */
/* Counter 1 has external input, one cycle, pulse wave, retriggerable */
/* Counter 2 has internal input, continuous cycle, square wave, retrigg. */
Int32_Write_Register(chip,CNT_1_MODE_SPEC,CNT_MODE_ECE | CNT_MODE_EOE | CNT_MODE_REB);
tmp = CNT_MODE_CONT_PULSE | CNT_MODE_SQWV | CNT_MODE_REB | CNT_MODE_EOE;
Int32_Write_Register(chip,CNT_2_MODE_SPEC,tmp);
/* Set-up to stop pulses (CTR2) when counter one reaches zero */
tmp = Int32_Read_Register(chip,MASTER_CONF_CNTR);
Int32_Write_Register(chip,MASTER_CONF_CNTR,tmp | CNT_1_GATE_2);
}
/*****************************************************************************/
/* GO_POSITION */
/* Move the stepper a specified number of steps, in specified direction */
/* */
/* parameters: */
/* cline - input string with number of steps (signed) and velocity */
/* */
/*****************************************************************************/
int go_position(int chip,int position,float velocity) {
int tmp;
/* Check if counter one is running (indicating a position command is not */
/* finished) */
tmp = count_in_prog(chip, 1);
if (tmp != 0) return(-2);
/* Turn off RUN line on driver or we get one more step when counter resets */
tmp = set_HOLD(chip);
/* Pause counters so they don't start prematurely when re-enabled */
tmp = pause_ctr(chip,1);
tmp = pause_ctr(chip,2);
/* Disable counters while setting them up */
tmp = disable_ctrs(chip);
/* IP is set whenever a counter reaches terminal count, can cause trouble if */
/* not reset */
tmp = clear_IP(chip,1);
tmp = clear_IP(chip,2);
/* Determine the direction based on the sign of the position, and set I/O */
/* lines accordingly. */
tmp = abs(position);
if (tmp == position) set_CW(chip);
else set_CCW(chip);
tmp = set_position(chip, tmp); /* Load time constant for counter one */
/* Compute time constant to get desired frequency and load into counter 2 */
tmp = set_frequency(chip,(float)fabs(velocity));
if (tmp !=0) return(-3); /* Check if range violation occurred */
/* re-enable counters after set-up */
tmp=enable_ctrs(chip);
tmp = set_RUN(chip); /* Set driver to step on pulses */
/* Start the counters running */
tmp = continue_ctr(chip,1);
tmp = continue_ctr(chip,2);
tmp = trigger_ctr(chip,2);
tmp = trigger_ctr(chip,1);
return(0);
}
/*****************************************************************************/
/* GO_VELOCITY */
/* Move the stepper at a specified rate of steps/s and direction. */
/* */
/* parameters */
/* cline - input line for velocity */
/* */
/*****************************************************************************/
int go_velocity(int chip,float velocity) {
int tmp;
/* Check if counter one is running (indicating a position command is not */
/* finished) */
tmp = count_in_prog(chip, 1);
if (tmp != 0) return(-2);
/* Pause counters so they don't start prematurely when re-enabled */
tmp = pause_ctr(chip,1);
tmp = pause_ctr(chip,2);
/* Disable counters while setting them up */
tmp = disable_ctrs(chip);
/* IP is set whenever a counter reaches terminal count, can cause trouble if */
/* not reset */
tmp = clear_IP(chip,1);
tmp = clear_IP(chip,2);
/* If velocity is set to zero, we're done */
if (fabs(velocity)<.01) return(0);
/* Determine the direction based on the sign of the velocity and set I/O */
/* lines accordingly. */
if ( ((float)fabs(velocity)- velocity) <.01 ) set_CW(chip);
else set_CCW(chip);
/* Compute time constant to get desired frequency and load into counter 2 */
tmp = set_frequency(chip,(float)fabs(velocity));
if (tmp !=0) return(-3); /* Check if range violation occurred */
/* re-enable counters after set-up */
tmp=enable_ctrs(chip);
tmp = set_RUN(chip); /* Set driver to step on pulses */
/* Start counter 2 running */
tmp = continue_ctr(chip,2);
tmp = trigger_ctr(chip,2);
return(0);
}
/*****************************************************************************/
/* STOP */
/* Stop the stepper motion */
/*****************************************************************************/
void stop(int chip) {
int tmp;
/* Turn off RUN line on driver or we get one more step when counter resets */
tmp = set_HOLD(chip);
/* Pause counters so they don't start prematurely when re-enabled */
tmp = pause_ctr(chip,1);
tmp = pause_ctr(chip,2);
/* Disable counter to clear the count in progress flag on counter one to */
/* allow further commands */
tmp = disable_ctrs(chip);
}
/*****************************************************************************/
/* SET_POSITION */
/* Set the number of steps to go. */
/* */
/* parameters */
/* chip - the timer chip on the board to use */
/* position - the number of steps to move */
/* */
/*****************************************************************************/
int set_position(int chip, int position) {
int tmp;
/* This union provides a clean way to write each byte of a 2 byte value */
union countload
{
char a[2];
int b;
}cntld;
if (chip == 1 || chip == 2) { /* Be sure chip selection is valid */
cntld.b = abs(abs(position)-1); /* subtract one to get right count */
/* Pause counter while loading new time constant, in case of retrigger */
tmp = pause_ctr(chip,1);
/* Load new position time constant - no effect until counter triggered */
Int32_Write_Register(chip,CNT_1_TIME_CNST_MSB,cntld.a[1]);
Int32_Write_Register(chip,CNT_1_TIME_CNST_LSB,cntld.a[0]);
tmp = continue_ctr(chip,1);
return(0);
} else return(-1);
}
/*****************************************************************************/
/* SET_FREQUENCY */
/* Set the frequency for the steps (timer 2) */
/* */
/* parameters */
/* chip - the timer chip on the board to use */
/* frequency - the desired step frequecy */
/* */
/*****************************************************************************/
int set_frequency(int chip, float frequency ) {
int tmp;
/* This union provides a clean way to write each byte of a 2 byte value */
union countload
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -