Simple Source Code

Generating Pulse Trains for Step/Direction Motor Drivers

Simple Source Code

Postby vegipete » Fri Aug 12, 2011 11:40 am

I've decided the easiest way to present the source code I have created is to post it here in this forum. That way it is easy for others to see the parts and comment on or question what they read.

It should be easy for others to copy the source code sections into an MPLAB project.

This simple source code example uses two files, the processor configuration file and the source code itself. I set my projects up very simply and I use an _absolute_ memory model. No doubt some might object to the use of absolute mode, saying that relative mode is the correct way to do things, yada, yada, yada, but I prefer to control where all the pieces go myself.
/vp
vegipete
 
Posts: 37
Joined: Sat Jul 09, 2011 7:38 pm

Simple Source Code

Sponsor

Sponsor
 

Re: Simple Source Code

Postby vegipete » Mon Aug 15, 2011 1:19 pm

EDIT: There are some bugs in the sample code below - single step moves fail and slow speed moves can fail. The failed moves become huge!
I'll put corrected code in a new topic shortly...

=========================================

Here's the processor configuration file:
Code: Select all
;==========================================================================
;
;   IMPORTANT: For the PIC18 devices, the __CONFIG directive has been
;              superseded by the CONFIG directive.  The following settings
;              are available for this device.
;
;   Oscillator Selection bits:
;     OSC = LP             LP oscillator
;     OSC = XT             XT oscillator
;     OSC = HS             HS oscillator
;     OSC = RC             External RC oscillator, CLKO function on RA6
;     OSC = EC             EC oscillator, CLKO function on RA6
;     OSC = ECIO6          EC oscillator, port function on RA6
 CONFIG   OSC = HSPLL   ;HS oscillator, PLL enabled (Clock Frequency = 4 x FOSC1)
;     OSC = RCIO6          External RC oscillator, port function on RA6
;     OSC = INTIO67        Internal oscillator block, port function on RA6 and RA7
;     OSC = INTIO7         Internal oscillator block, CLKO function on RA6, port function on RA7
;
;   Fail-Safe Clock Monitor Enable bit:
 CONFIG   FCMEN = OFF   ;Fail-Safe Clock Monitor disabled
;     FCMEN = ON           Fail-Safe Clock Monitor enabled
;
;   Internal/External Oscillator Switchover bit:
 CONFIG   IESO = OFF   ;Oscillator Switchover mode disabled
;     IESO = ON            Oscillator Switchover mode enabled
;
;   Power-up Timer Enable bit:
;     PWRT = ON            PWRT enabled
 CONFIG   PWRT = OFF   ;PWRT disabled
;
;   Brown-out Reset Enable bits:
 CONFIG   BOREN = OFF   ;Brown-out Reset disabled in hardware and software
;     BOREN = ON           Brown-out Reset enabled and controlled by software (SBOREN is enabled)
;     BOREN = NOSLP        Brown-out Reset enabled in hardware only and disabled in Sleep mode (SBOREN is disabled)
;     BOREN = SBORDIS      Brown-out Reset enabled in hardware only (SBOREN is disabled)
;
;   Brown-out Reset Voltage bits:
;     BORV = 0             Maximum setting
 CONFIG   BORV = 1             
;     BORV = 2             
;     BORV = 3             Minimum setting
;
;   Watchdog Timer Enable bit:
 CONFIG   WDT = OFF   ;WDT disabled (control is placed on the SWDTEN bit)
;     WDT = ON             WDT enabled
;
;   Watchdog Timer Postscale Select bits:
;     WDTPS = 1            1:1
;     WDTPS = 2            1:2
;     WDTPS = 4            1:4
;     WDTPS = 8            1:8
;     WDTPS = 16           1:16
;     WDTPS = 32           1:32
;     WDTPS = 64           1:64
 CONFIG   WDTPS = 128   ;1:128
;     WDTPS = 256          1:256
;     WDTPS = 512          1:512
;     WDTPS = 1024         1:1024
;     WDTPS = 2048         1:2048
;     WDTPS = 4096         1:4096
;     WDTPS = 8192         1:8192
;     WDTPS = 16384        1:16384
;     WDTPS = 32768        1:32768
;
;   MCLR Pin Enable bit:
;     MCLRE = OFF          RE3 input pin enabled; MCLR disabled
 CONFIG   MCLRE = ON   ;MCLR pin enabled; RE3 input pin disabled
;
;   Low-Power Timer1 Oscillator Enable bit:
 CONFIG   LPT1OSC = OFF   ;Timer1 configured for higher power operation
;     LPT1OSC = ON         Timer1 configured for low-power operation
;
;   PORTB A/D Enable bit:
 CONFIG   PBADEN = OFF   ;PORTB<4:0> pins are configured as digital I/O on Reset
;     PBADEN = ON          PORTB<4:0> pins are configured as analog input channels on Reset
;
;   CCP2 MUX bit:
;     CCP2MX = PORTBE      CCP2 input/output is multiplexed with RB3
 CONFIG   CCP2MX = PORTC   ;CCP2 input/output is multiplexed with RC1
;
;   Stack Full/Underflow Reset Enable bit:
 CONFIG   STVREN = OFF   ;Stack full/underflow will not cause Reset
;     STVREN = ON          Stack full/underflow will cause Reset
;
;   Single-Supply ICSP Enable bit:
 CONFIG   LVP = OFF   ;Single-Supply ICSP disabled
;     LVP = ON             Single-Supply ICSP enabled
;
;   Extended Instruction Set Enable bit:
 CONFIG   XINST = OFF   ;Instruction set extension and Indexed Addressing mode disabled (Legacy mode)
;   XINST = ON   ;Instruction set extension and Indexed Addressing mode enabled
;
;   Background Debugger Enable bit:
 CONFIG   DEBUG = ON   ;Background debugger enabled, RB6 and RB7 are dedicated to In-Circuit Debug
;     DEBUG = OFF          Background debugger disabled, RB6 and RB7 configured as general purpose I/O pins
;
;   Code Protection bit Block 0:
;     CP0 = ON             Block 0 (000800-001FFFh) code-protected
 CONFIG   CP0 = OFF   ;Block 0 (000800-001FFFh) not code-protected
;
;   Code Protection bit Block 1:
;     CP1 = ON             Block 1 (002000-003FFFh) code-protected
 CONFIG   CP1 = OFF   ;Block 1 (002000-003FFFh) not code-protected
;
;   Code Protection bit Block 2:
;     CP2 = ON             Block 2 (004000-005FFFh) code-protected
 CONFIG   CP2 = OFF   ;Block 2 (004000-005FFFh) not code-protected
;
;   Code Protection bit Block 3:
;     CP3 = ON             Block 3 (006000-007FFFh) code-protected
 CONFIG   CP3 = OFF   ;Block 3 (006000-007FFFh) not code-protected
;
;   Boot Block Code Protection bit:
;     CPB = ON             Boot block (000000-0007FFh) code-protected
 CONFIG   CPB = OFF   ;Boot block (000000-0007FFh) not code-protected
;
;   Data EEPROM Code Protection bit:
;     CPD = ON             Data EEPROM code-protected
 CONFIG   CPD = OFF   ;Data EEPROM not code-protected
;
;   Write Protection bit Block 0:
;     WRT0 = ON            Block 0 (000800-001FFFh) write-protected
 CONFIG   WRT0 = OFF   ;Block 0 (000800-001FFFh) not write-protected
;
;   Write Protection bit Block 1:
;     WRT1 = ON            Block 1 (002000-003FFFh) write-protected
 CONFIG   WRT1 = OFF   ;Block 1 (002000-003FFFh) not write-protected
;
;   Write Protection bit Block 2:
;     WRT2 = ON            Block 2 (004000-005FFFh) write-protected
 CONFIG   WRT2 = OFF   ;Block 2 (004000-005FFFh) not write-protected
;
;   Write Protection bit Block 3:
;     WRT3 = ON            Block 3 (006000-007FFFh) write-protected
 CONFIG   WRT3 = OFF   ;Block 3 (006000-007FFFh) not write-protected
;
;   Boot Block Write Protection bit:
;     WRTB = ON            Boot block (000000-0007FFh) write-protected
 CONFIG   WRTB = OFF   ;Boot block (000000-0007FFh) not write-protected
;
;   Configuration Register Write Protection bit:
;     WRTC = ON            Configuration registers (300000-3000FFh) write-protected
 CONFIG   WRTC = OFF   ;Configuration registers (300000-3000FFh) not write-protected
;
;   Data EEPROM Write Protection bit:
;     WRTD = ON            Data EEPROM write-protected
 CONFIG   WRTD = OFF   ;Data EEPROM not write-protected
;
;   Table Read Protection bit Block 0:
;     EBTR0 = ON           Block 0 (000800-001FFFh) protected from table reads executed in other blocks
 CONFIG   EBTR0 = OFF   ;Block 0 (000800-001FFFh) not protected from table reads executed in other blocks
;
;   Table Read Protection bit Block 1:
;     EBTR1 = ON           Block 1 (002000-003FFFh) protected from table reads executed in other blocks
 CONFIG   EBTR1 = OFF   ;Block 1 (002000-003FFFh) not protected from table reads executed in other blocks
;
;   Table Read Protection bit Block 2:
;     EBTR2 = ON           Block 2 (004000-005FFFh) protected from table reads executed in other blocks
 CONFIG   EBTR2 = OFF   ;Block 2 (004000-005FFFh) not protected from table reads executed in other blocks
;
;   Table Read Protection bit Block 3:
;     EBTR3 = ON           Block 3 (006000-007FFFh) protected from table reads executed in other blocks
 CONFIG   EBTR3 = OFF   ;Block 3 (006000-007FFFh) not protected from table reads executed in other blocks
;
;   Boot Block Table Read Protection bit:
;     EBTRB = ON           Boot block (000000-0007FFh) protected from table reads executed in other blocks
 CONFIG   EBTRB = OFF   ;Boot block (000000-0007FFh) not protected from table reads executed in other blocks
;
;==========================================================================
;
;       Configuration Bits
;
;   NAME            Address
;   CONFIG1H        300001h
;   CONFIG2L        300002h
;   CONFIG2H        300003h
;   CONFIG3H        300005h
;   CONFIG4L        300006h
;   CONFIG5L        300008h
;   CONFIG5H        300009h
;   CONFIG6L        30000Ah
;   CONFIG6H        30000Bh
;   CONFIG7L        30000Ch
;   CONFIG7H        30000Dh
;
;==========================================================================

And here is the actual source code:
Code: Select all
;****************************************************************
;   RampStepPIC                                                 *
;   Generating ramped stepper signals in software               *
;   Hardware:                                                   *
;   10MHz crystal with 4x PLL enabled                           *
;   RA0 -
;   RA1 -
;   RA2 -
;   RA3 -
;   RA4 -
;   RA5 -
;
;   RB0 - RB3 - LCD DATA
;   RB4 - LCD E - toggle hi to latch data
;   RB5 - LCD R/S - lo for ctrl, hi for data
;   RB6 - RB7 - ICD
;
;   RC0 - Direction
;   RC1 - LED  - (CCP2) - PWM1
;   RC2 - Step - (CCP1)
;   RC3 -
;   RC4 -
;   RC6 -
;   RC7 -
;                                                                                          
;****************************************************************
;Resources Used:                                                *
;   Timer2      Periodic Interrupt                              *
;****************************************************************

   LIST   P=18F2520   ;directive to define processor
   #include <P18F2520.INC>   ;processor specific variable definitions
   radix   dec

   #include <P18F2520_CONFIG.INC>   ;set config registers

#define   LED_ON      bsf   PORTC,1
#define   LED_OFF      bcf   PORTC,1

#define   BUTTON_P   PORTA,4

;Stepper interface definitions
#define   STEP_PIN   PORTC,2
#define   STEP_HI      bsf   PORTC,2
#define   STEP_LO      bcf   PORTC,2

#define DIR_PIN      PORTC,0

;LCD Port Definitions
#define   EN_ON   bsf   LATB,4
#define   EN_OFF   bcf   LATB,4
#define   LCDctrl   bcf   LATB,5      ;lo = access control regs
#define   LCDdata   bsf   LATB,5      ;hi = access data memory

#define clc      bcf   STATUS,C
#define sec      bsf   STATUS,C

;
;   _Print   macro
;   _Print   " Menu items\n\n\r"
_Print   MACRO   str      ; print in-line string macro
   call   PutString   ;
   db   str,0
   ENDM
;

;step_state flag bits
;step_state is primed with the value b'00010001'
;then a simple rrncf rotates to the next state
IDLE      equ   0   ;no move currently active
DECEL      equ   1   ;approaching end of move
MAX_V      equ   2   ;moving at full speed
ACCEL      equ   3   ;move starting

;******************************************************************************
;Variable definitions
; These variables are only needed if low priority interrupts are used.
; More variables may be needed to store other special function registers used
; in the interrupt routines.
;
;   CBLOCK   0x080
;   WREG_TEMP   ;variable used for context saving
;   STATUS_TEMP   ;variable used for context saving
;   BSR_TEMP   ;variable used for context saving
;   PRODL_TEMP
;   PRODH_TEMP
;   FSR0_TEMP
;   FSR0_ISR   ;save value from one interupt to the next
;   ENDC

;step ramp control variables
;the first 3 variables should be set by the calling routine.
;step_middle will be calculated by the call to Start_Move
   CBLOCK   0x000
   step_move:4   ;total move requested
   step_spmax:2   ;maximum speed
   step_accel:2   ;accel/decel rate, 8.8 bit format

   step_middle:4   ;mid-point of move, = (step_move - 1) >> 1
   
   step_dur:1   ;counter for duration of step pulse HI
   step_count:4   ;step counter
   step_frac:2   ;step counter fraction
   step_speed:4   ;current speed, 16.8 bit format (hi byte always 00)
   step_state:1   ;move profile state
   step_flags:1   ;any useful flags
   ENDC

M_MID   EQU   0   ;a flag! set when step_count = step_middle

;general (non isr) variables
   CBLOCK
   pausecount:2   ;counters for pause routines
   temp00:1
   LCDtemp:1
   count:1
   temp:1
   result:4
   L_byte
   H_byte
   U_byte
   ENDC

   CBLOCK   0x100
   ENDC

;******************************************************************************
;EEPROM data
; Data to be programmed into the Data EEPROM is defined here

   ORG   0xf00000

   DE   "Test Data",0,1,2,3,4,5

;******************************************************************************
;Reset vector
; This code will start executing when a reset occurs.

   ORG   0x0000     ;start address 0
   nop         ;maybe for icd
   nop         ;maybe for icd
;   clrf   INTCON      ;make sure interupts are off
   goto   Main      ;go to start of main code

;******************************************************************************
;High priority interrupt vector
; This code will start executing when a high priority interrupt occurs or
; when any interrupt occurs if interrupt priorities are not enabled.

   ORG   0x0008

;   bra   HighInt      ;go to high priority interrupt routine

;******************************************************************************
;Low priority interrupt vector and routine
; This code will start executing when a low priority interrupt occurs.
; This code can be removed if low priority interrupts are not used.
;
;   ORG   0x0018
;
;   movff   STATUS,STATUS_TEMP   ;save STATUS register
;   movff   WREG,WREG_TEMP      ;save working register
;   movff   BSR,BSR_TEMP      ;save BSR register
;
;   *** low priority interrupt code goes here ***
;
;   movff   BSR_TEMP,BSR      ;restore BSR register
;   movff   WREG_TEMP,WREG      ;restore working register
;   movff   STATUS_TEMP,STATUS   ;restore STATUS register
;   retfie

;******************************************************************************
;High priority interrupt routine

HighInt:
   movlb   0
   btfss   PIR1,TMR2IF   ;test for T2 match interrupt
   bra   unknown_int   ;jump if not T2 interrupt

;******************************************************************************
;process step pulse train
;1) Interrupt housekeeping
;2) Step pulse duration control
;Perform the following if motion is currently active
;3) Calculate next time slice, generate step pulse if needed
;4) Adjust stepping speed if needed
T2_int
   bcf   PIR1,TMR2IF   ;clear T2 interrupt
   LED_ON
   
;2) Step pulse duration control
;Since the max step rate is 1/2 the interrupt rate, set the step pulse
;HI duration to 1/2 of this max step rate - ie 1 interrupt period
   bcf   STATUS,C
   rrcf   step_dur,f
   btfsc   STATUS,C
   STEP_HI
   btfss   STATUS,C
   STEP_LO

   btfsc   step_state,IDLE   ;are things in motion?
   bra   T2_Int_Done   ;no - we're done

;3) Calculate next time slice, generate step pulse if needed
;Calculate {step_frac += step_speed + 1} (16 bit values)
;If this overflows, it's time for the next step pulse
;Note: if we're at maximum speed, calculate {step_frac += step_spmax + 1} instead
   bsf   STATUS,C   ; + 1 of above equation
   movf   step_speed+1,w   ;grab speed lo byte
   btfsc   step_state,MAX_V
   movf   step_spmax+0,w   ;get max speed instead
   addwfc   step_frac+0,f   ;add to position fraction lo
   movf   step_speed+2,w   ;grab hi byte
   btfsc   step_state,MAX_V
   movf   step_spmax+1,w   ;get max speed instead
   addwfc   step_frac+1,f   ;add to position fraction hi
   bnc   TimeSlice_done   ;no carry so no step pulse yet

   incfsz   step_count+0,f   ;increment 32 bit step counter
   bra   Test_Step_Mid
   incfsz   step_count+1,f
   bra   Test_Step_Mid
   infsnz   step_count+2,f   ;does NOT handle 32 bit overflow gracefully
   incf   step_count+3,f   ; but max move requested is <=32 bits so ...

;check if the mid-point (or decel-start-point or end) has been reached
Test_Step_Mid
   bcf   step_flags,M_MID   ;assume not at middle to begin

   movf   step_count+3,w
   cpfseq   step_middle+3
   bra   New_Step_Pulse

   movf   step_count+2,w
   cpfseq   step_middle+2
   bra   New_Step_Pulse

   movf   step_count+1,w
   cpfseq   step_middle+1
   bra   New_Step_Pulse

   movf   step_count+0,w
   cpfseq   step_middle+0      ;too bad there's no 'cpfsne'
   bra   New_Step_Pulse
   bsf   step_flags,M_MID   ;flag mid point for later calcs
   
New_Step_Pulse
;   movlw   b'11'      ;request step pulse HI for 2 interrupts,
   movlw   b'01'      ;request step pulse HI for 1 interrupt,
   movwf   step_dur   ;  starting next interrupt
   
TimeSlice_done

;4) Adjust stepping speed if needed
;There are 3 conditions:
; 1) if we are accelerating, increase the stepping speed,
;   if we hit the midpoint, switch to decelerating
;   if we hit max speed, switch to at-max
; 2) if we are at max speed, make no change to speed
;   if we are approaching the end, switch to decelerating
; 3) if we are decelerating, decrease the stepping speed,
;   if we hit the end, stop

   btfss   step_state,ACCEL   ;check for accel state
   bra   state_max_speed      ;branch if not
;acceleration ramp
;test if we've hit the midpoint during acceleration phase
   btfss   step_flags,M_MID
   bra   accel_not_mid
   bcf   step_flags,M_MID   ;clear the flag
;we've hit the middle, time to start decelerating
;must attend to even/odd number of steps
   rrncf   step_state,f   ;switch to max speed state
   rrncf   step_state,f   ;switch to deceleration state
   bra   load_end   ;set up end condition

;******************************************************************************
; tuck this in here to remain within range of branch at ISR start
unknown_int
   bra   unknown_int   ;Oh Dear! Unexpected interrupt
;******************************************************************************

accel_not_mid
;still accelerating, not yet at midpoint
;so increase speed and test for max speed
   movf   step_accel+0,w   ;add 16 bit acceleration value
   addwf   step_speed+0,f   ;to 24 bit current speed value
   movf   step_accel+1,w
   addwfc   step_speed+1,f
   movlw   0
   addwfc   step_speed+2,f

   movf   step_speed+2,w   ;grab current speed hi byte
   subwf   step_spmax+1,w   ;compare to limit hi byte
   bnz   accel_test_max   ;if they are not equal, result is in N
   movf   step_speed+1,w   ;grab current speed lo byte
   subwf   step_spmax+0,w   ;compare to limit lo byte, result is in N
accel_test_max
   btfss   STATUS,N   ;N is clear if step_speed < step_spmax
   bra   T2_Int_Done
   rrncf   step_state,f   ;switch to max speed state
;calculate {step_middle = step_move - step_count} to determine
;when to start slowing down
;we can store this in step_middle because the mid point no longer matters
   movf   step_count+0,w
   subwf   step_move+0,w
   movwf   step_middle+0
   movf   step_count+1,w
   subwfb   step_move+1,w
   movwf   step_middle+1
   movf   step_count+2,w
   subwfb   step_move+2,w
   movwf   step_middle+2
   movf   step_count+3,w
   subwfb   step_move+3,w
   movwf   step_middle+3
   bra   T2_Int_Done

state_max_speed
   btfss   step_state,MAX_V   ;check for max speed state
   bra   state_decel      ;branch if not
;cruising along at max so leave step_speed unchanged
; and check if we're approaching the end
;if step_count = step_middle, it's time to decelerate
   btfss   step_flags,M_MID
   bra   T2_Int_Done
   bcf   step_flags,M_MID   ;clear the flag
   rrncf   step_state,f   ;shift to deceleration state
load_end
;copy requested move to step_middle to allow compare for end of move
   movf   step_move+0,w
   movwf   step_middle+0
   movf   step_move+1,w
   movwf   step_middle+1
   movf   step_move+2,w
   movwf   step_middle+2
   movf   step_move+3,w
   movwf   step_middle+3

   bra   T2_Int_Done

state_decel
;we're decelerating so calculate {step_speed -= step_accel}
;if we have reached the end of the move, shift to idle state
   movf   step_accel+0,w
   subwf   step_speed+0,f
   movf   step_accel+1,w
   subwfb   step_speed+1,f
   movlw   0
   subwfb   step_speed+2,f

;check if we've reached the end
   btfss   step_flags,M_MID
   bra   T2_Int_Done
   bcf   step_flags,M_MID   ;clear the flag

;step_count = step_move - we are done
   rrncf   step_state,f   ;shift to idle state
   clrf   step_speed+0
   clrf   step_speed+1
   clrf   step_speed+2
   clrf   step_speed+3   ;zero the speed just in case

T2_Int_Done
   LED_OFF
   retfie   FAST

Start_Move
;prepare variables for start of move
   clrf   step_count+0
   clrf   step_count+1
   clrf   step_count+2
   clrf   step_count+3
   
   clrf   step_frac+0
   clrf   step_frac+1

   clrf   step_speed+0
   clrf   step_speed+1
   clrf   step_speed+2
   clrf   step_speed+3

;calculate middle = move / 2
   bcf   STATUS,C
   rrcf   step_move+3,w
   movwf   step_middle+3
   rrcf   step_move+2,w
   movwf   step_middle+2
   rrcf   step_move+1,w
   movwf   step_middle+1
   rrcf   step_move+0,w
   movwf   step_middle+0

;   rrncf   step_state,f   ;shift to accel state to get things going
   movlw   b'10001000'
   movwf   step_state   ;set ACCEL state to get things going

   return
   
;******************************************************************************
;Start of main program

Main:
;initialize hardware
   clrf   LATA       ; Initialize PORTA by clearing output data latches
   clrf   ADCON0       ; turn off analog module
   movlw   B'00001111'    ; RA all digital
   movwf   ADCON1       ;
   movwf   CMCON      ; comparator off
   
   movlw   b'00010000'
   movwf   TRISA      ;Set PORTA mostly output

   clrf   LATB
   movlw   b'00000000'
   movwf   TRISB      ;Set PORTB all output - LCD

   clrf   LATC
   movlw   b'11111000'   ;enable serial port and other bits
   movwf   TRISC

;set up timer 2
   clrf   TMR2      ;clear Timer2 value
;   movlw   250      ;suitable period register value for 16kHz (8MHz clock)
;   movlw   100      ;suitable period register value for 16kHz (8MHz clock)
   movlw   40      ;suitable period register value for 16kHz (8MHz clock)
   movwf   PR2      ;set Timer2 rate
   movlw   b'00001100'   ;1:2 postscale, T2 on, 1:1 prescale
      ; .0001...   1:2 post scale
      ; .....1..   Timer 2 on
      ; ......00   1:1 prescale
      ; ......11   1:16 prescale
   movwf   T2CON      ;setup Timer2


;clear memory - bank 0
   movlw   0
   lfsr   FSR0,0      ;start of ram
clearmem
   clrf   POSTINC0
   incfsz   WREG,f
   bra   clearmem


;initialize some variable
   movlw   b'00010001'
   movwf   step_state   ;set IDLE state
   
   call   LCD_Init
   _Print   "Move   Change Curr\0"

;display count down to give driver time to get ready
   movlw   0x40+20
   call   LCD_Posn   ;move to screen position
   movlw   5
   call   LCDprintnum
   call   pause1sec
   movlw   0x40+20
   call   LCD_Posn   ;move to screen position
   movlw   4
   call   LCDprintnum
   call   pause1sec
   movlw   0x40+20
   call   LCD_Posn   ;move to screen position
   movlw   3
   call   LCDprintnum
   call   pause1sec
   movlw   0x40+20
   call   LCD_Posn   ;move to screen position
   movlw   2
   call   LCDprintnum
   call   pause1sec
   movlw   0x40+20
   call   LCD_Posn   ;move to screen position
   movlw   1
   call   LCDprintnum
   call   pause1sec
   movlw   0x40+20
   call   LCD_Posn   ;move to screen position
   movlw   0
   call   LCDprintnum
   call   pause1sec

;set up interrupts
;   bsf   RCON,IPEN   ;enable interrupt priorities
   bsf   INTCON,GIEH   ;enable (high priority) interrupts
   bsf   INTCON,PEIE   ;enable peripheral interrupts (needed if priorities off)
;   bsf   IPR1,TMR2IP   ;set Timer2 interrupt to hi priority
   bcf   PIR1,TMR2IF   ;clear any pending T2 match flag
   bsf   PIE1,TMR2IE   ;turn on Timer2 interrupt

;******************************************************************************
;*
;* End of initialization.
;* Let the coding begin!
;*
;******************************************************************************

;   step_move:4   ;total move requested
;   step_spmax:2   ;maximum speed
;   step_accel:2   ;accel/decel rate

move_loop
;set the move
   movlw   low   120000   ;2975
   movwf   step_move+0
   movlw   high   120000
   movwf   step_move+1
   movlw   upper   120000
   movwf   step_move+2
   clrf   step_move+3

;set the max speed
   movlw   low   32000
   movwf   step_spmax+0
   movlw   high   32000
   movwf   step_spmax+1

;set the acceleration
   movlw   low   150
   movwf   step_accel+0
   movlw   high   150
   movwf   step_accel+1
   
;display the total move requested
   lfsr   0,step_move+0
   movlw   0x40      ;start of second line
   call   disp_16bitBCD

   call   Start_Move   ;make the move commence

waitmove01
   rcall   Display_stuff
   btfss   step_state,IDLE   ;wait for move to finish
   bra   waitmove01

   rcall   Display_stuff
   btg   DIR_PIN      ;change direction
   movlw   0
   call   pausewms
   call   pausewms   ;pause about 1/2 a second
moveloop
   bra   move_loop

Display_stuff
;display middle of move
   lfsr   0,step_middle+0
   movlw   0x47
   call   disp_16bitBCD

;display the current position
   lfsr   0,step_count+0
   movlw   0x4E
   call   disp_16bitBCD

;display the current speed
   lfsr   0,step_speed+1
   movlw   0x40+20
;   bra   disp_16bitBCD

disp_16bitBCD
   call   LCD_Posn   ;move to screen position
   movff   POSTINC0,L_byte   ;copy source
   movff   POSTINC0,H_byte   ; to conversion working
   movff   POSTINC0,U_byte   ; to conversion working
   call   Bin_2_BCD   ;convert it to 5 BCD digits
   lfsr   0,result+0   ;point to result
   movf   POSTINC0,w   ;get digit
   call   send_byte   ;send 110 thousands
   call   send_byte
   bra   send_byte

;===================================================
;Initialize the LCD
; RB0 - LCD D4
; RB1 - LCD D5
; RB2 - LCD D6
; RB3 - LCD D7
; RB4 - LCD E
; RB5 - LCD R/S     0 for control, 1 for display
LCD_Init
   movlw   .40
   call   pausewms      ;wait for LCD after powerup

   movlw   b'00000011'   ;send 0x30
   movwf   PORTB
   nop
   nop
   EN_ON
   nop
   nop
   nop
   EN_OFF
   movlw   .20
   call   pausewms

   movlw   b'00000011'   ;send 0x30
   movwf   PORTB
   nop
   nop
   EN_ON
   nop
   nop
   nop
   EN_OFF
   movlw   .2
   call   pause100uS

   movlw   b'00000010'   ;select 4 bit interface
   movwf   PORTB
   nop
   nop
   EN_ON
   nop
   nop
   nop
   EN_OFF
   movlw   .2
   call   pause100uS

   movlw   b'00101000'   ;4 bit, 2 line, 5x7, x, x
   call   LCDcmd
   movlw   .2
   call   pause100uS

   movlw   b'00001000'   ;disp off, cursor off, blink off
   call   LCDcmd
   movlw   .2
   call   pause100uS

   movlw   b'00000001'   ;clear and home
   call   LCDcmd
   movlw   .5
   call   pausewms

   movlw   b'00000110'   ;increment and no shift
   call   LCDcmd
   movlw   .2
   call   pause100uS

   movlw   b'10000000'   ;DD RAM address 0
   call   LCDcmd
   movlw   .2
   call   pause100uS

   movlw   b'00001100'   ;disp on, cursor off, blink off
   call   LCDcmd
   movlw   .2
   call   pause100uS

   return

;===================================================
;clear the LCD and home
LCD_Home
   movlw   b'00000001'
   call   LCDcmd
   movlw   5
   goto   pausewms

;===================================================
;move the 'cursor' position on the LCD
;note that the 2nd line often starts at $40
;call with position in WREG
LCD_Posn
   iorlw   b'10000000'   ;set DD RAM address
;   call   LCDcmd

;===================================================
;send byte in w-reg to LCD as command
LCDcmd
   movwf   LCDtemp
   swapf   LCDtemp,w
   andlw   b'00001111'
   iorlw   b'00010000'   ;RS clear, E set
   movwf   PORTB
;   nop
   nop
   nop
   EN_OFF

   movf   LCDtemp,w
   andlw   b'00001111'
   iorlw   b'00010000'
   movwf   PORTB
;   nop
   nop
   nop
   EN_OFF

   movlw   1
   call   pause100uS
   movf   LCDtemp,w
   return

;===================================================
;convert lo nibble (BCD) to ASCII number and send as data
LCDprintnum
   andlw   0x0F      ;strip high nibble
   iorlw   0x30      ;convert to ascii
;===================================================
;send byte in w-reg to LCD as data
LCDprint
   movwf   LCDtemp      ;save character
   swapf   LCDtemp,w
   andlw   b'00001111'
   iorlw   b'00110000'   ;RS set, E set
   movwf   PORTB
;   nop
   nop
   nop
   nop
   EN_OFF

   movf   LCDtemp,w
   andlw   b'00001111'
   iorlw   b'00110000'
   movwf   PORTB
;   nop
   nop
   nop
   nop
   EN_OFF

   movlw   1
   call   pause100uS
   movf   LCDtemp,w   ;recover character
   return

;===================================================
;send bytes pointed to by FSR out to LCD as 2
; ascii characters. Move FSR to next byte.
; expects high order bytes lower in memory
;send_wreg changes temp00
send_wreg
   movwf   temp00
   movlw   temp00
   movwf   FSR0
   goto   send_byte
send_lword
   call   send_byte
send_3byte
   call   send_byte
send_word
   call   send_byte
send_byte
   swapf   INDF0,w
   call   send_nyb
   movf   POSTINC0,w
;   call   send_nyb
;===================================================
;convert lo nibble to an ascii character
; $0 - $f to '0' - '9','A' - 'F'  (note '9' + 8 = 'A')
send_nyb
   andlw   b'00001111'   ;strip hi bits
   addlw   -.10      ; Test to see if W < 10
   btfsc   STATUS,C   ; If yes, skip ahead
   addlw   'A'-'0'-.10   ; Add ASCII 'A', and negate next line
   addlw   '0'+.10      ; Add ASCII '0' and original 10
   goto   LCDprint

;===================================================
spout   movlw   ' '
   goto   LCDprint
;===================================================
crout   movlw   .13      ;carriage return
   call   LCDprint
   movlw   .10      ;line feed
   goto   LCDprint

;===================================================
;take 2 ascii chars pointed to by w and convert to 1 byte
;high nibble stored first
;Z should be set if ascii byte is zero
ascii2byte
   movwf   FSR0
   swapf   INDF0,w      ;swap hi nibble into result
   btfsc   POSTINC0,6   ;check if in range 'A'-'F'
   addlw   0x8F      ;add correction swapf('1'-'A'+1)
;   incf   FSR0,f
   addwf   INDF0,w      ;add lo nibble
   btfsc   POSTINC0,6   ;check if in range 'A'-'F'
   addlw   0xF9      ;add correction '9'-'A'+1
;   incf   FSR0,f
   addlw   0xCD      ;adjust final result -0x33
   return

;===================================================
;convert a 16 bit binary number to a 5 digit BCD
;Input:  H_byte, L_byte
;Output: result+2, result+1, result+0
;Used:   w-reg, count, temp, FSR
; Ex: the 16 bit binary number = FFFF
; After conversion the Decimal Number
; in result+0,result+1,result+2 = 06,55,35
;
;Bin_2_BCD
;   bcf   STATUS,0   ; clear the carry bit
;   movlw   .16
;   movwf   count
;   clrf   result+0
;   clrf   result+1
;   clrf   result+2
;loop16   rlcf   L_byte, F
;   rlcf   H_byte, F
;   rlcf   result+2, F
;   rlcf   result+1, F
;   rlcf   result+0, F
;
;   dcfsnz   count, F
;   retlw   0
;
;adjDEC   lfsr   0,result+2
;   call   adjBCD
;
;   lfsr   0,result+1
;   call   adjBCD
;
;   lfsr   0,result+0
;   call   adjBCD
;
;   goto   loop16
;===================================================
;convert a 24 bit binary number to an 8 digit BCD
;Input:  U_byte, H_byte, L_byte
;Output: result+3, result+2, result+1, result+0
;Used:   w-reg, count, temp, FSR
; Ex: the 16 bit binary number = FFFFFF
; After conversion the Decimal Number
; in result+0,result+1,result+2,result+3 = 16,77,72,15

Bin_2_BCD
   bcf   STATUS,0   ; clear the carry bit
   movlw   .24
   movwf   count
   clrf   result+0
   clrf   result+1
   clrf   result+2
   clrf   result+3
loop24   rlcf   L_byte, F
   rlcf   H_byte, F
   rlcf   U_byte, F
   rlcf   result+3, F
   rlcf   result+2, F
   rlcf   result+1, F
   rlcf   result+0, F

   dcfsnz   count, F
   retlw   0

adjDEC   lfsr   0,result+3
   call   adjBCD

   lfsr   0,result+2
   call   adjBCD

   lfsr   0,result+1
   call   adjBCD

   lfsr   0,result+0
   call   adjBCD

   goto   loop24

adjBCD   movlw   3
   addwf   INDF0,W
;   movwf   temp
   btfsc   WREG,3   ; test if result > 7
   movwf   INDF0
   movlw   0x30
   addwf   INDF0,W
;   movwf   temp
   btfsc   WREG,7   ; test if result > 7
   movwf   INDF0   ; save as MSD
   RETLW   0

;===================================================
;  PutString - print in-line string via Stack and TBLPTR
;
;  string must be terminated with a 00 byte and does not need
;  to be word aligned
;
PutString
        movff   TOSH,TBLPTRH    ; copy return address to TBLPTR
        movff   TOSL,TBLPTRL    ;
        clrf    TBLPTRU         ; assume PIC with < 64-KB
PutNext
        tblrd   *+              ; get in-line string character
        movf    TABLAT,W        ; last character (00)?
        bz      PutExit         ; yes, exit, else
        rcall   LCDprint          ; print character
        bra     PutNext         ; and do another
PutExit
        btfsc   TBLPTRL,0       ; odd address?
        tblrd   *+              ; yes, make it even (fix PC)
        movf    TBLPTRH,W       ; setup new return address
        movwf   TOSH            ;
        movf    TBLPTRL,W       ;
        movwf   TOSL            ;
        return                  ;
;

pause1sec
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
;===================================================
; pause for 100 millisecs
pause100ms
   movlw   .100
;===================================================
; pause for w millisecs
pausewms
   movwf   pausecount+1
pause100ms_2
   movlw   .10
   call   pause100uS
   decfsz   pausecount+1,f
   goto   pause100ms_2
   nop
   return

;===================================================
; pause for w00 microsecs (4x10MHz)
pause100uS
;   return
   movwf   pausecount+0      ;save pause time
pause100uS_2
   movlw   .249
pause100uS_3
   nop
   nop
   decfsz   WREG,f
   bra   pause100uS_3
   decfsz   pausecount+0,f
   bra   pause100uS_2
   return


;===================================================
;End of program

   END

Hopefully there isn't too much jibberish in the source code, and the comments are actually useful.
/vp
vegipete
 
Posts: 37
Joined: Sat Jul 09, 2011 7:38 pm

Re: Simple Source Code

Postby vegipete » Mon Aug 15, 2011 1:34 pm

Looks like the formatting got a bit munched. Oh well.

Some comments about the code:
Interrupt priorities are not used. This simplifies the interrupt routine slightly, giving a slight
speed increase. Since I only use one interrupt source, priorities are not needed.

Step pulses can be made longer at the cost of lower maximum step rates, either by
slowing the Timer 2 interrupt rate or by keeping the pulses high for more than one
interrupt period.

While assorted variables are 32 bits wide, the display routines as implemented are
limited to 24 bits - peculiar values appear if values are too large.

As can be seen in the source code, this demo uses a single, hard coded move value, and
toggles the direction between move requests, resulting in a simple back and forth motion.

===============
Appearing soon, another version that includes serial interfacing, and following that, a
rudimentary single axis G-Code interpreter.

Stay tuned!
/vp
vegipete
 
Posts: 37
Joined: Sat Jul 09, 2011 7:38 pm

Re: Simple Source Code

Postby vegipete » Thu Aug 25, 2011 8:37 pm

In case it's not obvious, the above source code includes routines for displaying output on a standard character LCD using a 4-bit interface. The wiring details are explained in the comments at the start of the source code describing the hardware configuration. As is often the case, adapting these routines to other LCD's may require adjustments to the timing values.
/vp
vegipete
 
Posts: 37
Joined: Sat Jul 09, 2011 7:38 pm

Re: Simple Source Code

Postby domen » Sun Aug 28, 2011 11:38 am

it will be much more easyer to have serial interfacing. It is posible to have 3 independet axis with this approach on 18F?
domen
 

Re: Simple Source Code

Postby vegipete » Tue Aug 30, 2011 8:35 am

it will be much more easyer to have serial interfacing.
The above code is just a demo to present the algorithm and show it working. I have serial port routines working, although at this stage they merely echo characters. I'll whack something together when I get a chance that allows serial commands to set the move parameters.
It is posible to have 3 independet axis with this approach on 18F?
Well, that is the question. Three independent axes is easy, coordinated 3-d moves less so. That takes considerably more computer power than an 18F can provide. Three axis linear moves combining my algorithm above with Bresenham's algorithms should be relatively easy to implement without much of a performance hit.

vp
/vp
vegipete
 
Posts: 37
Joined: Sat Jul 09, 2011 7:38 pm

Re: Simple Source Code

Postby vegipete » Sun Jan 08, 2012 1:36 pm

Here is an updated and improved version of the Stepper Speed Ramp code. A very basic 2-axis G Code interpreter has been implemented. It operates in units of steps and has no understanding of human units such as inches or millimeters. Since the units are steps, it ignores numbers after decimal points.

(Sorry about the formatting - I suppose I really ought to let MPLAB convert tabs to spaces...)

The program is now based on a PIC18F14K22 chip, running on a slightly hacked break out board from Piclist. As a result of the slight hacking, x-axis signals appear on the board's z-axis and y-axis is mapped to the board's a-axis. LCD routines are gone.

Bresenham line routines are implemented, although feed rates are not scaled to account for the fact that diagonal lines are longer and therefore should take longer. For example, a feed move of (x10000 y10000) takes exactly as long as (x10000 y0) even though the diagonal move is 14142 steps long. Thus the feedrate of this diagonal move is 1.4 times too high.

There is a terse help screen at the end of the source code that may slightly help understand usage.

Feel free to fire away with questions or comments.

(As a teaser, I have a much more full blown G Code interpreter running. Alas, it is not quite ready for public consumption...)

Code: Select all
;****************************************************************
;   BOBStep
;   Generating ramped stepper signals in software
;   Written by VegiPete   picprog.strongedge.net
;
;   Version Control:
;   0.1   Ported to PIC18F14K22
;   0.2   Bug fixes - 1 step moves and slow moves failed
;   0.3   Added drive enable command, slaved A axis to Z axis
;   0.4   Two axis! Accept Z and X inputs, Bresenham lines.
;   0.5   G-Code interpretter
;   0.5b   Cleanup, expand G-Code comment recognition
#define   version   "0.5b"
;
;   Hardware:
;   Internal RC OSC with 4x PLL enabled
;   RA0 - ICD/PICkit PGD
;   RA1 - ICD/PICkit PGC
;   RA2 - (input only)
;   RA3 - !MCLR
;   RA4 - Z-Step
;   RA5 - A-Step
;
;   RB4 - !CTS flow control output - drive low to receive
;   RB5 - RX - Serial in
;   RB6 -
;   RB7 - TX - Serial out
;
;   RC0 -
;   RC1 -
;   RC2 - Z-Dir
;   RC3 - A-Dir
;   RC4 -
;   RC6 -
;   RC5 -
;   RC7 - Drive Enable
;
;****************************************************************
;Resources Used:
;   Timer2      Periodic Interrupt
;   FSR2      dedicated to ISR for RX buffer
;   Serial Port   polled by the ISR, buffer 0x100-0x17F (128 bytes)
;****************************************************************

   LIST   P=18F14K22   ;directive to define processor
   #include <P18F14K22.INC>   ;processor specific variable definitions
   radix   dec

;==========================================================================
;
; CONFIGURATION WORD SETUP
;
; The 'CONFIG' directive is used to embed the configuration word within the
; .asm file. The lables following the directive are located in the respective
; .inc file.  See the data sheet for additional information on configuration
; word settings.
;
;==========================================================================

     ;Setup CONFIG11H
     CONFIG FOSC = IRC, PLLEN = ON, PCLKEN = OFF, FCMEN = OFF, IESO = OFF
     ;Setup CONFIG2L
     CONFIG PWRTEN = ON, BOREN = OFF, BORV = 30
     ;Setup CONFIG2H
     CONFIG WDTEN = OFF, WDTPS = 1
     ;Setup CONFIG3H
     CONFIG MCLRE = ON, HFOFST = OFF
     ;Setup CONFIG4L
     CONFIG STVREN = OFF, LVP = OFF, BBSIZ = OFF, XINST = OFF
     ;Setup CONFIG5L
     CONFIG CP0 = OFF, CP1 = OFF
     ;Setup CONFIG5H
     CONFIG CPB = OFF, CPD = OFF
     ;Setup CONFIG6L
     CONFIG WRT0 = OFF, WRT1 = OFF
     ;Setup CONFIG6H
     CONFIG WRTB = OFF, WRTC = OFF, WRTD = OFF
     ;Setup CONFIG7L
     CONFIG EBTR0 = OFF, EBTR1 = OFF
     ;Setup CONFIG7H
     CONFIG EBTRB = OFF

;==========================================================================

#define   LED_ON      bsf   LATC,1
#define   LED_OFF      bcf   LATC,1

;serial port flow control - CTS low to permit incoming data
#define   RTS_STOP   bsf   LATB,4
#define   RTS_GO      bcf   LATB,4

;Stepper interface definitions
#define   STEP_PORT   LATA      ;all step pins must be on same port
STEP_X_PIN      equ   0
STEP_Y_PIN      equ   1
STEP_Z_PIN      equ   4
STEP_A_PIN      equ   5

#define   DIR_PORT   LATC      ;all dir pins ought to be on the same port
DIR_X_PIN      equ   0   ;  (but they could be scattered...)
DIR_Y_PIN      equ   1
DIR_Z_PIN      equ   2
DIR_A_PIN      equ   3

#define   DRIVE_EN   LATC,7

;hard coded motion parameters
_MAXFEED   equ   7500   ;as determined by hardware
_ACCELERATION   equ   30

;#define STEP_HI_LEN   b'11'      ;request step pulse HI for 2 interrupts,
#define STEP_HI_LEN   b'01'      ;request step pulse HI for 1 interrupt,


;A nice macros for outputting strings stored in-line in FLASH
;used with subroutine near end of source code
;
;   _SERPrint   " Menu items\n\n\r"
_SERPrint   MACRO   str      ; print in-line string macro
      call   SERPutString   ;
      db   str,0
      ENDM
;

;step_state flag bits
;step_state is primed with the value b'00010001'
;then a simple rrncf rotates to the next state
IDLE      equ   0   ;no move currently active
DECEL      equ   1   ;approaching end of move
MAX_V      equ   2   ;moving at full speed
ACCEL      equ   3   ;move starting

;step ramp control variables
;the first 3 variables should be set by the calling routine.
;step_middle will be calculated by the call to Start_Move
      CBLOCK   0x000
      step_move:4   ;long axis move requested
      step_move_sh:4   ;short axis move requested
      step_spmax:4   ;maximum speed
      step_accel:2   ;accel/decel rate, 8.8 bit format
      step_parallel:1   ;bit pattern to identify longer Bresenham axis
            ;b'00010000' (z-axis longer) -or- b'00100000' (a-axis longer)
            ;these bits match the appropriate Port pin
      step_active:1   ;working bit pattern per each bresenham loop iteration
   
      step_middle:4   ;mid-point of move, = (step_move - 1) >> 1
      step_error:4   ;Bresenham error value
      
      step_dur:1   ;counter for duration of step pulse HI
      step_count:4   ;step counter
      step_frac:2   ;step counter fraction
      step_speed:4   ;current speed, 16.8 bit format (hi byte always 00)
      step_state:1   ;move profile state
      step_flags:1   ;any useful flags
      ENDC
   
M_MID      EQU   0   ;a flag! set when step_count = step_middle
STEP_1st   EQU   1   ;set after the first step
FAST_START   EQU   2   ;set if accel phase ended without a step pulse

;serial buffer variables
      CBLOCK
      flags:1
      flags_found:1   ;serial input stream codes found
      flags_sign:1   ;serial input stream sign of numbers found
      rx_count:1   ;number of characters in buffer
      rx_point:1   ;next character to pull from buffer
      cmd_point:1
   
      GCode_New:1   ;new G Code found in serial input
      FRate_New:2   ;new Feed Rate found in serial input

      GCode_Active:1   ;currently active G-Code, = 0 if none
      FRate_Active:2
      FRate_Rapid:2
      ENDC

;bits used in flags
_COMMENT   EQU   0   ;comment has been found, ignore chars until EOL
_DECIMAL   EQU   1   ;decimal point found, ignore following digits
_NEGATIVE   EQU   2   ;set if a negative number was found
_ABSOLUTE   EQU   3   ;set for absolute mode, clear for relative

_NEWMOVE   EQU   7   ;set bit when new move command is waiting

;set appropriate bit in 'flags_sign' if move value found is negative (valid for XYZA only)
;set appropriate bit in 'flags_found' if block found in last line
_XCOORD      EQU   0
_YCOORD      EQU   1
_GCODE      EQU   2
_FRATE      EQU   3
_ZCOORD      EQU   4
_ACOORD      EQU   5

;general (non isr) variables
      CBLOCK
      pausecount:2   ;counters for pause routines
      temp00:1
      count:1
      serial_count:1
      result:5
      source:4
      ENDC

;single location at end of list for debugging
      CBLOCK
      _LAST_ONE   ;debug purposes
      ENDC

;these are outside of the access bank, must be accessed with BSR=0
      CBLOCK   0x60
      XCoord_New:4   ;new X Coordinate found in serial input
      YCoord_New:4   ;new Y Coordinate found in serial input
      ZCoord_New:4   ;new Z Coordinate found in serial input
      ACoord_New:4   ;new A Coordinate found in serial input
      X_Position:4   ;absolute X position
      Y_Position:4   ;absolute Y position
      Z_Position:4   ;absolute Z position
      A_Position:4   ;absolute A position
      ENDC

      CBLOCK   0x100
      rx_buffer:128   ;128 character receive buffer
      cmd_buffer:128   ;g-code command buffer
      ENDC

_RX_BUF_LEN   EQU   128   ;receive buffer size


;******************************************************************************
;EEPROM data
; Data to be programmed into the Data EEPROM is defined here

   ORG   0xf00000

   DE   "Test Data",0,1,2,3,4,5

;******************************************************************************
;Reset vector
; This code will start executing when a reset occurs.

   ORG   0x0000     ;start address 0
   nop         ;maybe for icd
   clrf   INTCON      ;make sure interupts are off
   bra   Main      ;go to start of main code
   movf   _LAST_ONE,f   ;for debug

;******************************************************************************
;High priority interrupt vector
; This code will start executing when a high priority interrupt occurs or
; when any interrupt occurs if interrupt priorities are not enabled.

   ORG   0x0008

;   bra   HighInt      ;go to high priority interrupt routine
;High priority interrupt routine
HighInt:
   movlb   0
   btfss   PIR1,TMR2IF   ;test for T2 match interrupt
;******************************************************************************
;unexpected interrupt - what to do, what to do...
unknown_int
   reset
;   bra   unknown_int   ;Oh Dear! Unexpected interrupt
;******************************************************************************

;******************************************************************************
;process step pulse train
;1) Interrupt housekeeping
;2) Step pulse duration control
;Perform the following if motion is currently active
;3) Calculate next time slice, generate step pulse if needed
;4) Adjust stepping speed if needed
;******************************************************************************
;1) Housekeeping - reset the interupt flag and turn on an LED to monitor load
T2_int
   bcf   PIR1,TMR2IF   ;clear T2 interrupt
   LED_ON
   
;******************************************************************************
;2) Step pulse duration control
;Since the max step rate is 1/2 the interrupt rate, set the step pulse
;HI duration to 1/2 of this max step rate - ie 1 interrupt period
;step_active holds the necessary axis pins to step high
   bcf   STATUS,C
   rrcf   step_dur,f
   bnc   Set_Step_Low
   movf   step_active,w   ;grab the step bit pattern
   iorwf   STEP_PORT   ;set the appropriate bits
   bra   Test_Idle
Set_Step_Low
   bcf   STEP_PORT,STEP_Z_PIN   ;force STEP Z low
   bcf   STEP_PORT,STEP_A_PIN   ;force STEP A low
Test_Idle
   btfsc   step_state,IDLE   ;are things in motion?
   bra   T2_Int_Done   ;no - we're done

;******************************************************************************
;3) Calculate next time slice, generate step pulse if needed
;Calculate {step_frac += step_speed + 1} (16 bit values)
;If this overflows, it's time for the next step pulse
;Note: if we're at maximum speed, calculate {step_frac += step_spmax + 1} instead
   bsf   STATUS,C   ; + 1 of above equation
   movf   step_speed+1,w   ;grab speed lo byte
   btfsc   step_state,MAX_V
   movf   step_spmax+0,w   ;get max speed instead
   addwfc   step_frac+0,f   ;add to position fraction lo
   movf   step_speed+2,w   ;grab hi byte
   btfsc   step_state,MAX_V
   movf   step_spmax+1,w   ;get max speed instead
   addwfc   step_frac+1,f   ;add to position fraction hi
   bnc   TimeSlice_done   ;no carry so no step pulse yet

;step_frac has oveflowed so it's time to generate a step
   bsf   step_flags,STEP_1st
   incfsz   step_count+0,f   ;increment 32 bit step counter
   bra   Test_Step_Mid
   incfsz   step_count+1,f
   bra   Test_Step_Mid
   infsnz   step_count+2,f   ;does NOT handle 32 bit overflow gracefully
   incf   step_count+3,f   ; but max move requested is <=32 bits so ...

;check if the mid-point (or decel-start-point or end) has been reached
;this comparison is done here so it only ocurrs when a step is generated
Test_Step_Mid
   bcf   step_flags,M_MID   ;assume not at middle to begin

   movf   step_count+3,w
   cpfseq   step_middle+3
   bra   New_Step_Pulse

   movf   step_count+2,w
   cpfseq   step_middle+2
   bra   New_Step_Pulse

   movf   step_count+1,w
   cpfseq   step_middle+1
   bra   New_Step_Pulse

   movf   step_count+0,w
   cpfseq   step_middle+0      ;too bad there's no 'cpfsne'
   bra   New_Step_Pulse
   bsf   step_flags,M_MID   ;flag mid point for later calcs
   
New_Step_Pulse
   movlw   STEP_HI_LEN      ;set step pulse HI,
   movwf   step_dur      ;  starting next interrupt

;NOTE: Should check if this interrupt ran long, given how much calculation
;is involved. If it did take too long, the step pulse generated by the
;next interrupt could be shorter than expected/desired.

;figure out inside of bresenham loop here
;   x := x + 1      ;increment long axis every time
   movf   step_parallel,w
   movwf   step_active   ;assume single axis move to begin

;   error := error - deltay
   movf   step_move_sh+0,w
   subwf   step_error+0,f
   movf   step_move_sh+1,w
   subwfb   step_error+1,f
   movf   step_move_sh+2,w
   subwfb   step_error+2,f
   movf   step_move_sh+3,w
   subwfb   step_error+3,f   ;calculate new error value
;   if error >= 0 then skip ahead
   bnn   TimeSlice_done   ;branch if error > 0
;   bz   TimeSlice_done   ;branch if error = 0

;   y := y + 1      ;increment short axis only if error went negative
   movlw   1<<_ZCOORD | 1<<_ACOORD
   movwf   step_active   ;switch to double axis move

;   error := error + deltax
   movf   step_move+0,w
   addwf   step_error+0,f
   movf   step_move+1,w
   addwfc   step_error+1,f
   movf   step_move+2,w
   addwfc   step_error+2,f
   movf   step_move+3,w
   addwfc   step_error+3,f
   
TimeSlice_done

;******************************************************************************
;4) Adjust stepping speed if needed
;There are 3 conditions:
; 1) if we are accelerating, increase the stepping speed,
;   if we hit the midpoint, switch to decelerating
;   if we hit max speed, switch to at-max
; 2) if we are at max speed, make no change to speed
;   if we are approaching the end, switch to decelerating
; 3) if we are decelerating, decrease the stepping speed,
;   if we hit the end, stop

   btfss   step_state,ACCEL   ;check for accel state
   bra   state_max_speed      ;branch if not
;acceleration ramp
;test if we've hit the midpoint during acceleration phase
   btfss   step_flags,M_MID
   bra   accel_not_mid
   bcf   step_flags,M_MID   ;clear the flag
;we've hit the middle, time to start decelerating
;must attend to even/odd number of steps
   rrncf   step_state,f   ;switch to max speed state
   rrncf   step_state,f   ;switch to deceleration state
   bra   load_end   ;set up end condition

accel_not_mid
;still accelerating, not yet at midpoint
;so increase speed and test for max speed
   movf   step_accel+0,w   ;add 16 bit acceleration value
   addwf   step_speed+0,f   ;to 24 bit current speed value
   movf   step_accel+1,w
   addwfc   step_speed+1,f
   movlw   0
   addwfc   step_speed+2,f

   movf   step_speed+2,w   ;grab current speed hi byte
   subwf   step_spmax+1,w   ;compare to limit hi byte
   bnz   accel_test_max   ;if they are not equal, result is in C
   movf   step_speed+1,w   ;grab current speed lo byte
   subwf   step_spmax+0,w   ;compare to limit lo byte, result is in C
accel_test_max
   bc   T2_Int_Done   ;C is set if step_speed < step_spmax
   rrncf   step_state,f   ;switch to max speed state
;calculate {step_middle = step_move - step_count} to determine
;when to start slowing down
;we can store this in step_middle because the mid point no longer matters
   movf   step_count+0,w
   subwf   step_move+0,w
   movwf   step_middle+0
   movf   step_count+1,w
   subwfb   step_move+1,w
   movwf   step_middle+1
   movf   step_count+2,w
   subwfb   step_move+2,w
   movwf   step_middle+2
   movf   step_count+3,w
   subwfb   step_move+3,w
   movwf   step_middle+3
   btfsc   step_flags,STEP_1st   ;have we had a first step pulse yet?
   bra   T2_Int_Done      ;yes so decel state will have at least one step
   bsf   step_flags,FAST_START   ;special case - no steps occurred during acceleration
               ;this flag indicates that we can skip the decel state
   bra   T2_Int_Done

state_max_speed
   btfss   step_state,MAX_V   ;check for max speed state
   bra   state_decel      ;branch if not
;cruising along at max so leave step_speed unchanged
; and check if we're approaching the end
;if step_count = step_middle, it's time to decelerate
   btfss   step_flags,M_MID
   bra   T2_Int_Done
   bcf   step_flags,M_MID   ;clear the flag
   rrncf   step_state,f      ;shift to deceleration state
;following test deals with the special case in which the acceleration state
;ends before the first step pulse is generated, likely due to either a low
;max speed or a high acceleration. Since zero steps occurred during acceleration,
;the end of the max speed state happens when the end of the move is reached
;and we can skip the deceleration state entirely.
   btfsc   step_flags,FAST_START
   bra   no_decel
load_end
;copy requested move to step_middle to allow compare for end of move
   movff   step_move+0,step_middle+0
   movff   step_move+1,step_middle+1
   movff   step_move+2,step_middle+2
   movff   step_move+3,step_middle+3

   bra   T2_Int_Done

state_decel
;if we have reached the end of the move, shift to idle state
   btfsc   step_flags,M_MID
   bra   no_decel

;we're decelerating so calculate {step_speed -= step_accel}
   movf   step_accel+0,w
   subwf   step_speed+0,f
   movf   step_accel+1,w
   subwfb   step_speed+1,f
   movlw   0
   subwfb   step_speed+2,f

   bra   T2_Int_Done

no_decel
;step_count = step_move - we are done
   clrf   step_flags
   rrncf   step_state,f   ;shift to idle state
   clrf   step_speed+0
   clrf   step_speed+1
   clrf   step_speed+2
   clrf   step_speed+3   ;zero the speed just in case

T2_Int_Done
   LED_OFF
;******************************************************************************
;poll serial receive, stuff anything new into circular buffer
;128 byte circular receive buffer
;FSR2 always points to next free slot in buffer
;CTS set high only when receive buffer is full
;CTS stays high until receive buffer is below 1/2 full
   btfss   rx_count,6
   RTS_GO         ; only (re)enable CTS if bit 6 clear
   
   btfss   PIR1,RCIF   ; set if new character has been received
   retfie   FAST
   btfss   RCSTA,OERR   ; test for overrun error
   bra   ser_framing   ; jump if no overrun
   bcf   RCSTA,CREN   ; clear continous receive bit
   bsf   RCSTA,CREN   ; and set it again
ser_framing
   btfss   RCSTA,FERR   ; check from framing errors
   bra   ser_buffer   ; jump if no framing error
ser_discard
   movf   RCREG,w      ; read byte and discard
   retfie   FAST

;put new character into buffer
ser_buffer
   movff   RCREG,POSTINC2   ; move new char to buffer
   bcf   FSR2L,7      ; wrap pointer back to start if needed
   incf   rx_count,f   ; indicate new reception

   movlw   _RX_BUF_LEN-4   ; this magic number determined by flow control latency
   cpfsgt   rx_count   ; is the receive buffer full?
   retfie   FAST      ; no so ISR finished
   RTS_STOP      ; no more chacters please!

   retfie   FAST

;******************************************************************************
;Start a move
;Input:
;   step_accel:2   allowable acceleration, static
;   step_spmax:2   maximum speed for move, as required
;   step_move:4   number of steps for 1st axis
;   step_move_sh:4   number of steps for 2nd axis, order unimportant
;   Hardware direction pins already set
;Output:
;   step_parallel (contains bit set for long axis) (Hard coded for Z&A)
;   Bresenham variables
;   zero step and single step moves handled
;   ISR started for longer moves
;Comments:
;   'steep' lines are handled correctly
;
;   At present, x-axis moves are forced to the z-axis
;   and y-axis moves are forced to the a-axis.
;   This is hardcoded and obviously must be fixed/improved
;
Start_Move
   clrf   step_flags

   clrf   step_count+0
   clrf   step_count+1
   clrf   step_count+2
   clrf   step_count+3
   
   clrf   step_frac+0
   clrf   step_frac+1

   clrf   step_speed+0
   clrf   step_speed+1
   clrf   step_speed+2
   clrf   step_speed+3

   movlw   1<<STEP_Z_PIN   ;set single bit - assume Z to start
   movwf   step_parallel

;if (step_move_sh > step_move) then swap
   movf   step_move_sh+0,w
   subwf   step_move+0,w   ; w := f-w
   movf   step_move_sh+1,w
   subwfb   step_move+1,w
   movf   step_move_sh+2,w
   subwfb   step_move+2,w
   movf   step_move_sh+3,w
   subwfb   step_move+3,w
   btfsc   STATUS,C   ;skip if step_move < step_move_sh
   bra   SM_No_Swap

;swap values so bigger one is in step_move
   movf   step_move+3,w
   movff   step_move_sh+3,step_move+3
   movwf   step_move_sh+3
   movf   step_move+2,w
   movff   step_move_sh+2,step_move+2
   movwf   step_move_sh+2
   movf   step_move+1,w
   movff   step_move_sh+1,step_move+1
   movwf   step_move_sh+1
   movf   step_move+0,w
   movff   step_move_sh+0,step_move+0
   movwf   step_move_sh+0

   movlw   1<<STEP_A_PIN   ;set A axis as long axis instead
   movwf   step_parallel
SM_No_Swap

;test for zero or single step move
;if the long axis is zero, the short one must be also
   movf   step_move+3,w
   iorwf   step_move+2,w
   iorwf   step_move+1,w
   bnz   SM_Calc_Mid   ;branch if upper 3 bytes are != 0
   movf   step_move+0,w
   bz   SM_No_Move   ;requested zero steps so do nothing
   decfsz   step_move+0,w
   bra   SM_Calc_Mid   ;requested move is more than 1 step
;long axis is single step, check short axis for single step also
;NOTE:    do we need to make sure that the next move doesn't start before this
;   (manually forced) one is finished?
   movf   step_parallel,w   ;get current long axis step bit
   tstfsz   step_move_sh+0   ;only need to check low byte
   iorlw   1<<STEP_Z_PIN | 1<<STEP_A_PIN   ;force both axis bits - diagonal move
   movwf   step_parallel
   movlw   b'01'      ;generate single step pulse manually,
   movwf   step_dur   ;  starting next interrupt
   bra   SM_No_Move

SM_Calc_Mid
;calculate middle = error = move / 2
;step_error is the initial setting for the Bresenham error variable
   bcf   STATUS,C
   rrcf   step_move+3,w
   movwf   step_middle+3
   movwf   step_error+3
   rrcf   step_move+2,w
   movwf   step_middle+2
   movwf   step_error+2
   rrcf   step_move+1,w
   movwf   step_middle+1
   movwf   step_error+1
   rrcf   step_move+0,w
   movwf   step_middle+0
   movwf   step_error+0

;   rrncf   step_state,f   ;shift to accel state to get things going
   movlw   b'10001000'
   movwf   step_state   ;set ACCEL state to get things going, ISR takes over
SM_No_Move
   return

;******************************************************************************
;Start of main program

Main:
;initialize hardware
   movlb   0      ;point BSR to 1st bank

   movlw   b'01111100'   ;sleep, 16MHz, R, R, Primary Clock
   movwf   OSCCON      ;set oscillator

   clrf   ADCON0       ; turn off analog module
   clrf   ADCON1      ; Vref = Vdd, Vss
   clrf   ADCON2      ;
   clrf   ANSEL      ; enable digital inputs
   clrf   ANSELH      ; enable even more digital inputs
   clrf   CM1CON0      ; comparator 1 off
   clrf   CM2CON0      ; comparator 2 off

   clrf   LATA       ; Initialize PORTA by clearing output data latches
   clrf   WPUA      ; Turn off weak pullups
   movlw   b'00000000'
   movwf   TRISA      ; Set PORTA all output

   clrf   LATB
   movlw   b'10100000'   ; Set RX & TX pins as inputs
   movwf   TRISB      ; Set PORTB bits

   clrf   LATC
   movlw   b'00000000'   ; Set PORTC all output
   movwf   TRISC

;set up timer 2
   clrf   TMR2      ;clear Timer2 value
   movlw   39      ;suitable period register value for 200kHz (16MHz clock)
   movwf   PR2      ;set Timer2 rate
   movlw   b'00001100'   ;1:2 postscale, T2 on, 1:1 prescale
      ; .0001...   1:2 post scale
      ; .....1..   Timer 2 on
      ; ......00   1:1 prescale
      ; ......11   1:16 prescale
   movwf   T2CON      ;setup Timer2

;set up serial port (USART)
   movlw   b'00000000'
      ; 0.......    Auto-Baud Acquisition Rollover Status bit
      ; .0......    (read only) 1 = Receive operation is Idle
      ; ..0..... RXDTP 0 = Receive data (RX) is not inverted (active-high)
      ; ...0.... TXCKP 0 = Idle state for transmit (TX) is a high level
      ; ....0... BRG16 1 = 16-bit Baud Rate Generator – SPBRGH and SPBRG
      ; .....0..    Unimplemented: Read as ‘0’
      ; ......0.    Wake-up Enable bit
      ; .......0    0 = Baud rate measurement disabled or completed
   movwf   BAUDCON
   clrf   SPBRGH       ;EUSART Baud Rate Generator Register High Byte
   movlw   51      ;19k2 @ 16MHz (16MHz x4 PLL / 4 clocks/instruction)
   movwf   SPBRG      ;EUSART Baud Rate Generator Register Low Byte
   movlw   b'00100000'
      ; 0....... CSRC  don't care
      ; .0...... TX9   0 = Selects 8-bit transmission
      ; ..1..... TXEN  1 = Transmit enabled
      ; ...0.... SYNC  0 = Asynchronous mode
      ; ....0... SENDB Send Break Character bit
      ; .....0.. BRGH  1 = High speed BRGH: High Baud Rate Select bit
      ; ......0. TRMT  TRMT: Transmit Shift Register Status bit (1=TSR empty)
      ; .......0 TX9D  9th Bit of Transmit Data
   movwf   TXSTA
   movlw   b'10010000'
      ; 1....... SPEN  1 = Serial port enabled
      ; .0...... RX9   0 = Selects 8-bit transmission
      ; ..0..... SREN  don't care
      ; ...1.... CREN  1 = Enables receiver
      ; ....0... ADDEN don't care
      ; .....0.. FERR  1 = Framing error
      ; ......0. OERR  1 = Overrun error
      ; .......0 RX9   read only, don't care
   movwf   RCSTA

;clear memory - bank 0 and 1
   lfsr   FSR0,0      ;start of RAM
clearmem
   clrf   POSTINC0
   btfss   FSR0H,1      ;just past end of RAM
   bra   clearmem

;initialize some variable
   lfsr   2,rx_buffer   ;point to start of receive buffer
   movlw   b'00010001'
   movwf   step_state   ;set IDLE state
   
   setf   FRate_New+1   ;indicate no valid feedrate

   movlw   low _MAXFEED
   movwf   FRate_Rapid+0
   movlw   high _MAXFEED
   movwf   FRate_Rapid+1

   movlw   low _ACCELERATION
   movwf   step_accel+0
   movlw   high _ACCELERATION
   movwf   step_accel+1

   call   pause100ms   ;wait a bit to get serial port settled (Why?)
   _SERPrint "\n\n\rReady... "

   bsf   DRIVE_EN   ;enable the drive

   call   Splash_Help

;set up interrupts
;   bsf   RCON,IPEN   ;enable interrupt priorities
   bsf   INTCON,GIEH   ;enable (high priority) interrupts
   bsf   INTCON,PEIE   ;enable peripheral interrupts (needed if priorities off)
;   bsf   IPR1,TMR2IP   ;set Timer2 interrupt to hi priority
   bcf   PIR1,TMR2IF   ;clear any pending T2 match flag
   bsf   PIE1,TMR2IE   ;turn on Timer2 interrupt

;******************************************************************************
;*
;* End of initialization.
;* Let the coding begin!
;*
;******************************************************************************

move_loop
   tstfsz   rx_count   ;Has ISR found any new characters?
   rcall   Serve_Ser   ;process them

   btfss   flags,_NEWMOVE   ;have we received a new move command?
   bra   move_loop   ;no so loop back for more

ml_new_move
   btfss   step_state,IDLE   ;is the motor stopped?
   bra   ml_new_move   ;no, stuck until ISR is ready for new move

;set speed for this move
   movff   FRate_Active+0,step_spmax+0
   movff   FRate_Active+1,step_spmax+1

;HARD CODE - set move based on X and Y
;IF new_X, set step_move = XCoord_New, ELSE = 0
   btfsc   flags_found,_XCOORD
   bra   ml_set_long
   clrf   step_move+0
   clrf   step_move+1
   clrf   step_move+2
   clrf   step_move+3
   bra   ml_test_short
ml_set_long
   movff   XCoord_New+0,step_move+0
   movff   XCoord_New+1,step_move+1
   movff   XCoord_New+2,step_move+2
   movff   XCoord_New+3,step_move+3

ml_test_short
;IF new_Y, set step_move_sh = YCoord_New, ELSE = 0
   btfsc   flags_found,_YCOORD
   bra   ml_set_short
   clrf   step_move_sh+0
   clrf   step_move_sh+1
   clrf   step_move_sh+2
   clrf   step_move_sh+3
   bra   ml_set_dir
ml_set_short
   movff   YCoord_New+0,step_move_sh+0
   movff   YCoord_New+1,step_move_sh+1
   movff   YCoord_New+2,step_move_sh+2
   movff   YCoord_New+3,step_move_sh+3

ml_set_dir
   bcf   DIR_PORT,DIR_Z_PIN   ;assume forward to start
   btfsc   flags_sign,_XCOORD   ;test requested dir
   bsf   DIR_PORT,DIR_Z_PIN   ;change direction to reverse
   bcf   DIR_PORT,DIR_A_PIN   ;assume forward to start
   btfsc   flags_sign,_YCOORD   ;test requested dir
   bsf   DIR_PORT,DIR_A_PIN   ;change direction to reverse

   call   Start_Move   ;make the move commence

   bcf   flags,_NEWMOVE   ;clear the flag so next move can be found
   bra   move_loop

;******************************************************************************
;build a single g-code string in cmd_buffer
;called whenever characters are in the receive buffer
;filter out uninteresting characters
; \r or \n indicate end of line so process what has been found
; / ; ( indicate start of comment so ignore rest of line
; ignore spaces
; ignore decimals - ie truncate numbers at the dp
Serve_Ser
   lfsr   1,cmd_buffer   ;buffer base
   movf   cmd_point,w   ;index to end of received string so far
   addwf   FSR1L,f      ;point to command (never overflows)
   
   lfsr   0,rx_buffer   ;point to start of serial rx buffer
   movf   rx_point,w   ;index to next available character
   incf   rx_point,f   ;advance pointer
   bcf   rx_point,7   ;wrap (max 127)
   movf   PLUSW0,w   ;grab character
   decf   rx_count,f   ;one less character available
   andlw   0x7F      ;ensure 7 bits
   xorlw   '\r'
   bz   parse_cmd   ;EOL so evaluate what we found
   xorlw   '\r'^'\n'
   bz   parse_cmd   ;Alternate EOL so evaluate what we found

   btfsc   flags,_COMMENT   ;are we in a comment?
   return         ;yes so ignore char

   xorlw   '\n'^'/'
   bz   start_comment   ;check for "/" - block delete
   xorlw   '/'^';'
   bz   start_comment   ;check for ";" - non-standard comment?
   xorlw   ';'^'('
   bz   start_comment   ;check for "(" - don't worry about ")"
   xorlw   '('^'.'
   bz   start_decimal   ;ignore digits after a decimal point
   xorlw   '.'      ;recover character
   movwf   INDF1      ;store into command buffer

;test for ' ' and less; if yes, ignore
   movlw   ' '
   cpfsgt   INDF1      ;skip if f > W
   return         ;ignore CTRL characters and space

;test for lower case; if yes, convert to upper case
   movlw   'a'-1
   cpfsgt   INDF1      ;skip if f > W
   bra   upper_case   ;char is less than 'a'
   movlw   'z'
   cpfsgt   INDF1      ;skip if f < W
   bcf   INDF1,5      ;change lower case to upper case
upper_case

;ignore digits after a decimal point
   btfss   flags,_DECIMAL
   bra   next_cmd_char
   movlw   '0'-1
   cpfsgt   INDF1      ;skip if f > W
   bra   end_decimal
   movlw   '9'
   cpfsgt   INDF1      ;skip if f > W
   bra   ignore_char   ;branch if this character is a digit
end_decimal
   bcf   flags,_DECIMAL   ;found non-digit so end decimal ignore mode
next_cmd_char
   incf   cmd_point,f   ;new character accepted
   return
start_decimal
   bsf   flags,_DECIMAL
   return
start_comment
   bsf   flags,_COMMENT
ignore_char
   return

;******************************************************************************
;evaluate the string that has been built in cmd_buffer
parse_cmd
   bcf   flags,_COMMENT
   bcf   flags,_DECIMAL
   clrf   cmd_point   ;start next command string at beginning of buffer
   clrf   POSTINC1   ;zero terminate string
   clrf   POSTINC1   ;double zero terminate string

;******************************************************************************
;echo the received string
;   lfsr   1,cmd_buffer   ;point to buffer start
;echo_loop
;   movf   POSTINC1,w   ;grab character
;   bz   echo_done
;   rcall   SERprint   ;send it
;   bra   echo_loop
;echo_done
;   clrf   cmd_point
;   movlw   '\r'
;   rcall   SERprint
;   movlw   '\n'
;   rcall   SERprint

;******************************************************************************
;start evaluating command string
;
;only the codes G, F, N and X,Y,Z,A are recognized
;comments, spaces and decimals have already been removed
;(code order unimportant : g1f2x3 = f2g1x3 = x3f2g1 etc)
;G00   rapid
;G01   feed
;G90   absolute
;G91   incremental
;G9?   set zero any coords on same line
;in absolute mode, a G00 or G01 with no coord gives no move HMMMMMMMMM
;
;N - line numbers will be ignored
;
;If a particular code appears more than once, only the LAST valid one is recognized
;ex: g1x2g3x-4gx-f5 evaluates to G03X-000004F0005
;This needs to be changed.
   lfsr   1,cmd_buffer   ;point to buffer start
   clrf   flags_found   ;assume no codes found at start
parse_loop
   movf   POSTINC1,w   ;grab character
   bz   parse_end   ;branch if end of this command
   xorlw   'N'
   bz   read_LNum
   xorlw   'G'^'N'
   bz   read_GCode
   xorlw   'F'^'G'
   bz   read_Feedrate
   xorlw   'X'^'F'
   bz   read_XCoord
   xorlw   'Y'^'X'
   bz   read_YCoord
   xorlw   'Z'^'Y'
   bz   read_ZCoord
   xorlw   'A'^'Z'
   bz   read_ACoord
cmd_error
   bra   parse_loop   ;unrecognized so skip and keep looking

;look for a line number following an N
;read it in and ignore it
read_LNum
   rcall   read_number   ;attempt to read numeric value from buffer
   bra   parse_loop   ;keep looking for more

;look for a G Code
;the G code is stored incremented by 1 so that the value 0 indicates an error
;ignore negative sign if present
read_GCode
   clrf   GCode_New   ;assume no code to begin
   rcall   read_number   ;attempt to read numeric value from buffer
   bz   cmd_error   ;branch if no numeric value found
   bsf   flags_found,_GCODE
   rcall   find_trim_2   ;trim result to 2 digits (0-99)
   incf   result+0,w   ;add 1 - this way, 0 is an error
   movwf   GCode_New   ;save it (G Code is byte sized)
   bra   parse_loop   ;keep looking for more

;look for a Feed Rate
;ignore negative sign if present
read_Feedrate
   rcall   read_number   ;attempt to read numeric value from buffer
   bz   cmd_error   ;branch if no numeric value found
   bsf   flags_found,_FRATE
   movf   result+1,w   ;Feed Rate is limited to 15 bits (0-32767)
   andlw   b'10000000'
   iorwf   result+2,w
   iorwf   result+3,w   ;if any bits are set, clamp value to 0x3FF
   bz   read_Feed_ok
   movlw   0xFF
   movwf   result+0
   movlw   0x7F      ;limit it to max 0x3FF
   movwf   result+1
read_Feed_ok   
   movff   result+0,FRate_New+0   ;save new value
   movff   result+1,FRate_New+1
   bra   parse_loop   ;keep looking for more

;look for an X Coordinate - max is 999,999
;need to keep record of sign
read_XCoord
   rcall   read_number   ;attempt to read numeric value from buffer
   bz   cmd_error   ;branch if no numeric value found
   bsf   flags_found,_XCOORD
   bcf   flags_sign,_XCOORD   ;assume positive to start
   btfsc   flags,_NEGATIVE      ;test for +/-
   bsf   flags_sign,_XCOORD   ;set to negative
   movff   result+0,XCoord_New+0   ;X Coord is longword sized
   movff   result+1,XCoord_New+1
   movff   result+2,XCoord_New+2
   movff   result+3,XCoord_New+3
   bra   parse_loop   ;keep looking for more
read_YCoord
   rcall   read_number   ;attempt to read numeric value from buffer
   bz   cmd_error   ;branch if no numeric value found
   bsf   flags_found,_YCOORD
   bcf   flags_sign,_YCOORD   ;assume positive to start
   btfsc   flags,_NEGATIVE      ;test for +/-
   bsf   flags_sign,_YCOORD   ;set to negative
   movff   result+0,YCoord_New+0   ;Y Coord is longword sized
   movff   result+1,YCoord_New+1
   movff   result+2,YCoord_New+2
   movff   result+3,YCoord_New+3
   bra   parse_loop   ;keep looking for more
read_ZCoord
   rcall   read_number   ;attempt to read numeric value from buffer
   bz   cmd_error   ;branch if no numeric value found
   bsf   flags_found,_ZCOORD
   bcf   flags_sign,_ZCOORD   ;assume positive to start
   btfsc   flags,_NEGATIVE      ;test for +/-
   bsf   flags_sign,_ZCOORD   ;set to negative
   movff   result+0,ZCoord_New+0   ;Z Coord is longword sized
   movff   result+1,ZCoord_New+1
   movff   result+2,ZCoord_New+2
   movff   result+3,ZCoord_New+3
   bra   parse_loop   ;keep looking for more
read_ACoord
   rcall   read_number   ;attempt to read numeric value from buffer
   bz   cmd_error   ;branch if no numeric value found
   bsf   flags_found,_ACOORD
   bcf   flags_sign,_ACOORD   ;assume positive to start
   btfsc   flags,_NEGATIVE      ;test for +/-
   bsf   flags_sign,_ACOORD   ;set to negative
   movff   result+0,ACoord_New+0   ;A Coord is longword sized
   movff   result+1,ACoord_New+1
   movff   result+2,ACoord_New+2
   movff   result+3,ACoord_New+3
   bra   parse_loop   ;keep looking for more

;found the end of the received string
parse_end
   rcall   echo_new_blocks
   movf   flags_found,f   ;test all flags at once
   btfsc   STATUS,Z   ;Z set if no blocks found
   return

;Check for G Code, evaluate it if found
;If there's no G code, leave GCode_Active untouched
;The G-Code number is stored as 1 higher than the actual value so that
;  a value of 0 indicates nothing found (yet)
   btfss   flags_found,_GCODE
   bra   eval_done_G
   decfsz   GCode_New,w   ;fetch new G code
   bra   eval_no_G00
;G00 found - rapid move mode
   movlw   0+1
   movwf   GCode_Active   ;set active G code to [00]
   bra   eval_done_G
eval_no_G00
   decfsz   WREG,w
   bra   eval_no_G01
;G01 found - feed move mode
   movlw   1+1
   movwf   GCode_Active   ;set active G code to [01]
   bra   eval_done_G
eval_no_G01
;evaluate other G codes here
   bra   eval_other

eval_done_G
;test for an XY move
   movlw   1<<_XCOORD | 1<<_YCOORD | 1<<_ZCOORD | 1<<_ACOORD
   andwf   flags_found,w   ;test all 4 axes at once
   bz   eval_no_axis
;now G must have valid mode value
   movf   GCode_Active,w   ;grab current GCode (+1)
   bz   ERR_No_GCode   ;0 indicates not set
   decfsz   WREG,w      ;G00 = skip
   bra   eval_feed   ;branch if > G00
;set up a rapid move - no feedrate angle compensation required
   movff   FRate_Rapid+0,FRate_Active+0
   movff   FRate_Rapid+1,FRate_Active+1
   bra   eval_done_axis

;1 (or more) axis feed has been requested
eval_feed
;test for valid feed rate
   btfsc   FRate_New+1,7   ;if bit 7 set, no feed rate has been given
   bra   ERR_No_FRate
;NOTE: need to do feedrate angle compensation here
   movff   FRate_New+0,FRate_Active+0
   movff   FRate_New+1,FRate_Active+1

   decfsz   WREG,w      ;G01 = skip
   bra   eval_other
;at this point, G is valid, FRate_Active is valid, X,Y,Z and/or A found
eval_done_axis   
   bsf   flags,_NEWMOVE   ;set flag indicating new move is ready
eval_no_axis
   return   

;evaluate other G Code moves
eval_other
   _SERPrint "ERR: Unsupported G-Code. (G"
   decf   GCode_New,w   ;grab it
   rcall   Bin_2_BCD   ;convert it to 2 BCD digits
   rcall   SER_send_wreg   ;transmit 'em
   _SERPrint ")\n\r"
   return

ERR_No_GCode
   _SERPrint "ERR: G-Code mode not set.\n\r"
   return
   
ERR_No_FRate
   _SERPrint "ERR: Invalid Feed Rate.\n\r"
   return
;******************************************************************************
;echo whatever blocks were found back out serial port
echo_new_blocks
   movf   flags_found,f   ;test all flags at once
   bnz   echo_block   ;branch if something found
   _SERPrint "Nothing Found!\n\r"
   return

echo_block
;Check for G Code, display 2 digit code if found
   btfss   flags_found,_GCODE
   bra   echo_no_G
   movlw   'G'
   rcall   SERprint
   decf   GCode_New,w
   rcall   Bin_2_BCD
   rcall   SER_send_wreg
   movlw   ' '
   rcall   SERprint

echo_no_G
;check for x-coord, display 6 digits if found
   btfss   flags_found,_XCOORD
   bra   echo_no_X
   movlw   'X'
   rcall   SERprint
   btfss   flags_sign,_XCOORD   ;was the number negative?
   bra   echo_x_pos      ;branch if yes
   movlw   '-'         ;display a minus sign
   rcall   SERprint
echo_x_pos
   lfsr   0,XCoord_New+0   ;point to source
   rcall   Bin_24_BCD   ;convert it to 8 BCD digits
   lfsr   0,result+1   ;point to result
   rcall   SER_send_byte   ;send 100 thousands & 10 thousands
   rcall   SER_send_byte   ;send thousands and hundreds
   rcall   SER_send_byte   ;send tens and ones
   movlw   ' '
   rcall   SERprint

echo_no_X
;check for y-coord, display 6 digits if found
   btfss   flags_found,_YCOORD
   bra   echo_no_Y
   movlw   'Y'
   rcall   SERprint
   btfss   flags_sign,_YCOORD   ;was the number negative?
   bra   echo_Y_pos      ;branch if yes
   movlw   '-'         ;display a minus sign
   rcall   SERprint
echo_Y_pos
   lfsr   0,YCoord_New+0   ;point to source
   rcall   Bin_24_BCD   ;convert it to 8 BCD digits
   lfsr   0,result+1   ;point to result
   rcall   SER_send_byte   ;send 100 thousands & 10 thousands
   rcall   SER_send_byte   ;send thousands and hundreds
   rcall   SER_send_byte   ;send tens and ones
   movlw   ' '
   rcall   SERprint

echo_no_Y
;check for z-coord, display 6 digits if found
   btfss   flags_found,_ZCOORD
   bra   echo_no_Z
   movlw   'Z'
   rcall   SERprint
   btfss   flags_sign,_ZCOORD   ;was the number negative?
   bra   echo_Z_pos      ;branch if yes
   movlw   '-'         ;display a minus sign
   rcall   SERprint
echo_Z_pos
   lfsr   0,ZCoord_New+0   ;point to source
   rcall   Bin_24_BCD   ;convert it to 8 BCD digits
   lfsr   0,result+1   ;point to result
   rcall   SER_send_byte   ;send 100 thousands & 10 thousands
   rcall   SER_send_byte   ;send thousands and hundreds
   rcall   SER_send_byte   ;send tens and ones
   movlw   ' '
   rcall   SERprint

echo_no_Z
;check for a-coord, display 6 digits if found
   btfss   flags_found,_ACOORD
   bra   echo_no_A
   movlw   'A'
   rcall   SERprint
   btfss   flags_sign,_ACOORD   ;was the number negative?
   bra   echo_A_pos      ;branch if yes
   movlw   '-'         ;display a minus sign
   rcall   SERprint
echo_A_pos
   lfsr   0,ACoord_New+0   ;point to source
   rcall   Bin_24_BCD   ;convert it to 8 BCD digits
   lfsr   0,result+1   ;point to result
   rcall   SER_send_byte   ;send 100 thousands & 10 thousands
   rcall   SER_send_byte   ;send thousands and hundreds
   rcall   SER_send_byte   ;send tens and ones
   movlw   ' '
   rcall   SERprint

echo_no_A
;check for F code, display 5 digits if found
   btfss   flags_found,_FRATE
   bra   echo_no_F
   movlw   'F'
   rcall   SERprint
   lfsr   0,FRate_New+0   ;point to source
   rcall   Bin_16_BCD   ;convert it to 5 BCD digits
   lfsr   0,result+0   ;point to result
   movf   POSTINC0,w
   call   SER_send_nyb
   call   SER_send_byte   ;send thousands and hundreds
   call   SER_send_byte   ;send tens and ones

echo_no_F
   movlw   '\n'
   rcall   SERprint
   movlw   '\r'
   bra   SERprint
;   return


;******************************************************************************
;Look for a numeric value starting at FSR1
;A non-numeric digit terminates the numeric value
;Z is set if NO numeric value was found, clear otherwise
;On exit, FSR1 points to next available character, which could be EOL
;A minus sign starts a numeric value if it is followed by a digit
;Multiple minus signs in a row evaluate as expected (odd number is negative)
read_number
;keep grabbing chars until we find a digit, '-' or EOL
   bcf   flags,_NEGATIVE   ;cancel negative number
find_digit
   movf   POSTINC1,w   ;grab character
   bz   find_false   ;end of string?

   movwf   temp00      ;save it
   movlw   '-'      ;check for negative
   cpfseq   temp00
   bra   find_numeral   ;not '-' so look for numeral instead
   btg   flags,_NEGATIVE
   bra   find_digit   ;look for more
find_numeral
   movlw   '0'-1
   cpfsgt   temp00      ;skip if f > W
   bra   find_false   ;branch if char is less than digit
   movlw   '9'+1
   cpfslt   temp00      ;skip if f < W
   bra   find_false   ;branch if char is greater than digit
;found a digit - start of numeric value
   movf   temp00,w   ;recover character
   andlw   0x0F      ;strip hi bits to convert ascii to value
   movwf   result+0   ;save as first digit
   clrf   result+1
   clrf   result+2
   clrf   result+3

;look for more digits and stuff them into the value
;a non-digit or an EOL indicates end of numeric value
find_another
   movf   POSTINC1,w   ;grab another character
   bz   find_true   ;end of string?

   movwf   temp00      ;save it
   movlw   '0'-1
   cpfsgt   temp00      ;skip if f > W
   bra   find_true   ;done if char is less than digit
   movlw   '9'+1
   cpfslt   temp00      ;skip if f < W
   bra   find_true   ;done if char is greater than digit

   rcall   Times10      ;multiply previous result by 10
   movf   temp00,w   ;recover character
   andlw   0x0F      ;strip hi bits
   addwf   result+0,f   ;add new digit
   movlw   0
   addwfc   result+1,f   ;propagate carry
   addwfc   result+2,f
   addwfc   result+3,f
   bra   find_another   ;look for more digits

;found a numeric value so clear Z
find_true
   movf   POSTDEC1,f   ;shift pointer back to next char available
   bcf   STATUS,Z   ;clear Z to indicate numeric value found
   return

;didn't find a numeric value so set Z
find_false
   movf   POSTDEC1,f   ;shift pointer back to next char available
   bcf   flags,_NEGATIVE   ;cancel possible negative number
   bsf   STATUS,Z   ;set Z to indicate NO numeric value found
   return

;******************************************************************************
;Trim the binary number in result:4 to two digits
find_trim_2
   movf   result+3,w   ;any bits set in the upper bytes
   iorwf   result+2,w   ; means the result is too high
   iorwf   result+1,w   ; and must be clamped to 99
   movlw   99      ;load clamp value (flags unaffected)
   bnz   save_clamp_2   ;branch if result1:3 is NOT zero
   cpfsgt   result+0   ;IF f > W then keep 99 instead of orig value
   movf   result+0,w   ;get orig value instead
save_clamp_2
   movwf   result+0   ;save clamped value
   return
;******************************************************************************

;unrecognized command so report error
;cmd_error
;   _SERPrint "ERR - unrecognized command: "
;echo the received string
;   lfsr   0,cmd_buffer   ;point to buffer start

;SER_echo_string
;   movf   POSTINC0,w   ;grab character
;   bz   SER_echo_done
;   rcall   SERprint   ;send it
;   bra   SER_echo_string
;SER_echo_done
;   movlw   '\r'
;   rcall   SERprint
;   movlw   '\n'
;   bra   SERprint

;******************************************************************************
;Serial Port routines
;******************************************************************************
;send BCD bytes pointed to by FSR0 out serial port as 2
; ascii characters. Move FSR0 to next byte.
; expects high order bytes lower in memory
;send_wreg changes temp00
SER_send_wreg
   movwf   temp00
   lfsr   0,temp00
   bra   SER_send_byte
SER_send_lword
   rcall   SER_send_byte
SER_send_3byte
   rcall   SER_send_byte
SER_send_word
   rcall   SER_send_byte
SER_send_byte
   swapf   INDF0,w
   rcall   SER_send_nyb
   movf   POSTINC0,w
;   call   send_nyb
;******************************************************************************
;convert lo nibble to an ascii character
; $0 - $f to '0' - '9','A' - 'F'  (note '9' + 8 = 'A')
SER_send_nyb
   andlw   b'00001111'   ;strip hi bits
   addlw   -.10      ; Test to see if W < 10
   btfsc   STATUS,C   ; If yes, skip ahead
   addlw   'A'-'0'-.10   ; Add ASCII 'A', and negate next line
   addlw   '0'+.10      ; Add ASCII '0' and original 10
;   bra   SERprint

;******************************************************************************
;wait until the serial transmit reg is empty, then send w-reg
SERprint
   btfss   PIR1,TXIF   ;test if TXREG is empty
   goto   SERprint   ;loop until serial port ready
   nop
   movwf   TXREG
   return

;******************************************************************************
; send WREG as a binary string
; uses a rather brute force method - really ought to loop it
SER_send_bin
   movwf   temp00
   movlw   '0'>>1      ;preload shifted ascii char for '0'
   rlcf   temp00,f   ;roll bit into carry
   rlcf   WREG,w      ;roll bit into WREG forming 0x30 or 0x31
   rcall   SERprint   ;and serial print it
   movlw   b'00011000'
   rlcf   temp00,f
   rlcf   WREG,w
   rcall   SERprint
   movlw   b'00011000'
   rlcf   temp00,f
   rlcf   WREG,w
   rcall   SERprint
   movlw   b'00011000'
   rlcf   temp00,f
   rlcf   WREG,w
   rcall   SERprint
   movlw   b'00011000'
   rlcf   temp00,f
   rlcf   WREG,w
   rcall   SERprint
   movlw   b'00011000'
   rlcf   temp00,f
   rlcf   WREG,w
   rcall   SERprint
   movlw   b'00011000'
   rlcf   temp00,f
   rlcf   WREG,w
   rcall   SERprint
   movlw   b'00011000'
   rlcf   temp00,f
   rlcf   WREG,w
   bra   SERprint

;******************************************************************************
;Bin2BCD
;Enter:   binary number in w-reg
;Exit:   BCD number in w-reg, temp1 changed
;Convert (binary) number to BCD number
;Uses temp00 as workspace
;Note: values larger than 99 are converted to 99
;---------------------------------------------------
Bin_2_BCD
   movwf   temp00      ;save value
   movlw   99
   cpfsgt   temp00      ;IF f > W then keep 99 instead of orig value
   movf   temp00,w   ;recover value

   clrf   temp00
gtenth
   incf   temp00,f
   addlw   -10
   btfsc   STATUS,C   ;C clear if no overflow
   goto   gtenth
   decf   temp00,f   ;correct for MSD overshoot
   addlw   10      ;correct for LSD overshoot
   swapf   temp00,f   ;shift MSD to hi nibble
   iorwf   temp00,w   ;join with low nibble
   return

;******************************************************************************
;take 2 ascii chars pointed to by w and convert to 1 byte
;high nibble stored first
;value returned in WREG
;Z should be set if ascii byte is zero
ascii2byte
   movwf   FSR0
   swapf   INDF0,w      ;swap hi nibble into result
   btfsc   POSTINC0,6   ;check if in range 'A'-'F'
   addlw   0x8F      ;add correction swapf('1'-'A'+1)
   addwf   INDF0,w      ;add lo nibble
   btfsc   POSTINC0,6   ;check if in range 'A'-'F'
   addlw   0xF9      ;add correction '9'-'A'+1
   addlw   0xCD      ;adjust final result -0x33
   return

;******************************************************************************
;convert a 16 bit binary number to a 5 digit BCD
;Input:  INDF0 points to source 16 bits (low byte low mem)
;(Alt Entry: 16 bit number already in source+0,1)
;Output: result+2, result+1, result+0
;Used:   w-reg, count, temp, FSR, source+0,1
; Ex: the 16 bit binary number = FFFF
; After conversion the Decimal Number
; in result+0,result+1,result+2 = 06,55,35 (low byte hi mem)
;
Bin_16_BCD
   movff   POSTINC0,source+0   ; copy source binary value
   movff   POSTINC0,source+1   ;   to conversion
Bin_16_BCD_PL
   bcf   STATUS,0   ; clear the carry bit
   movlw   .16
   movwf   count
   clrf   result+0
   clrf   result+1
   clrf   result+2
loop16   rlcf   source+0,f
   rlcf   source+1,f
   rlcf   result+2,f
   rlcf   result+1,f
   rlcf   result+0,f

   dcfsnz   count,f
   retlw   0

adj16DEC
   lfsr   0,result+2
   call   adjBCD
   call   adjBCD
   call   adjBCD

   goto   loop16
;******************************************************************************
;convert a 24 bit binary number to an 8 digit BCD
;Input:  INDF0 points to source 24 bits (low byte low mem)
;(Alt Entry: 24 bit number already in source+0,1,2)
;Output: result+3, result+2, result+1, result+0
;Used:   w-reg, count, FSR0, source+0,1,2
; Ex: the 24 bit binary number = FFFFFF
; After conversion the Decimal Number
; in result+0,result+1,result+2,result+3 = 16,77,72,15 (low byte hi mem)

Bin_24_BCD
   movff   POSTINC0,source+0   ; copy source binary value
   movff   POSTINC0,source+1   ;   to conversion
   movff   POSTINC0,source+2   ;    work space
Bin_24_BCD_PL
   movlw   .24      ;number of bits to do
   movwf   count
   bcf   STATUS,0   ; clear the carry bit
   clrf   result+0
   clrf   result+1
   clrf   result+2
   clrf   result+3
loop24   rlcf   source+0,f
   rlcf   source+1,f
   rlcf   source+2,f
   rlcf   result+3,f
   rlcf   result+2,f
   rlcf   result+1,f
   rlcf   result+0,f

   dcfsnz   count,f
   retlw   0

adj24DEC
   lfsr   0,result+3
   rcall   adjBCD
   rcall   adjBCD
   rcall   adjBCD
   rcall   adjBCD
   bra   loop24

adjBCD   movlw   0x33      
   addwf   INDF0,f      ; add to both nybbles
   btfsc   INDF0,3      ; test if low result › 7
   andlw   0xf0      ; low result ? so take the 3 out
   btfsc   INDF0,7      ; test if high result › 7
   andlw   0x0f      ; high result › 7 so ok
   subwf   POSTDEC0,f   ; any results ‹= 7, subtract back & point to next
   retlw   0

;******************************************************************************
;  SERPutString - send an in-line string out the serial port via Stack and TBLPTR
;
;  string must be terminated with a 00 byte and does not need
;  to be word aligned
;
SERPutString
        movff   TOSH,TBLPTRH   ; copy return address to TBLPTR
        movff   TOSL,TBLPTRL   ;
        clrf    TBLPTRU      ; assume PIC with < 64-KB
SERPutNext
        tblrd   *+      ; get in-line string character
        movf    TABLAT,w   ; last character (00)?
        bz      SERPutExit   ; yes, exit, else
        rcall   SERprint   ; print character
        bra     SERPutNext   ; and do another
SERPutExit
        btfsc   TBLPTRL,0   ; odd address?
        tblrd   *+      ; yes, make it even (fix PC)
        movf    TBLPTRH,w   ; setup new return address
        movwf   TOSH      ;
        movf    TBLPTRL,w   ;
        movwf   TOSL      ;
        return         ;
;

;******************************************************************************
;multiply 32 bit number (stored in result:4) by 10
;inline code, uses no extra memory
Times10
   movf   result+0,w   ;grab 1st digit
   mullw   10
   movf   result+1,w   ;rescue 2nd digit
   movff   PRODL,result+0   ;save result
   movff   PRODH,result+1

   mullw   10
   movf   PRODL,w      ;grab result low byte
   addwf   result+1,f   ;add to highest previous byte
   movlw   0
   addwfc   PRODH,f      ;propagate the carry
   movf   result+2,w   ;rescue 3rd digit
   movff   PRODH,result+2

   mullw   10
   movf   PRODL,w      ;grab result low byte
   addwfc   result+2,f   ;add to highest previous byte, plus any carry
   movlw   0
   addwfc   PRODH,f      ;propagate the carry
   movf   result+3,w   ;rescue 4th digit
   movff   PRODH,result+3
   
   mullw   10      ;don't care about overflow byte
   movf   PRODL,w      ;grab result low byte
   addwfc   result+3,f   ;add to highest previous byte, plus any carry
   
   return

;******************************************************************************
; pause for roughly 1 second
pause1sec
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
   call   pause100ms
;******************************************************************************
; pause for 100 millisecs
pause100ms
   movlw   .100
;******************************************************************************
; pause for w millisecs
pausewms
   movwf   pausecount+1
pause100ms_2
   movlw   .10
   call   pause100uS
   decfsz   pausecount+1,f
   goto   pause100ms_2
   nop
   return

;******************************************************************************
; pause for w00 microsecs (4x10MHz)
; It would be wise to rewrite this so that clock speed is used to determine
; delay constants, instead of using some random constant.
pause100uS
;   return
   movwf   pausecount+0      ;save pause time
pause100uS_2
   movlw   .249
pause100uS_3
   nop
   nop
   decfsz   WREG,f
   bra   pause100uS_3
   decfsz   pausecount+0,f
   bra   pause100uS_2
   return

;******************************************************************************
Splash_Help
   _SERPrint "\n\n\r"
   _SERPrint "|-------------------------------------------------------------\n\r"
   _SERPrint "|  VelociPIC - Stepper Motor Control with Linear Speed Ramps\n\r"
   call   SERPutString
   db     "|  (version: " version ")\n\r",0
   _SERPrint "|Implements a rudimentary G Code interpreter.\n\r"
   _SERPrint "|G00, G01 (Incremental mode only)\n\r"
   _SERPrint "|X Y Z A  - Axis move (range: -16M to 16M steps)\n\r"
   _SERPrint "|   (At present, X is mapped to Z, Y to A, Z & A ignored)\n\r"
   _SERPrint "|F [data] - Set feed rate (range: 0 to 32767)\n\r"
   _SERPrint "|; / (    - Comments ignored\n\r"
   _SERPrint "|N [data] - Line numbers ignored\n\r"
   _SERPrint "|Values after decimal points ignored\n\r"
   _SERPrint "|-------------------------------------------------------------\n\r"
   return

;******************************************************************************
;End of program

   END
/vp
vegipete
 
Posts: 37
Joined: Sat Jul 09, 2011 7:38 pm

Re: Simple Source Code

Postby michalsok » Mon Jun 04, 2012 5:24 am

Hello,
Thank you for sharing your work.
I have one question (for now anyway).
Does the note "EDIT: There are some bugs in the sample code below..." in the first post apply to the second code in post 17?
In other words, have the bugs been addressed and fixed in the code in post 17 or do I need to try to debug it?

Thanks,

Michalsok
michalsok
 
Posts: 1
Joined: Mon Jun 04, 2012 5:18 am

Re: Simple Source Code

Postby vegipete » Tue Jun 05, 2012 10:04 pm

michalsok wrote:In other words, have the bugs been addressed and fixed in the code in post 17 or do I need to try to debug it?
Yes, the code in post 17 has the bugs fixed. (I found two bugs: 1] single step moves became 32 bits long - fixed by manually generating the single step without starting the ISR and 2] moves in which the acceleration state was completed before the first step was generated didn't go the correct distance, wrapping instead past 32 bits. This second bug was corrected with a rather inelegant brute force kludge needing an extra flag to track the first step.)

This particular branch of code was more proof of concept of the real time stepper speed ramp algorithms than a useful end product. Feel free to experiment with it as you please but ask away if you need specifics.

I have newer/better versions that I haven't posted yet. Development is ongoing...

I've been building a PIC24 based version which has identified a few neat improvements that I must fold back into the PIC18 version.

/vp
/vp
vegipete
 
Posts: 37
Joined: Sat Jul 09, 2011 7:38 pm

Re: Simple Source Code

Postby andy123 » Mon Feb 04, 2013 5:13 pm

Hi
I was searching around for LCD output routines for a project I am doing using PBP3 basic for the bulk of the code. The PICBASIC LCD input routines run too slow for my update loops to execute in the time frame required. Anyhow, I became intrigued with you G code stepper drive program - BOBStep- which inputs and outputs commands through the serial link to PC and tried running it through my Microcode IDE editor with the ASM - ENDASM options turned on to process the assembler code and send it straight to the assembler, (MPASM).
The error listings returned "ASM ERROR progra01 overwriting previous address contents" for large sections of code.
Question; Is the code embedded in the code window in the proper sequence with the ORG statements in the right order?
It's quite a while since I used much assembler coding with PIC 16F84's (still a more distant memory coding INTEL 8085's in assembler).
Just a few hint to point me in the right direction would be much appreciated.
Thank you
andy123
 
Posts: 6
Joined: Mon Feb 04, 2013 4:39 pm

Next

Return to Stepper Speed Ramps

Who is online

Users browsing this forum: No registered users and 1 guest