Stepper Motor Module

Coding and general discussion relating to user created compiler modules

Moderators: David Barker, Jerry Messina

Post Reply
AndyO
Registered User
Registered User
Posts: 48
Joined: Sat Oct 27, 2007 7:08 pm
Location: Beijing, China

Stepper Motor Module

Post by AndyO » Sat Mar 15, 2008 11:15 pm

Influenced and inspired by Gramo's stepper module, here's my effort. I wrote it because I needed to be able to connect the stepper to four non-contiguous pins. It works in a slightly different way to Gramo's module and it has what I think are a couple of useful features:

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
P.S. - I've found Gramo's site - http://users.tpg.com.au/gramo/Site/index.htm - really useful. Loads of info, very clear and easy to understand. Heartily recommended for anyone else getting started with PICs.

User avatar
octal
Registered User
Registered User
Posts: 586
Joined: Thu Jan 11, 2007 12:49 pm
Location: Paris IDF
Contact:

Post by octal » Sun Mar 16, 2008 8:40 am

Hi Andy0,
thanks for your contribution. You should move this module to the Modules Section of the WIKI http://www.sfcompiler.co.uk/wiki/pmwiki ... er.Modules

regards
octal

AndyO
Registered User
Registered User
Posts: 48
Joined: Sat Oct 27, 2007 7:08 pm
Location: Beijing, China

Post by AndyO » Sun Mar 16, 2008 8:21 pm

Thanks for the reply. I wasn't sure if it was "Wiki-worthy" but have done as you suggest and put the module on the user site.

Hope it is of use to someone.

TimB
Posts: 262
Joined: Wed Oct 04, 2006 7:25 am
Location: London UK

Post by TimB » Sun Mar 16, 2008 9:04 pm

Andy

Great stuff!!

What ever is posted its a help to some one. I can vouch for that as I find little snippets like that and even if the do not do exactly what I want they can be modified.

Post Reply