Driving two stepper motors

[Bill Legge]

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

' *****************************************************************************
' *                                                                           *
' * 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

[tenaja]

Bill, that's very good looking. I wrote a similar program in Proton. I made one timer a high priority interrupt, and the other low. Obviously not necessary but since I wanted one axis more accurate than the other, the one always got priority.

If you ever decide to try to squeeze more speed out of it, toggle the Step pin high (your Drive pin) immediately at the top of the interrupt axis section. Then at the bottom of the interrupt, toggle it back. (My motor drive moved when the pin went high.) What that will do is allow you to go through the interrupt without the delay loop. Most drives will have a spec for the minimum pulse width, and the rest of the int routine took longer than that requirement on my drives. If you implement this, then your motion can be more consistent. (But maybe that doesn't matter for your application.)

[Bill Legge]

Tenaja,

Thanks for your comment. That's a good idea - I presume toggling the drive pins will be OK as long as they stay high for the minimum time for my driver - 5uS.

I'll give it a try.

What I want to do next is incorporate a ramp-up/down (for constant accelleration) but can't see how to do it with both motors?

I've done it with one motor OK:
1. Using a list of pulse times held in EEPROM, as in David Benson,s book 'Easy Step'n'.
2. Using a maths routine to calculate the periods - too slow in PBP.

Any ideas how to do it with two motors?

Regards Bill Legge

[tenaja]

Well, to get true linear accel, you either need a long lookup table or a complicated calculation--just as you've already worked out.

I've tried a calculation (look on embedded.com for a few acceleration formulas), and it takes up to 105uS to do it in Proton, for each pass through the calc (on a 40MHz 18F). PBP would be similar, or maybe a couple uS slower. Either way, you are limited to about 9khz for a single axis or 4.5 for two, but you'll have to calculate them out of the interrupt so they are prepped for use inside it. (You will also have to deal with context saving, which you short-cut on your posted code.) I've considered buffering the calculations in an array so when the motors are going slow you can calculate a lot of values so when it is going fast you have some done ahead of time. You'd be limited by RAM, though.

Since I was more concerned about simplicity and overall travel than smooth motion, I did it a little different. I used a delay scale, then by trial & error came up with a few Case statements that shifted it left (increasing the delay offset) or right (reducing the offset) depending on the current speed. I just tuned it by ear (literally), listening for the motor accel to sound smooth. The problem with adding/subtracting a constant to the time delay is that at higher speeds the change is drastic but at slower speeds the same change is hardly noticeable. That's why I did the select/case to shift it. You end up with stepped acceleration, but it sounded smooth and worked for me.

I wouldn't do it for cnc, though. For CNC you need one of the two previously mentioned accel methods.

[tenaja]

Oh, yeah... if your drive only needs a 5uS pulse, you should minimize the delay in the interrupt, or use it to check for a second interrupt. At 40MHz that is 50 instructions minus the call/return, and you could exit early if you were going to handle the other motor stepping.

[Bill Legge]

Tanaja,

Yes, I've seen the embedded.com article by David Austin. There is also a good application note on the ATMEL site (Application note AVR446) that has rampup/down calculation:

Cn = Cn_1 - 2*Cn_1/(4n+1)

In the C language - I have yet to see if PBP is fast enough to do this.

Was the formula you used like the one above or was it the more complicated 'exact' calculation that needs square roots? If it takes 105uS that's in the ball park for my application.

Re 'context saving' - if you mean 'wsave' and so on - I don't think it's necessary on the PIC18F8722?

Regards Bill Legge

[tenaja]

Yes, I've seen the embedded.com article by David Austin. There is also a good application note on the ATMEL site (Application note AVR446) that has rampup/down calculation:
Cn = Cn_1 - 2*Cn_1/(4n+1)

In the C language - I have yet to see if PBP is fast enough to do this.

Was the formula you used like the one above or was it the more complicated 'exact' calculation that needs square roots? If it takes 105uS that's in the ball park for my application.

1. The concept that Basic is slow and C is fast is a remnant of the 70's when there were no basic compilers, just interpreters. Although it has bee nreduced to a myth that won't let go, in reality it is now all about which compiler author wrote efficient code--not which language he wrote. Recently someone posted that PBP compiled some code in 460 words, but MikroB wouldn't even fit in the chip. My guess would be that the MikroC would be just as bad, unless they had two compiler authors and one was a lousy coder. BTW, in short code, PBP and Proton are similar... but when it gets long, or you use a mix of Words and Longs, Proton really shines. (And when using interrupts.)
2. That's the article and formula I was talking about. without much tweaking, it was running at the speed I previously quoted. It takes around 400 instructions to do a 32 bit sqrrt on a PIC... only good for buffered or slow moves.

Re 'context saving' - if you mean 'wsave' and so on - I don't think it's necessary on the PIC18F8722?

The 18F does save the wreg if you use the Fast Return, but not the variables you are changing and the registers related to them. For instance, if you are in the middle of changing an array value and an interrupt occurs, and you access an array (any array) in the interrupt, you have to save the INDF & FSR registers and anything else PBP uses to manipulate arrays. Proton has this built in, and many PBP users have used the DT Instant Interrupts... which is kind of an oxymoron since all of the saving removes the instant part.