MEL PICBASIC Forum - Driving two stepper motors


  • Driving two stepper motors

    After some months, and considerable help from Bruce 'at Renton' - I've got my PBP code working to drive two stepper motors at the same time. This gives diagonal moves on an X/Y plotter.

    The basic idea is that: X_steps * X_period = Y_steps * Y_period. So that both motors finish turning at the same time. I've used Timer0 and Timer1 with some asm code to make the pulses and PBP to work out the steps and periods.

    To get a diagonal move one motor is set at one of the pre-define speeds(crawl,slow,fast..)
    and the period of the other motor is calculated from the desired move.

    To get the longest periods possible, the 'Prescales' of Timer0 and Timer1 are used if necessary. Timer0 is allocated to the Y axis because it's prescale goes to 'divide' by 256 and it's on the SHORTEST physical axis that you need the longest periods.

    The code has tested well for a few days and, so far, is stable. It's far from gereral purpose code and is tuned to my home built XY plotter (scan area 16000 by 32000 steps so WORD variables can hold the data OK). I've also stripped out all the LCD, home/limit switch stuff and the input now is only by RS232 from a terminal at 9200 Baud.

    I've posted it here because:

    1. It may be a good starting point to writing your own XY plotter code - I can't find any code to make diagonal moves anywhere else?
    2. Someone might spot better ways to doing this - advice and comments are welcome.

    Finally - thank you very much Bruce; you got me started on interrupts and I'm now 'at ease' with them, simple asm and MPLAB.

    The code is fairly well commented but I've worked with it so long that it's difficult to tell.

    Regards Bill legge

    Code:
    ' *****************************************************************************
    ' *                                                                           *
    ' * File...... 8722 Scanner_RS232                                             *
    ' * Purpose... Plotter/Scanner XY table with diagonal moves                   * 
    ' * Date...... April 2010 by Bill legge                                                     * 
    ' *                                                                           *
    ' *****************************************************************************
    
    ' 1.  Outline: software for a XY plotter/scanner table, axis driven by stepper
    '     motors that can move at the same time to achieve diagonal plotting/scanning
    '     The main variables are:
    '       *  Current location of the plot head: X_position, Y_position
    '       *  Desired location: X_demand, Y_demand
    '       *  Speed of move
    '     The X-demand, Y_demand and Speed words are input by RS232 @ 19200 Baud
    
    ' 2.  MCU PIC18F8722. Board is Futurlec ET-BASE PIC8722 (ICD2) 10MHz Xtal
    ' 3.  ETT relay board, opto isolated, active low, jumpers select PIC or Mach3 drive
    ' 4.  ETT LED display, active low
    ' 5.  ETT button input, active low
    ' 6.  Chinese stepper dive, absolute max voltage is 24V, Use 20V. DB25 Pins:
    '      1   X Drive        To Port D0
    '      2   Y Enable       To Port D5. High = enable, low = disable
    '      3   Y Direction    To Port D4. Low = move up, high = move down    
    '      4   Z Direction    Unused
    '      5   Z Step          Unused
    '      6   Z Enable       Unused
    '      7   X Direction    To Port D1. Low = move right, high = move left
    '      8   Y Drive        To Port D3
    '      9   Relay          Unused input
    '      10  X Home         To Port D6. Active low
    '      11  Y Home         To Port D7. Active low
    '      12                 Unused input
    '      13                 Unused input
    '      14  X Enable       To Port D2. High = enable, low = disable
    '      15                 Unused Input
    '      16
    '      17 
    '      18-25 Ground
    
    ' Mechanical
    ' Motor settings  200 steps/rev*16 pulses/step = 3,200 pulses/rev
    ' Belt pitch      2mm
    ' Pulley          36 teeth. 1rev = 36*2mm = 72mm
    ' X axis length   720mm = 10 revs = 32,000 pulses
    ' Y axis length   360mm =  5 revs = 16,000 pulses
    ' WORD [$ffff or 65,525] will hold maximum X or Y steps
    
    ' Motor Speeds
    ' MCU Osc is 4*10MHz. Period = 0.025uS. Tcy = 4*Period = 0.1uS
    ' Pulse periods: 800uS gives v.slow rotation and 100uS is maximum speed
    ' Use pulse rates that are 'powers of 2' so division/multiplication is fast
    ' Slug = 8192[Tcy] = 819.2uS = 819.2*3,200 = 2,621,440uS = 2.6S/rev (2^13)
    ' Slow = 4096[Tcy] = 409.6uS = 409.6*3,200 = 1,310,720uS = 1.3S/rev (2^12)
    ' Fast = 2048[Tcy] = 204.8uS = 204.8*3,200 =   655,360uS = 0.6S/rev (2^11)
    ' Race = 1024[Tcy] = 102.4uS = 102.4*3,200 =   327,680uS = 0.3S/rev (2^10)
    
    ' Timer1 in 16 Bit Mode, used for X axis
    ' Maximum count = $FFFF = 65,535
    ' Maximum prescale = 8
    ' Max count with max prescale = 65,535*8 = 524,280[Tcy]
    ' = 52,428uS = 52.4mS
    
    ' Timer0 in 16 Bit Mode, used for Y axis
    ' Maximum count = $FFFF = 65,535
    ' Maximum prescale = 256
    ' Max count with max prescale = 65,535*256 = 16,776,960[Tcy] 
    ' = 1,677,696uS = 1677mS = 1.677 Seconds = 0.596Hz
    
    ' Straight Line Plots
    ' If the X or Y steps are zero, a special routine is used to avoid
    ' division by zero   
    
    ' *****************************************************************************
    ' *                                                                           *
    ' *                             INCLUDE & DEFINES                             * 
    ' *                                                                           *
    ' *****************************************************************************
    
    clear
    define OSC 40                ' Use HSPLL during compilation
    DEFINE INTHAND Timer_ints
    include "modedefs.bas"      ' Include serout defines
    
    define    HSER_RCSTA 90h        ' RS232-1 TX is C7, RX is C6
    define    HSER_TXSTA 24h
    define    HSER_BAUD 19200
    define    HSER_CLROERR 1
    
    ' *****************************************************************************
    ' *                                                                           *
    ' *                                 VARIABLES                                 * 
    ' *                                                                           *
    ' *****************************************************************************
    
    T0_delay        var word    BankA   System      ' Subtracted from 65,535 in asm
    T1_delay        var word    BankA   System      ' Subtracted from 65,535 in asm
    X_steps         var word    BankA   System      ' Steps to move = Demand - Current
    Y_steps         var word    BankA   System      ' Steps to move = Demand - Current
    lcount          var byte    BankA   System      ' Delay to make pulse out
    
    X_position      var word            ' Current X position
    Y_Position      var word            ' Current Y position
    X_demand        var word            ' Demanded X position
    Y_demand        var word            ' Demanded Y position
    
    Temp_delay      var long            ' Holds temporaty pulse period calculations
    Prescale        var word            ' Timer0/1 prescale to get long delays
    
    Slug            con 8192            ' Delay = 8192[Tcy]. 2^13. 1rev = 2.62S
    Slow            con 4096            ' Delay = 4096[Tcy]. 2^12. 1rev = 1.31S
    Fast            con 2048            ' Delay = 2048[Tcy]. 2^11. 1rev = 0.65S
    Race            con 1024            ' Delay = 1024[Tcy]. 2^10. 1rev = 0.33S
    Speed           var word            ' Selected before issuing D_move command
       
    X_drive         var PORTD.0
    X_dir           var PORTD.1         ' Low = move right, high = move left
    X_ena           var PORTD.2         ' High = enable, low = disable
    Y_drive         var PORTD.3
    Y_dir           var PORTD.4         ' Low = move up, high = move down
    Y_ena           var PORTD.5         ' High = enable, low = disable
    
    Heartbeat       var PORTH.0         ' Green LED on MCU board
    
    ' *****************************************************************************
    ' *                                                                           *
    ' *                                INITIALISE                                 * 
    ' *                                                                           *
    ' *****************************************************************************
    
    ADCON1    = %00001101                 ' A0, A1 analog, rest digital
    CMCON    = %00000111                 ' Comparators off, this frees up PORTF
                                    
    TRISC    = %10000000                 ' C0-C3 is output, C6 is HSER Tx, C7 is HSER Rx
    TRISD   = %11000000                 ' D0-D5 is stepper out, D6,D7 are HOME inputs
    
    Speed = Slow                        ' 409.6uS pulses
    goto Main                           ' Skip over ASM code
    
    ' *****************************************************************************
    ' *                                                                           *
    ' *                              ASSEMBLER CODE                               * 
    ' *                                                                           *
    ' *****************************************************************************
    ASM
    Timer_ints
       bcf    INTCON,7              ; Disable all interrupts
       btfss  INTCON,TMR0IF         ; Is Timer0 interrupt flag set?
       bra    Check_T1              ; No so branch to Timer1 interrupt  
       movff  T0_delay+1,TMR0H      ; Load Timer0 with delay_hb first
       movff  T0_delay,TMR0L        ; Load Timer0 with delay_lb last
       bcf    INTCON,TMR0IF         ; Clear Timer0 interrupt flag
           
       bsf    PORTD,3               ; T0 is always Y axis
       call   Short_pulse           ; Make output pulse
       bcf    PORTD,3               ; Low Y drive
       
       movf   Y_steps,f             ; Update STATUS register
       btfsc  STATUS,Z              ; Is low byte zero?
       decf   Y_steps+1,f           ; Yes so decrement high byte
       decf   Y_steps,f             ; Decrament low byte
              
    Check_T1 
       btfss  PIR1,TMR1IF           ; Is Timer1 interrupt flag set?
       bra    Int_exit              ; No so branch to end of ISR
       movff  T1_delay+1,TMR1H      ; Load Timer0 with delay_hb first
       movff  T1_delay,TMR1L        ; Load Timer0 with delay_lb last
       bcf    PIR1,TMR1IF           ; Clear Timer1 interrupt flag
                       
       bsf    PORTD,0               ; T1 is always X axis
       call   Short_pulse           ; Make output pulse
       bcf    PORTD,0               ; low X drive
      
       movf   X_steps,f             ; Update STATUS register
       btfsc  STATUS,Z              ; Is low byte zero?
       decf   X_steps+1,f           ; Yes so decrement high byte
       decf   X_steps,f             ; Decrament low byte   
             
    Int_exit   
       bsf    INTCON,7              ; Enable all interrupts
       retfie FAST                  ; Return automatic restore
       
    ;  Pulse out subroutine 
    Short_pulse                     ; Delay = 3.n+7 [0.1uS]
       movlw  0x62                  ; $21 = 10uS, $62 = 30uS, $ff = 77.2uS
       movwf  lcount
    Pulse_loop
       decfsz lcount                ; Decrement the file Count
       goto   Pulse_loop            ; Loop if not zero
       return
    ;*************************************************************************
    ;*                                 MAIN ASM                              *
    ;*************************************************************************
    ENDASM
    
    ASM
    _Main_ASM 
       comf   T0_delay+1       ; So that the computed delay
       comf   T0_delay         ; does not have to be subtracted
       comf   T1_delay+1       ; from 65,535, the overflow
       comf   T1_delay         ; of Timer0 and Timer1 in 16 bit mode     
    ; Set up interrupt conditions                          
       bcf    RCON,7           ; Disable priority levels, IPEN=0
       bsf    INTCON,7         ; Enable all unmasked interrupts    
       bsf    INTCON,6         ; Enable all unmasked peripheral interrupts
       bcf    INTCON,3         ; Disable PORTB interrupts
       bcf    INTCON2,2        ; Timer0 overflow low priority
       bcf      IPR1,0           ; Timer1 overflow low priority                     
    ; Load timers, T0CON and T1CON set in PBP
       movff  T0_delay+1,TMR0H ; Load Timer0 with delay_hb first
       movff  T0_delay,TMR0L   ; Load Timer0 with delay_lb after hi byte
       movff  T1_delay+1,TMR1H ; Load Timer1 with delay_hb first
       movff  T1_delay,TMR1L   ; Load Timer1 with delay_lb after hi byte
       bcf    INTCON,TMR0IF    ; Timer0 clear interrupt flag
       bsf    INTCON,TMR0IE    ; Timer0 enable interrupt   
       bcf    PIR1,TMR1IF      ; Timer1 clear interrupt flag
       bsf      PIE1,TMR1IE       ; Timer1 enable interrupt
                       
    Main_loop   
       tstfsz X_steps+1        ; Is high byte zero?
       goto   Test_Y           ; Not zero so test Y_steps
       tstfsz X_steps          ; Is low byte zero?
       goto   Test_Y           ; Not zero so test Y_steps
       bcf      PIE1,TMR1IE      ; Is zero so disable Timer1 interrupt
       bcf    PIR1,TMR1IF      ; Is zero so clear Timer1 flag
       movlw  b'10000000'      ; Stop timer1 running
       movwf  T1CON            ; Kill Timer1 now  
    Test_Y
       tstfsz Y_steps+1        ; Is high byte zero?
       goto   Main_loop        ; Not zero so keep running
       tstfsz Y_steps          ; Is low byte zero?
       goto   Main_loop        ; Not zero so do it all again
       bcf    INTCON,TMR0IE    ; Timer0 disable interrupt
       bcf    INTCON,TMR0IF    ; Timer0 clear interrupt flag
       movlw  b'00000000'      ; Stop Timer0 running
       movwf  T0CON            ; Kill timer0 now
          
       btfsc  PIE1,TMR1IE      ; Has Timer1 interrupt been killed?
       goto   Main_loop        ; Not zero so keep running
     
    All_done
       bcf    INTCON,7         ; Disable all interrupts   
       return
    ENDASM
    
    ' *****************************************************************************
    ' *                                                                           *
    ' *                                   MAIN                                    * 
    ' *                                                                           *
    ' *****************************************************************************
    
    Main:
        Speed = Slow
        gosub RS232_input
        gosub D_move  
        toggle Heartbeat
        pause 100
        goto Main  
        
    ' *****************************************************************************
    ' *                                                                           *
    ' *  DIAGONAL MOVE. Move to X_demand, Y_demand and update X/Y_position        * 
    ' *                                                                           *
    ' *****************************************************************************    
    
    D_move:    
        if X_demand > 32000 then X_demand = 32000     ' X upper limit
        IF Y_demand > 16000 THEN Y_demand = 16000     ' Y upper limit     
        
        if X_demand>=X_position then
            low X_dir                                 ' Move right
            X_Steps = X_demand - X_position
            Else
            High X_dir                                ' Move left
            X_Steps = X_position - X_demand
        endif
        if Y_demand>=Y_position then
            low Y_dir                                 ' Move up
            Y_Steps = Y_demand - Y_position
            ELSE
            HIGH Y_dir                                ' Move down
            Y_Steps = Y_position - Y_demand
        ENDIF 
        
        if X_steps = 0 then                 ' Only Y_steps, only Timer0 needed
            T0_delay = Speed                ' Timer0 delay is fixed
            T0CON = %10001000               ' Timer0 on, 16 bit, prescale=1
            T1CON = %10000000               ' Timer1 off
            goto D_move_done                ' No X steps to do so end
        endif
    
        IF Y_steps = 0 then                 ' Only X_steps, only Timer1 needed
            T1_delay = Speed                ' Timer1 delay is fixed
            T1CON = %10000001               ' Timer1 on, 16 bit, prescale=1
            T0CON = %00001000               ' Timer0 off       
            goto D_move_done                ' No Y steps to do so end
        endif
        
        if X_steps >= Y_steps then
            T1_delay = Speed                ' Timer1 delay is fixed    
            T1CON = %10000001               ' Timer1 on, 16 bit, prescale=1
            Temp_delay = X_steps*Speed      ' Calculate Timer0 delay
            Temp_delay = Temp_delay/Y_steps ' Delay needed without prescale in [Tcy]
            gosub Get_T0_prescale           ' Now divide delay by prescale
            goto D_move_done
        else
            T0_delay = Speed                ' Timer0 delay is fixed
            T0CON = %10001000               ' Timer0 on, 16 bit, prescale=1 
            Temp_delay = Y_steps*Speed      ' Calculate Timer1 delay
            Temp_delay = Temp_delay/X_steps ' Delay needed without prescale in [Tcy]
            gosub Get_T1_prescale           ' Now divide delay by prescale
            goto D_move_done
        endif
    
    D_move_done:
        CALL Main_ASM                       ' Make the pulses
        X_position=X_demand                 ' Move done so position = demand
        Y_position=Y_demand                 ' Move done so position = demand 
        return  
    
    ' *****************************************************************************
    ' *                                                                           *
    ' *                                SUB-ROUTINES                               * 
    ' *                                                                           *
    ' *****************************************************************************
        
    Get_T0_Prescale:
        select case Temp_delay
            case is > 8388480
                T0CON = %10000111
                Prescale = 8            ; Divide by 2.2.2.2.2.2.2.2
            case is > 4194240
                T0CON = %10000110
                Prescale = 7            ; Divide by 2.2.2.2.2.2.2
            case is > 2097120
                T0CON = %10000101
                Prescale = 6            ; Divide by 2.2.2.2.2.2
            case is > 1048560
                T0CON = %10000100
                Prescale = 5            ; Divide by 2.2.2.2.2
            case is > 524280
                T0CON = %10000011
                Prescale = 4            ; Divide by 2.2.2.2    
            case is > 262140
                T0CON = %10000010           
                Prescale = 3            ; Divide by 2.2.2             
            case is > 131070
                T0CON = %10000001            
                Prescale = 2            ; Divide by 2.2                 
            case is > 65535
                T0CON = %10000000           
                Prescale = 1            ; Divide by 2
            case else    
                T0CON = %10001000       
                Prescale = 0            ; Divide by 1
        end select                
            Temp_delay = Temp_delay >> Prescale
            T0_delay = Temp_delay.word0                             
        return
        
    Get_T1_Prescale:
        select case Temp_delay                       
            case is > 262140
                T1CON = %10110001            
                Prescale = 3            ; Divide by 2.2.2              
            case is > 131070
                T1CON = %10100001           
                Prescale = 2            ; Divide by 2.2                 
            case is > 65535
                T1CON = %10010001           
                Prescale = 1            ; Divide by 2
            case else    
                T1CON = %10000001      
                Prescale = 0            ; Divide by 1
        end select               
            Temp_delay = Temp_delay >> Prescale
            T1_delay = Temp_delay.word0                  
        return
        
    ' *****************************************************************************
    ' *                                                                           *
    ' *  RS232 INPUT. Get X_demand, Y_demand from terminal                        * 
    ' *                                                                           *
    ' *****************************************************************************
    
    RS232_input:
        hserout ["X-demand 0 to 32000",13]
        hserout ["Y_demand 0 to 16000",13]
        hserout ["Speed 2000(fast) to 8000(slow)",13]      
        hserout ["Enter X_demand, Y_demand, Speed",13]
        hserin [dec X_demand,dec Y_demand,dec Speed]            
        hserout ["You entered ",dec X_demand," ",dec Y_demand," ",dec Speed,13]   
        RETURN
    
        end
    This article was originally published in forum thread: Driving two stepper motors started by Bill Legge View original post