MoveToLimit - moves stepper in specified direction until a given PIC pin goes high. Can be used with a limit switch for calibrating the system to a known position.
Rotate - turns stepper through a given number of complete revolutions.
MoveToPosition - moves stepper as close as possible to a given position in the specified direction.
ShortestMoveToPosition - moves stepper as close as possible to a given position in the shortest direction.
Hopefully the code and other subs / functions are self-explanatory (I tend to forget why I've written something so I put quite a few comments in!). I'm posting for a couple of reasons:
- I hope it will be of use to someone;
- For my own learning. This is one of my first forays into Swordfish and PICs so I'd welcome any comments and / or (constructive) critisisms. If I've done something silly or you know a more efficient way of achieving the same results then I'd like to hear it.
Cheers,
Andy.
Code: Select all
{
********************************************************************************
* Name : Stepper Motor Module *
* Author : AndyO *
* Notice : Copyright (c) 2008 AndyO *
* : All Rights Reserved *
* Date : 01/03/2008 *
* Version : 1.0 *
* Notes : For use with 4 phase unipolar stepper motor *
* : *
* : All angles * 100 (i.e. 7.5deg in reality = 750 in module) *
********************************************************************************
}
Module Stepper
Include "Math.bas"
'===============================================================================
'User definable options
'-------------------------------------------------------------------------------
#option Stepper_Port = PORTB 'Which PORT stepper is connected to
#option Stepper_Pin1 = %00000001 '}
#option Stepper_Pin2 = %00000010 '}Which 4 pins the stepper motor is
#option Stepper_Pin3 = %00000100 '}connected to. (Pins 0-3 shown here)
#option Stepper_Pin4 = %00001000 '}
#option Stepper_FullStepSize = 750 'Degrees motor moves in one full step
#option Stepper_HalfStepMode = True 'Drive motor in half step mode?
#option Stepper_DefaultSpeed = 4 'Delay in mS between steps
#option Stepper_LimitSwitch = PORTB.4 'Pin which limit switch is connected
'to. Note: Active HIGH
#option Stepper_TRIS = getTRIS(Stepper_Port)
'===============================================================================
'Variable and Const Declarations
'-------------------------------------------------------------------------------
'---Bring options into module
Private Const
#if Stepper_HalfStepMode Then
StepJump = 1,
StepSize = Stepper_FullStepSize / 2,
#else
StepJump = 2,
StepSize = Stepper_FullStepSize,
#endif
Pin1 As Byte = Stepper_Pin1,
Pin2 As Byte = Stepper_Pin2,
Pin3 As Byte = Stepper_Pin3,
Pin4 As Byte = Stepper_Pin4,
'PortMask used to turn on/off only the pins the stepper is connnected to
PortMask As Byte = Not(Pin1 Or Pin2 Or Pin3 Or Pin4),
'Step masks used to turn on the correct pins for the current step.
'Constant array loaded with half-step sequence
StepMask(8) As Byte = (Pin1, 'Step 1: 0001
Pin1 Or Pin2, 'Step 2: 0011
Pin2, 'Step 3: 0010
Pin2 Or Pin3, 'Step 4: 0110
Pin3, 'Step 5: 0100
Pin3 Or Pin4, 'Step 6: 1100
Pin4, 'Step 7: 1000
Pin4 Or Pin1), 'Step 8: 1001
DefaultSpeed = Stepper_DefaultSpeed
Private Dim
StepperPort As Stepper_Port,
LimitSwitch As Stepper_LimitSwitch.Stepper_LimitSwitch@,
StepperTRIS As Stepper_TRIS
'---Other Dims
Private Dim CurrentStep As Byte
Private Dim Position As Word
Private Dim DistanceClockwise As Word
Private Dim DistanceAntiClockwise As Word
Private Dim StartingPosition As Word
'===============================================================================
'Helper Subs and Functions (Private - can't be called from main program)
'-------------------------------------------------------------------------------
Private Sub SingleStep(pRotateClockwise As Boolean)
If pRotateClockwise Then
CurrentStep = CurrentStep + StepJump
Position = Position + StepSize
If Position > 35999 Then
Position = Position - 36000
EndIf
Else
CurrentStep = CurrentStep - StepJump
'Check for zero cross and update Position variable accordingly
If Position >= StepSize Then
Position = Position - StepSize
Else
Position = (36000 + Position) - StepSize
EndIf
EndIf
Select CurrentStep
Case >8
CurrentStep = StepJump
Case 0
CurrentStep = 8
EndSelect
'AND'ing Port with PortMask sets only stepper pins to '0', ignores others
'OR'ing Port with StepMask for current step sets the relevant pins high
StepperPort = (StepperPort And PortMask) Or StepMask(CurrentStep - 1)
End Sub
'===============================================================================
'Public Subs and Functions
'-------------------------------------------------------------------------------
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'Name: Move
'Purpose: Moves stepper with specified direction, number of steps and speed
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Public Sub Move(pRotateClockwise As Boolean = True, pNumberOfSteps As Word = 1, pSpeed As Byte = DefaultSpeed)
While pNumberOfSteps > 0
SingleStep(pRotateClockwise)
DelayMS(pSpeed)
Dec(pNumberOfSteps)
Wend
End Sub
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'Name: MoveToLimit
'Purpose: Moves stepper with specified direction and speed until limit switch
' is triggered. Optional pStepTimeout sets number of steps before
' movement stops regardless of limit switch state.
'
'Returns: True if limit switch reached before pStepTimeout,
' False otherwise
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Public Function MoveToLimit(pRotateClockwise As Boolean = True, pSpeed As Byte = DefaultSpeed, pStepTimeout As Word = 0) As Boolean
If pStepTimeout > 0 Then
'Timeout has been specified
While LimitSwitch = 0 And pStepTimeout > 0
SingleStep(pRotateClockwise)
DelayMS(pSpeed)
Dec(pStepTimeout)
Wend
If pStepTimeout = 0 Then
result = False
Else
result = True
EndIf
Else
'No timeout specified
While LimitSwitch = 0
SingleStep(pRotateClockwise)
DelayMS(pSpeed)
Wend
result = True
EndIf
End Function
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'Name: ResetPosition
'Purpose: Sets Position variable to specified value. Useful for setting zero
' point after move to limit switch or other calibration.
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Public Sub ResetPosition(pPosition As Word = 0)
Position = pPosition
End Sub
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'Name: MoveToPosition
'Purpose: Moves stepper as close as possible to desired position with
' specified direction and speed.
'
' Note: All positions * 100 i.e. 90 deg = 9000
'
'Returns: Actual final position of stepper. May vary from desired position
' due to resolution of step size.
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Public Function MoveToPosition(pDesiredPosition As Word, pRotateClockwise As Boolean = True, pSpeed As Byte = DefaultSpeed) As Word
'If desired position is 360deg or over then bring it into 0 - 35999 range
If pDesiredPosition > 35999 Then
pDesiredPosition = pDesiredPosition - 36000
EndIf
'Keep making steps until the actual position is within 1/2 a step of the
'desired position (closest we can get).
'2nd condition prevents problems caused around 360 / 0 degrees.
'i.e. without it then if desired position is 35999 and actual position is 0
'then the code will loop forever as Desired - Actual = 35999
While Math.abs(pDesiredPosition - Position) > StepSize / 2 And
Math.abs(pDesiredPosition - Position) < 36000 - (StepSize / 2)
SingleStep(pRotateClockwise)
DelayMS(pSpeed)
Wend
result = Position
End Function
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'Name: ShortestMoveToPosition
'Purpose: Moves stepper as close as possible to desired position with
' specified speed in the shortest direction.
'
' Note: All positions * 100 i.e. 90 deg = 9000
'
'Returns: Actual final position of stepper. May vary from desired position
' due to resolution of step size.
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Public Function ShortestMoveToPosition(pDesiredPosition As Word, pSpeed As Byte = DefaultSpeed) As Word
'If desired position is 360deg or over then bring it into 0 - 35999 range
If pDesiredPosition > 35999 Then
pDesiredPosition = pDesiredPosition - 36000
EndIf
'Calculate distance between desired and actual position in each direction
If pDesiredPosition > Position Then
DistanceClockwise = pDesiredPosition - Position
DistanceAntiClockwise = (36000 - pDesiredPosition) + Position
Else
DistanceClockwise = (36000 - Position) + pDesiredPosition
DistanceAntiClockwise = Position - pDesiredPosition
EndIf
'Call MoveToPosition function. Rotation direction determined by whether
'distance in clockwise direction is smaller than distance anticlockwise
result = MoveToPosition(pDesiredPosition, DistanceClockwise < DistanceAntiClockwise, pSpeed)
End Function
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'Name: Rotate
'Purpose: Rotates stepper the specified number of complete revolutions with
' specified direction and speed.
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Public Sub Rotate(pNumberOfRotations As Byte = 1, pRotateClockwise As Boolean = True, pSpeed As Byte = DefaultSpeed)
While pNumberOfRotations > 0
StartingPosition = Position
SingleStep(pRotateClockwise)
DelayMS(pSpeed)
While Position <> StartingPosition
SingleStep(pRotateClockwise)
DelayMS(pSpeed)
Wend
Dec(pNumberofRotations)
Wend
End Sub
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'Name: PowerOff
'Purpose: Stops all output on stepper pins
'
' Note: Stepper won't hold position
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Public Sub PowerOff()
StepperPort = StepperPort And PortMask
End Sub
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'Name: PowerOn
'Purpose: Resumes output on stepper pins
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Public Sub PowerOn()
StepperPort = (StepperPort And PortMask) Or StepMask(CurrentStep - 1)
End Sub
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'Name: GetStep
'Returns: Number of current step (1 - 8)
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Public Function GetStep() As Byte
result = CurrentStep
End Function
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
'Name: GetPosition
'Returns: Current stepper position (0 - 35,999)
'
' Note: All positions * 100 i.e. 90 deg = 9000
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Public Function GetPosition() As Word
result = Position
End Function
'===============================================================================
'Module Initialisation
'-------------------------------------------------------------------------------
'Initialise vars
CurrentStep = StepJump
Position = 0
'Set TRIS without changing non-stepper pins
StepperTRIS = StepperTRIS And PortMask