move.asm
;
;
; FILE NAME MOVE.ASM
;
;------------------ MOVE RELATED ROUTINES ---------------------------------
;
;
; SynPet Personal Electronic Technologies
; 7225 Franklin Road, Boise, ID 83709
;
; Created: 9/5/89 - LRE
; Edit: 12/31/89 - LRE Started adding follow wall stuff.
; Separate .ASM WALLFOL.ASM.
; Edit: 2/6/90 - LRE Split off MOVDEFS.ASM and
; MOVCNTRL.ASN.
;
;
;---------------------------------------------------------------------------
; FILE CONTENTS IN ORDER OF APEARANCE:
;
; INIT_MOVE
; Int handlers split off into MOVCNTRL.ASM 2/6/90
; RIGHT_MOTOR_OPTO_INT_HANDLER
; LEFT_MOTOR_OPTO_INT_HANDLER
; RIGHT_MOTOR_PWM_INT_HANDLER
; LEFT_MOTOR_PWM_INT_HANDLER
;
; START_RIGHT_MOTOR
; START_LEFT_MOTOR
; STOP_RIGHT_MOTOR
; STOP_LEFT_MOTOR
; CALC_PIVOT_COUNT
; CALC_DEGREES_ROTATED
; CALC_DISTANCE_COUNTS
; CALC_DICTANCE_MOVED
; MOVE_MAIN
; SETUP_N_START_ROTATE
; CONTINUE_ROTATE
; START_SL_MOVE
; CONTINUE_SL_MOVE
; PAUSE_R_N_M_MOVE
; R_N_M_PAUSED
; TWO MORE FILES ARE INCLUDED WHEN ASSEMBLING THIS FILE TO COMPLETE
; THE MOVE ROBOT ROUTINES. THE INCLUDE FILES ARE:
;
; ARCMOVES.ASM
; MOVUTILS.ASM
;
; SEE THE INCLUDE FILES FOR OTHER MOVE ROUTINES.
.chip 16003
.incld GLOBDEFS
.incld MOVEDEFS
.sect DATA,ram8
;---------------- MODULE PRIVATE VARIABLES --------------------------------
; NOTE: These varibles are also used in MOVCNTRL.ASM
.public left_on_sl_control
left_on_sl_control: .DSB 1 ;
.public right_on_sl_control
right_on_sl_control: .DSB 1 ;
.public left_on_control
left_on_control: .DSB 1 ;
.public right_on_control
right_on_control: .DSB 1 ;
.public right_motor_on
right_motor_on: .DSB 1 ;
.public right_motor_direction
right_motor_direction: .DSB 1 ;
.public right_move_tic
right_move_tic: .DSB 1 ;
.public left_motor_on
left_motor_on: .DSB 1 ;
.public left_motor_direction
left_motor_direction: .DSB 1 ;
.public left_move_tic
left_move_tic: .DSB 1 ;
waiting_for_move_tx: .DSB 1 ;
degrees_hi: .DSB 1 ;
degrees_lo: .DSB 1 ;
.public ft_hi
ft_hi: .DSB 1 ;
.public ft_lo
ft_lo: .DSB 1 ;
radius: .DSB 1 ;
move_cancel_byte: .DSB 1 ;
degrees_moved_hi: .DSB 1 ;
degrees_moved_lo: .DSB 1 ;
distance_moved_hi: .DSB 1 ;
distance_moved_lo: .DSB 1 ;
.public arc_abort_data_lo
arc_abort_data_lo: .DSB 1 ;
.public arc_abort_data_hi
arc_abort_data_hi: .DSB 1 ;
move_paused_state: .DSB 1 ;
right_motor_stall_limit: .DSB 1 ;
left_motor_stall_limit: .DSB 1 ;
.public speed
speed: .DSB 1 ;
.public stay_on
stay_on: .DSB 1 ;
.public control_motor
control_motor: .DSB 1 ;
.public outside_cnt_ext
outside_cnt_ext: .DSB 1 ;
.public inside_cnt_ext
inside_cnt_ext: .DSB 1 ;
.public adjusted_outside_cnt_ext
adjusted_outside_cnt_ext: .DSB 1 ;
.public arc_direction
arc_direction: .DSB 1 ;
.public fwd_or_rev_arc
fwd_or_rev_arc: .DSB 1 ;
move_cancel_found: .DSB 1 ;
move_jobs_in_q_copy: .DSB 1 ;
.public last_ramp_tic
last_ramp_tic: .DSB 1 ;
.public last_left_ramp_tic
last_left_ramp_tic: .DSB 1 ;
.public last_right_ramp_tic
last_right_ramp_tic: .DSB 1 ;
.public both_motors_on
both_motors_on: .DSB 1 ;
temp_left_ext: .DSB 1 ;
temp_right_ext: .DSB 1 ;
.public left_stop_flag
left_stop_flag: .DSB 1 ;
.public right_stop_flag
right_stop_flag: .DSB 1 ;
.public left_control_state
left_control_state: .DSB 1 ;
.public right_control_state
right_control_state: .DSB 1 ;
.public rotate_flag
rotate_flag: .DSB 1 ;
.public stop_flag
stop_flag: .DSB 1 ;
.public left_ramp_delta_lo
left_ramp_delta_lo: .DSB 1 ;
.public left_ramp_delta_hi
left_ramp_delta_hi: .DSB 1 ;
.public right_ramp_delta_lo
right_ramp_delta_lo: .DSB 1 ;
.public right_ramp_delta_hi
right_ramp_delta_hi: .DSB 1 ;
.public move_process_buff
move_process_buff: .DSB MAX_MSG_LENGTH ;
move_msg_loop_cnt: .DSB 1 ;
move_error_msg_code: .DSB 1 ;
move_error_msg_job_number: .DSB 1 ;
.public left_move_complete
left_move_complete: .DSB 1 ;
.public right_move_complete
right_move_complete: .DSB 1 ;
paused_stay_on: .DSB 1 ;
paused_right_cnts_to_move_lo: .DSB 1 ;
paused_right_cnts_to_move_hi: .DSB 1 ;
paused_left_cnts_to_move_lo: .DSB 1 ;
paused_left_cnts_to_move_hi: .DSB 1 ;
.public left_stopping
left_stopping: .DSB 1 ;
.public right_stopping
right_stopping: .DSB 1 ;
; .public right_noise_lo
; .public right_noise_hi
; .public left_noise_lo
; .public left_noise_hi
; .public reset_cnt
.public right_speed_too_hi
.public right_speed_too_lo
.public left_speed_too_hi
.public left_speed_too_lo
.public right_iret_error
.public left_iret_error
.public right_t0_ff
.public left_t0_ff
.public right_long_noise
.public left_long_noise
.public right_wd_hits
.public left_wd_hits
.public right_too_fast_cnt
.public left_too_fast_cnt
.public right_slowing
.public left_slowing
.public speed_slow_down_limit
.public right_dec_cnt
.public left_dec_cnt
.public right_timer_stop
.public left_timer_stop
; right_noise_lo: .DSB 1 ;
; right_noise_hi: .DSB 1 ;
; left_noise_lo: .DSB 1 ;
; left_noise_hi: .DSB 1 ;
; reset_cnt: .DSB 1 ;
right_speed_too_hi: .DSB 1 ;
right_speed_too_lo: .DSB 1 ;
left_speed_too_hi: .DSB 1 ;
left_speed_too_lo: .DSB 1 ;
right_iret_error: .DSB 1 ;
left_iret_error: .DSB 1 ;
right_t0_ff: .DSB 1 ;
left_t0_ff: .DSB 1 ;
right_long_noise: .DSB 1 ;
left_long_noise: .DSB 1 ;
right_wd_hits: .DSB 1 ;
left_wd_hits: .DSB 1 ;
right_too_fast_cnt: .DSB 1 ;
left_too_fast_cnt: .DSB 1 ;
right_slowing: .DSB 1 ;
left_slowing: .DSB 1 ;
speed_slow_down_limit: .DSB 1 ;
right_dec_cnt: .DSB 1 ;
left_dec_cnt: .DSB 1 ;
right_timer_stop: .DSB 1 ;
left_timer_stop: .DSB 1 ;
.public right_tilt_cnt
right_tilt_cnt: .DSB 1 ;
.public left_tilt_cnt
left_tilt_cnt: .DSB 1 ;
;--------------- END OF MODULE PRIVATE VARIABLES --------------------------
;-------------------- EXTERNAL GLOBAL VARIABLES ---------------------------
.extrn job_zero_out_q
.extrn job_zero_in_q
.extrn job_in_q_btm_ptrs
.extrn job_state
.extrn buffer_full_errors
.extrn msg_too_long_errors
.extrn jobs_in_q
.extrn total_jobs_in_qs
.extrn job_out_q_top_ptrs
.extrn job_out_q_btm_ptrs
.extrn msgs_to_send
.extrn total_msgs_to_send
.extrn right_motor_stall_cnt
.extrn right_motor_cnts_moved_ext
.extrn right_motor_cnts_to_move_ext
.extrn left_motor_stall_cnt
.extrn left_motor_cnts_moved_ext
.extrn left_motor_cnts_to_move_ext
.extrn asleep
.extrn tilt_enable
.extrn sonar_disable
.extrn sonar_disable_mask
.extrn sonar_on_off
.extrn cancel_in_q
.extrn continue_in_q
.extrn cancel_all
.extrn pause_job
.extrn job_paused
.extrn ramp_tic
.extrn left_t0_tic
.extrn right_t0_tic
.extrn left_t0_tic_pend
.extrn right_t0_tic_pend
.extrn sonar_status
.extrn sonar_1_min_limit_lo
.extrn sonar_1_min_limit_hi
.extrn sonar_2_min_limit_lo
.extrn sonar_2_min_limit_hi
.extrn sonar_3_min_limit_lo
.extrn sonar_3_min_limit_hi
.extrn sonar_4_min_limit_lo
.extrn sonar_4_min_limit_hi
.extrn sonar_1_max_limit_lo
.extrn sonar_1_max_limit_hi
.extrn sonar_2_max_limit_lo
.extrn sonar_2_max_limit_hi
.extrn sonar_3_max_limit_lo
.extrn sonar_3_max_limit_hi
.extrn sonar_4_max_limit_lo
.extrn sonar_4_max_limit_hi
.extrn move_reply_process_buff
.extrn intrn_reply_ids
.extrn on_remote_control
;----------------- END OF EXTERNAL GLOBAL VARIABLES -----------------------
.endsect
;---------------- START OF MODULE CODE -----------------------------------
.sect HIMEM,rom8
; .sect LOMEM,rom8
; .sect CODE,rom8
.public MOVE_MAIN
.public INIT_MOVE
.extrn FOLLOW_WALL
.public MOVE_MSG_LENGTH_ERROR
.public SND_ARC_ABORT_MSG
.public SND_SUCCESS_MSG
.public STOP_RIGHT_MOTOR
.public STOP_LEFT_MOTOR
.extrn RIGHT_MOTOR_OPTO_INT_HANDLER
.extrn LEFT_MOTOR_OPTO_INT_HANDLER
;--------------- START OF MOVE INITIALIZATION FUNCTION ---------------------
INIT_MOVE:
; Irq 3 & irq 4 are initially set zero. Interrupt on falling edge.
; Set to rising edge for limited noise rejection stuff.
SBIT 3,IRCD.B ;
.ifdef OLD_INTS
SBIT 4,IRCD.B ;
.else
SBIT 2,IRCD.B ;
.endif
LD IRPD.B,#RIGHT_MOTOR_CLR_PEND_MASK ;
LD IRPD.B,#LEFT_MOTOR_CLR_PEND_MASK ;
SBIT LEFT_MOTOR_DIRECTION_BIT,DIRB_HI.B ; Make pins outputs.
SBIT RIGHT_MOTOR_DIRECTION_BIT,DIRB_HI.B ;
; Don't care which direction set right now.
RBIT LEFT_MOTOR_TOGGLE_BIT,PORTP_LO.B ; Turn off left motor.
RBIT LEFT_MOTOR_PIN_BIT,PORTP_LO.B ;
RBIT RIGHT_MOTOR_TOGGLE_BIT,PORTP_LO.B ; Turn off right motor.
RBIT RIGHT_MOTOR_PIN_BIT,PORTP_LO.B ;
; Load the default sonar limits.
LD sonar_1_min_limit_lo.B,#DEF_SON_1_MIN_LIM_LO ;
LD sonar_1_min_limit_hi.B,#DEF_SON_1_MIN_LIM_HI ;
LD sonar_2_min_limit_lo.B,#DEF_SON_2_MIN_LIM_LO ;
LD sonar_2_min_limit_hi.B,#DEF_SON_2_MIN_LIM_HI ;
LD sonar_3_min_limit_lo.B,#DEF_SON_3_MIN_LIM_LO ;
LD sonar_3_min_limit_hi.B,#DEF_SON_3_MIN_LIM_HI ;
LD sonar_4_min_limit_lo.B,#DEF_SON_4_MIN_LIM_LO ;
LD sonar_4_min_limit_hi.B,#DEF_SON_4_MIN_LIM_HI ;
LD sonar_1_max_limit_lo.B,#DEF_SON_1_MAX_LIM_LO ;
LD sonar_1_max_limit_hi.B,#DEF_SON_1_MAX_LIM_HI ;
LD sonar_2_max_limit_lo.B,#DEF_SON_2_MAX_LIM_LO ;
LD sonar_2_max_limit_hi.B,#DEF_SON_2_MAX_LIM_HI ;
LD sonar_3_max_limit_lo.B,#DEF_SON_3_MAX_LIM_LO ;
LD sonar_3_max_limit_hi.B,#DEF_SON_3_MAX_LIM_HI ;
LD sonar_4_max_limit_lo.B,#DEF_SON_4_MAX_LIM_LO ;
LD sonar_4_max_limit_hi.B,#DEF_SON_4_MAX_LIM_HI ;
LD sonar_disable_mask.B,#0FFH ;
RET ;
;----------------- END OF MOVE INITIALIZATION FUNCTION ---------------------
;----------- START OF SUBROUTINE TO START RIGHT MOTOR -------------------
START_RIGHT_MOTOR:
SBIT RIGHT_MOTOR_DIRECTION_BIT,PORT_B_HI.B ; Assume forward.
IFEQ right_motor_direction,#REV ; Actually reverse?
RBIT RIGHT_MOTOR_DIRECTION_BIT,PORT_B_HI.B ; Yes - set reverse.
LD speed_slow_down_limit,#SPEED_SLOW_DOWN_MISMATCH_LIMIT ;
LD right_dec_cnt,#0 ;
LD right_timer_stop,#0 ;
LD right_too_fast_cnt,#0 ;
LD right_move_tic,#0 ;
LD right_tilt_cnt,#0 ;
LD right_move_complete,#0 ;
LD right_stopping,#0 ;
LD right_stop_flag,#0 ;
LD right_motor_on,#0FFH ; Show motor running.
LD right_motor_stall_cnt.B,#0 ;
LD ramp_tic.B,#0 ; Just starting.
LD last_ramp_tic,#0 ;
LD last_right_ramp_tic,#0 ;
LD right_on_control,#0 ;
LD right_t0_tic.B,#2 ; Will force a load of capture.
LD right_t0_tic_pend.B,#0 ;
LD BP_right_speed.W,#0FFFFH ;
LD A,BP_right_set_pt.W ;
ADD A,#1000 ;
LD RIGHT_MOTOR_TIMER_REG.W,#A ;
LD RIGHT_MOTOR_PWM_LOAD_REG.W,#A ;
RBIT RIGHT_MOTOR_TOGGLE_BIT,PORTP_LO.B ;
SBIT RIGHT_MOTOR_PIN_BIT,PORTP_LO.B ; Set on to start.
SBIT RIGHT_MOTOR_ACK_BIT,PWMODE_LO.B ; Clr any pending.
SBIT RIGHT_MOTOR_PWM_INT_EN_BIT,PWMODE_LO.B ; Enable pwm int.
RBIT RIGHT_MOTOR_PWM_STP_BIT,PWMODE_LO.B ; & Start the timer.
RET ; Right hand motor is started.
;----------- END OF SUBROUTINE TO START RIGHT MOTOR -------------------
;----------- START OF SUBROUTINE TO START LEFT MOTOR -------------------
START_LEFT_MOTOR:
SBIT LEFT_MOTOR_DIRECTION_BIT,PORT_B_HI.B ; Assume forward.
IFEQ left_motor_direction,#REV ; Actually reverse?
RBIT LEFT_MOTOR_DIRECTION_BIT,PORT_B_HI.B ; Yes - set reverse.
LD speed_slow_down_limit,#SPEED_SLOW_DOWN_MISMATCH_LIMIT ;
LD left_dec_cnt,#0 ;
LD left_timer_stop,#0 ;
LD left_too_fast_cnt,#0 ;
LD left_move_tic,#0 ;
LD left_tilt_cnt,#0 ;
LD left_move_complete,#0 ;
LD left_stopping,#0 ;
LD left_stop_flag,#0 ;
LD left_motor_on,#0FFH ; Show motor running.
LD left_motor_stall_cnt.B,#0 ;
LD ramp_tic.B,#0 ; Just starting.
LD last_ramp_tic,#0 ;
LD last_left_ramp_tic,#0 ;
LD left_on_control,#0 ;
LD left_t0_tic.B,#2 ; Will force a load of capture.
LD left_t0_tic_pend.B,#0 ;
LD BP_left_speed.W,#0FFFFH ;
LD A,BP_left_set_pt.W ;
ADD A,#1000 ;
LD LEFT_MOTOR_TIMER_REG.W,#A ;
LD LEFT_MOTOR_PWM_LOAD_REG.W,#A ;
RBIT LEFT_MOTOR_TOGGLE_BIT,PORTP_LO.B ;
SBIT LEFT_MOTOR_PIN_BIT,PORTP_LO.B ; Set on to start.
SBIT LEFT_MOTOR_ACK_BIT,PWMODE_LO.B ; Clr any pending.
SBIT LEFT_MOTOR_PWM_INT_EN_BIT,PWMODE_LO.B ; Enable pwm int.
RBIT LEFT_MOTOR_PWM_STP_BIT,PWMODE_LO.B ; & Start the timer.
RET ; Right hand motor is started.
;----------- END OF SUBROUTINE TO START LEFT MOTOR -------------------
;------------ START OF SUBROUTINE TO STOP RIGHT MOTOR -----------------
STOP_RIGHT_MOTOR:
RBIT RIGHT_MOTOR_PWM_INT_EN_BIT,PWMODE_LO.B ; Disable the pwm int.
SBIT RIGHT_MOTOR_PWM_STP_BIT,PWMODE_LO.B ; & stop the timer.
NOP ; Delay for any pending.
NOP ;
SBIT RIGHT_MOTOR_ACK_BIT,PWMODE_LO.B ; & clear any pending.
RBIT RIGHT_MOTOR_TOGGLE_BIT,PORTP_LO.B ; Make pin latched output.
RBIT RIGHT_MOTOR_PIN_BIT,PORTP_LO.B ; Turns off motor.
LD right_motor_on,#0 ; Show motor is stopped.
LD both_motors_on,#0 ;
LD right_on_control,#0 ;
RET ; Motor is turned off.
;------------ END OF SUBROUTINE TO STOP RIGHT MOTOR -----------------
;------------ START OF SUBROUTINE TO STOP LEFT MOTOR -----------------
STOP_LEFT_MOTOR:
RBIT LEFT_MOTOR_PWM_INT_EN_BIT,PWMODE_LO.B ; Disable the pwm int.
SBIT LEFT_MOTOR_PWM_STP_BIT,PWMODE_LO.B ; & stop the timer.
NOP ; Delay for any pending.
NOP ;
SBIT LEFT_MOTOR_ACK_BIT,PWMODE_LO.B ; & clear any pending.
RBIT LEFT_MOTOR_TOGGLE_BIT,PORTP_LO.B ; Make pin latched output.
RBIT LEFT_MOTOR_PIN_BIT,PORTP_LO.B ; Turns off motor.
LD left_motor_on,#0 ; Show motor is stopped.
LD both_motors_on,#0 ;
LD left_on_control,#0 ;
RET ; Motor is turned off.
;------------ END OF SUBROUTINE TO STOP LEFT MOTOR -----------------
;------ START OF SUBROUTINE TO CALC PIVOT COUNT FOR OPTION ZERO --------
CALC_PIVOT_COUNT:
; Enter with degrees to pivot in A. Sets up all variables
; ready to start motors turning.
LD stay_on,#0 ;
IFBIT 7,A_REG_HI.B ; Negative degrees?
JMP ROTATE_RIGHT ; Yes - go clockwise.
; Otherwise fall thru to left (CCW) ;
LD right_motor_direction,#FWD ; Right motor forward.
LD left_motor_direction,#REV ; Left motor reverse.
JMP PIVOT_COUNT_CALC ;
ROTATE_RIGHT:
LD right_motor_direction,#REV ; Right motor reverse.
LD left_motor_direction,#FWD ; Left motor forward.
COMP A ; Turn degrees positive.
INC A ; Two's complement.
PIVOT_COUNT_CALC:
; Counts to turn = degrees*(PIVOT_CNT_CALC_MULT/1000)
MULT A,#PIVOT_CNT_CALC_MULT ;
DIVD A,#1000 ;
PUSH X ; This assembler does funny things with X, so want
POP B ; remainder in B for round off.
IFGT B,#500 ; Round up?
INC A ; Yes.
PUSH A ;
SHR A ;
COMP A ;
INC A ;
LD BP_right_motor_slow_down.W,A ;
LD BP_left_motor_slow_down.W,A ;
POP A ;
COMP A ; Make negative for interrupt routine.
INC A ;
LD BP_right_motor_cnts_to_move.W,A ; Set the count down.
LD BP_left_motor_cnts_to_move.W,A ;
LD BP_right_motor_cnts_moved.W,#0 ; Haven't moved yet.
LD BP_left_motor_cnts_moved.W,#0 ; Haven't moved yet.
RET ; Ready to start up motors.
;----- END OF SUBROUTINE TO SET UP PIVOT FOR OPTION ZERO -----------------
;-------- START OF SUBROUTINE TO CALCULATE DEGREES ROTATED ---------------
CALC_DEGREES_ROTATED:
LD A,BP_right_motor_cnts_moved.W ; Assume both turned same.
; Can only rotate 360 degrees max so don't need 24 bit ext.
MULT A,#1000 ; Inverse of calc counts.
DIVD A,#PIVOT_CNT_CALC_MULT ;
PUSH X ;
POP B ;
IFGT B,#DEG_PIVOTED_RND_OFF ;
INC A ;
IFBIT 7,degrees_hi ; Commanded negative?
JMP NEGATE_DEGREES_ROTATED ;
LD_DEGREES_ROTATED:
LD degrees_moved_hi,A_REG_HI.B ;
LD degrees_moved_lo,A_REG_LO.B ;
RET ; All done.
NEGATE_DEGREES_ROTATED:
COMP A ;
INC A ;
JMP LD_DEGREES_ROTATED ;
;--------- END OF SUBROUTINE TO CALCULATE DEGREES ROTATED ---------------
;------- START OF SUBROUTINE TO CALC COUNTS FOR A GIVEN DISTANCE ---------
CALC_DISTANCE_COUNTS:
; Enter with tenths of feet to move in A.
; Moves DIST_CALC_DIVISOR/10000 ft per opto interrupt.
; Moves DIST_CALC_DIVISOR/1000 tenths of a foot per opto int.
; Mult by 10,000 is max for 16 * 16 bit multiply.
; i.e. 100,000 is to big & don't want to get too tricky here.
; (tenths of feet) * 10,000 / DISTANCE_CALC_DIVISOR = counts to move.
IFBIT 7,A_REG_HI.B ; Negative value?
JMP MAKE_POSITIVE ;
IFEQ A,#07FFF ; Run forever distance?
JMP TURN_ON_INDEFINITE ;
LD stay_on,#0 ;
JMP DO_DIST_CNT_CALC ;
MAKE_POSITIVE:
COMP A ; Make positive.
INC A ; Two's comp.
IFGT A,#100 ; Not going to move more than 10 feet backwards.
LD A,#100 ; Just override any over 10 feet.
DO_DIST_CNT_CALC:
IFGT A,#DIST_CALC_MAX_DIST_CHK ; Going to keep cnts to move in one word.
LD A,#DIST_CALC_MAX_DIST_CHK ;
MULT A,#10000 ;
PUSH A ; Save lo word of result.
PUSH X ; Need hi word of result in A & this assemble won't
POP A ; do a LD A,X.
; Can't use regular DIVD 'cause result might not fit into
; 16 bit result.
DIV A,#DIST_CALC_DIVISOR ; Divide hi word.
LD B,A ; Result is 24 bit cnt extension - save. X has remainder.
POP A ; Lo word of distance * 100,000 into A.
DIVD A,#DIST_CALC_DIVISOR ; 16 bit counts to move in A.
PUSH B ;
PUSH X ; Get remainder into B for round off.
POP B ;
IFGT B,#DIST_CALC_RND_OFF ;
ADD A,#1 ;
POP B ;
IFC ; Carry on round off?
INC B ;
COMP A ; Make negative for interrupt routine.
XOR B,#-1 ;
INC A ;
IFGT A,#0 ;
RET ;
INC B ;
RET ; All done. A has lo word of cnts to move.
; B has 24 bit extension byte.
TURN_ON_INDEFINITE:
LD stay_on,#0FFH ;
RET ;
;-------- END OF SUBROUTINE TO CALC COUNTS FOR A GIVEN DISTANCE ---------
;----- START OF SUBROUTINE TO CALC DISTANCE MOVED FOR GIVEN COUNTS ------
CALC_DISTANCE_MOVED:
; Enter with B = lo word of counts moved.
; & with A = zero extended count extension.
; Inverse of calc counts.
MULT A,#DIST_CALC_DIVISOR ; Multiply extension.
PUSH A ; Save result - won't be over 16 bits.
LD A,B ; Now do lo word.
MULT A,#DIST_CALC_DIVISOR ;
POP B ; Double precision stuff.
PUSH A ;
PUSH X ;
POP A ;
ADD A,B ;
PUSH A ;
POP X ;
POP A ;
DIVD A,#10000 ; Finally have feet in A.
PUSH X ;
POP B ;
IFGT B,#5000 ;
INC A ;
IFBIT 7,ft_hi ; Negative command?
JMP NEGATE_DISTANCE_MOVED ;
LD_DISTANCE_MOVED:
LD distance_moved_hi,A_REG_HI.B ;
LD distance_moved_lo,A_REG_LO.B ;
RET ; All done.
NEGATE_DISTANCE_MOVED:
COMP A ;
INC A ;
JMP LD_DISTANCE_MOVED ;
;------ END OF SUBROUTINE TO CALC DISTANCE MOVED FOR GIVEN COUNTS ------
;-------------- START OF MOVE MAIN ROUTINE ------------------------------
MOVE_MAIN:
IFGT waiting_for_move_tx,#0 ; Waiting to send last msg??
JMP TRY_MOVE_TX_AGAIN ;
IFGT buffer_full_errors + 0.B,#0 ; Command didn't make it?
JSR SND_MOVE_BUFFER_FULL_ERROR ; If so tell 'em.
IFGT msg_too_long_errors + 0.B,#0 ;
JSR MOVE_MSG_TOO_LONG_ERROR ;
IFEQ jobs_in_q + 0.B,#0 ;
RET ;
IFGT job_state + 0.B,#0 ; Job already running?
JMP MOVE_ROBOT ; Yes - go continue.
; Else load parameters.
LD X,#move_process_buff ;
LD A,job_in_q_btm_ptrs + 0.B ; Pointer to count byte in q.
LD B,A ;
LD A,job_zero_in_q[B].B ; Get the count byte.
LD move_msg_loop_cnt,A ;
DECSZ move_msg_loop_cnt ;
NOP ;
X A,[X+].B ;
MOVE_NXT_BYTE:
INC B ; To get command byte.
AND B,#JOB_IN_Q_ROLLOVER ;
LD A,job_zero_in_q[B].B ; Get the next byte.
X A,[X+].B ; & store it.
DECSZ move_msg_loop_cnt ;
JMP MOVE_NXT_BYTE ;
LD A,job_zero_in_q[B].B ;
LD move_cancel_byte,A ;
AND A,#07 ; Do speed so we don't have to do again.
LD speed,A ;
INC B ;
AND B,#JOB_IN_Q_ROLLOVER ;
LD job_in_q_btm_ptrs + 0.B,B ; Save for next.
LD pause_job + 0.B,#0 ;
IFGT move_process_buff + 1,#03 ;
JMP SND_NOT_IMPLEMENTED_ERROR ;
IFBIT CANCEL_BIT,move_cancel_byte ;
DECSZ cancel_in_q + 0.B ;
NOP ;
IFBIT INTERNAL_REQUEST_BIT,move_cancel_byte ; Going to adjust count if
DECSZ move_process_buff + 0 ; internal msg for length checks.
; Will never be zero, so next will always execute.
IFGT on_remote_control.B,#0 ;
JSR CHK_FOR_REMOTE_INTERNAL ; Won't come back if not internal.
IFEQ move_process_buff + 1,#0 ; Rotate & move command?
JMP SET_ROTATE_AND_MOVE_PARAMS ;
IFEQ move_process_buff + 1,#1 ; Distance arc command?
JMP SET_DISTANCE_ARC_PARAMS ;
IFEQ move_process_buff + 1,#3 ; Wall follow?
JMP MOVE_ROBOT ;
; Fall thru means must be degree arc command.
LD degrees_lo,move_process_buff + 2 ;
LD degrees_hi,move_process_buff + 3 ;
LD radius,move_process_buff + 4 ;
JMP MOVE_ROBOT ;
SET_DISTANCE_ARC_PARAMS:
LD ft_lo,move_process_buff + 2 ;
LD ft_hi,move_process_buff + 3 ;
LD radius,move_process_buff + 4 ;
JMP MOVE_ROBOT ;
SET_ROTATE_AND_MOVE_PARAMS:
LD degrees_lo,move_process_buff + 2 ;
LD degrees_hi,move_process_buff + 3 ;
LD ft_lo,move_process_buff + 4 ;
LD ft_hi,move_process_buff + 5 ;
MOVE_ROBOT:
IFEQ move_process_buff + 1,#0 ; Rotate & move?
JMP ROTATE_AND_MOVE ;
IFEQ move_process_buff + 1,#3 ; Wall follow?
JMP TST_INT_TIME ; FOLLOW_WALL ;
; Else must be an arc move.
LD A,job_state + 0.B ; Where are we?
JID ; Jump according to state.
.PT DO_SETUP_ARC,DO_ARC_CONTINUE,DO_ARC_PAUSED
.PT DO_WAIT_FOR_ARC_PAUSE_STOP,DO_WAIT_FOR_ARC_COLLISION_STOP
DO_SETUP_ARC:
IFEQ move_process_buff + 0,#6 ;
JMP SETUP_ARC ;
JMP MOVE_MSG_LENGTH_ERROR ;
DO_ARC_CONTINUE:
JMP ARC_CONTINUE ;
DO_ARC_PAUSED:
JMP ARC_PAUSED ;
DO_WAIT_FOR_ARC_PAUSE_STOP:
JMP WAIT_FOR_ARC_PAUSE_STOP ;
DO_WAIT_FOR_ARC_COLLISION_STOP:
JMP WAIT_FOR_ARC_COLLISION_STOP ;
ROTATE_AND_MOVE:
LD A,job_state + 0.B ; Where are we?
JID ; Jump according to state.
.PT DO_SETUP_N_START_ROTATE,DO_CONTINUE_ROTATE,DO_START_SL_MOVE
.PT DO_CONTINUE_SL_MOVE,DO_R_N_M_PAUSED,DO_WAIT_FOR_PAUSE_STOP
.PT DO_WAIT_FOR_ROT_COLLISION_STOP,DO_WAIT_FOR_SL_COLLISION_STOP
DO_SETUP_N_START_ROTATE:
IFEQ move_process_buff + 0,#7 ;
JMP SETUP_N_START_ROTATE ;
JMP MOVE_MSG_LENGTH_ERROR ;
DO_CONTINUE_ROTATE:
JMP CONTINUE_ROTATE ;
DO_START_SL_MOVE:
JMP START_SL_MOVE ;
DO_CONTINUE_SL_MOVE:
JMP CONTINUE_SL_MOVE ;
DO_R_N_M_PAUSED:
JMP R_N_M_PAUSED ;
DO_WAIT_FOR_PAUSE_STOP:
JMP WAIT_FOR_PAUSE_STOP ;
DO_WAIT_FOR_ROT_COLLISION_STOP:
JMP WAIT_FOR_ROT_COLLISION_STOP ;
DO_WAIT_FOR_SL_COLLISION_STOP:
JMP WAIT_FOR_SL_COLLISION_STOP ;
;-------------- START OF CHECK FOR REMOTE INTERNAL ----------------------
CHK_FOR_REMOTE_INTERNAL:
IFBIT INTERNAL_REQUEST_BIT,move_cancel_byte ;
RET ;
POP X ; Get rid of return address.
JMP REJECTED_WHILE_REMOTE ;
;------------- END OF CHECK FOR REMOTE INTERNAL --------------------------
;---------- START OF MOVE 0 SETUP & STARTING ROTATION --------------------
.public SETUP_N_START_ROTATE
SETUP_N_START_ROTATE:
LD stay_on,#0 ;
LD degrees_moved_lo,#0 ; These will already be set if we
LD degrees_moved_hi,#0 ; have to abort with no movement.
LD distance_moved_lo,#0 ;
LD distance_moved_hi,#0 ;
LD rotate_flag,#0 ;
LD stop_flag,#0 ;
LD left_on_sl_control,#0 ;
LD right_on_sl_control,#0FFH ;
IFEQ speed,#0 ; Speed zero?
JMP R_N_M_ZERO_MOVE ; Yes - go abort zero move.
IFEQ tilt_enable.B,#0 ;
JMP CHK_MOVE_ZERO_STARTUP_SONAR ;
IFBIT TILT_BIT,DIGITAL_STATUS_PORT.B ; Tilted?
JMP CHK_MOVE_ZERO_STARTUP_SONAR ;
JMP MOVE_ZERO_STARTUP_TILT_ABORT ;
CHK_MOVE_ZERO_STARTUP_SONAR:
IFEQ sonar_on_off.B,#0FFH ; Sonar turned on?
JMP PAST_STRT_ROT_SONAR_CHK ;
IFEQ sonar_disable.B,#0FFH ; Sonar enabled?
JMP PAST_STRT_ROT_SONAR_CHK ;
JSR CHK_SONAR ;
IFBIT 7,A_REG_LO.B ; Drop off detected?
; IFGT A,#0 ; Collision?
JMP MOVE_ZERO_STARTUP_COLLISION_ABORT ;
PAST_STRT_ROT_SONAR_CHK:
LD right_motor_stall_limit,#R_N_M_STALL_LIMIT ;
LD left_motor_stall_limit,#R_N_M_STALL_LIMIT ;
IFGT degrees_lo,#0 ; Chk for no rotate.
JMP WILL_ROTATE ;
IFGT degrees_hi,#0 ;
JMP WILL_ROTATE ;
JMP SETUP_SL_MOVE ;
WILL_ROTATE:
LD rotate_flag,#0FFH ;
LD A_REG_LO.B,degrees_lo ; Get degrees to turn.
LD A_REG_HI.B,degrees_hi ;
JSR CALC_PIVOT_COUNT ; Go setup move.
LD A,speed ;
DEC A ;
PUSH A ;
MULT A,#CONTROL_SPEED_STEP ;
LD B,#MIN_CONTROL_SPEED_TIME ;
SC ;
SUBC B,A ;
LD BP_right_ramped_set_pt.W,B ;
LD BP_left_ramped_set_pt.W,B ;
POP A ;
MULT A,#ROT_SLOW_DOWN_MULTIPLIER ;
COMP A ;
LD B,A ;
LD A,BP_right_motor_cnts_to_move.W ;
SHR A ;
SBIT 7,A_REG_HI.B ;
IFGT A,B ;
LD B,A ;
LD BP_right_motor_slow_down.W,B ;
LD BP_left_motor_slow_down.W,B ;
LD BP_left_set_pt.W,#MIN_CONTROL_SPEED_TIME ;
LD BP_right_set_pt.W,#MIN_CONTROL_SPEED_TIME ;
LD A,#RAMP_STEP_DELTA ;
LD right_ramp_delta_lo,A_REG_LO.B ;
LD right_ramp_delta_hi,A_REG_HI.B ;
LD left_ramp_delta_lo,A_REG_LO.B ;
LD left_ramp_delta_hi,A_REG_HI.B ;
LD left_control_state,#RAMP_STATE ;
LD right_control_state,#RAMP_STATE ;
JSR START_RIGHT_MOTOR ; Crank up the move.
JSR START_LEFT_MOTOR ;
LD both_motors_on,#0FFH ;
DONE_WITH_SETUP_N_START_ROTATE:
LD job_state + 0.B,#1 ; Will go to continue rotate next time.
RET ; Setup & running.
;----------- END OF MOVE 0 SETUP & STARTING ROTATION --------------------
;--------- START OF CONTINUE ROTATE ON MOVE ZERO COMMAND ----------------
.public CONTINUE_ROTATE
CONTINUE_ROTATE:
; Chk for control & errors.
IFEQ right_motor_on,#0FFH ; Still running?
JMP CHK_ROTATE_PAUSE ;
IFEQ left_motor_on,#0FFH ; Chk both.
JMP CHK_ROTATE_PAUSE ;
; Fall thru means done with rotate.
LD rotate_flag,#0 ;
LD job_state + 0.B,#2 ;
RET ; Done with rotate.
CHK_ROTATE_PAUSE:
IFGT pause_job + 0.B,#0 ; Pause commanded?
JMP PAUSE_R_N_M_MOVE ;
IFEQ tilt_enable.B,#0 ;
JMP CHK_ROTATE_SONAR ;
IFGT left_tilt_cnt,#7 ;
JMP MOVE_ZERO_ROTATE_TILT_ABORT ;
IFGT right_tilt_cnt,#7 ;
JMP MOVE_ZERO_ROTATE_TILT_ABORT ;
; IFBIT TILT_BIT,DIGITAL_STATUS_PORT.B ; Tilted?
; JMP CHK_ROTATE_SONAR ;
; JMP MOVE_ZERO_ROTATE_TILT_ABORT ;
CHK_ROTATE_SONAR:
IFEQ sonar_on_off.B,#0FFH ; Sonar turned on?
JMP PAST_ROT_SONAR_CHK ;
IFEQ sonar_disable.B,#0FFH ; Sonar enabled?
JMP PAST_ROT_SONAR_CHK ;
JSR CHK_SONAR ;
IFBIT 7,A_REG_LO.B ; Drop off detected?
; IFGT A,#0 ; Collision?
JMP MOVE_ZERO_ROTATE_COLLISION_ABORT ;
PAST_ROT_SONAR_CHK:
IFGT cancel_in_q + 0.B,#0 ; Cancel received?
JMP CANCEL_MOVE_ROBOT ;
IFGT cancel_all + 0.B,#0 ; Cancel everything?
JMP CANCEL_ALL_MOVE_ROBOT ;
IFEQ right_motor_on,#0 ;
JMP CHK_ROT_LEFT_STALL ;
IFGT right_motor_stall_cnt.B,right_motor_stall_limit ; Stalled?
JMP ROT_STALL_ABORT ;
CHK_ROT_LEFT_STALL:
IFEQ left_motor_on,#0 ;
RET ;
IFGT left_motor_stall_cnt.B,left_motor_stall_limit ;
JMP ROT_STALL_ABORT ;
RET ;
;---------- END OF CONTINUE ROTATE ON MOVE ZERO COMMAND ----------------
;------------ START OF START STRAIGHT LINE MOVE -----------------------
.public START_SL_MOVE
START_SL_MOVE:
LD A,degrees_hi ; Successful, so we moved all the way, right?
LD degrees_moved_hi,A ;
LD A,degrees_lo ;
LD degrees_moved_lo,A ;
; Set up S/L move.
SETUP_SL_MOVE:
LD A_REG_LO.B,ft_lo ;
LD A_REG_HI.B,ft_hi ;
IFGT A,#0 ; All done?
JMP WILL_DO_SL_MOVE ;
LD job_state + 0.B,#0 ;
JMP DONE_WITH_R_N_M ;
WILL_DO_SL_MOVE:
; JSR CLR_COMPARE ;
IFBIT 7,A_REG_HI.B ; Reverse direction?
JMP SL_REVERSE ;
LD right_motor_direction,#FWD ; Fall thru is forward.
LD left_motor_direction,#FWD ; Both motors.
JMP CALC_SL_DISTANCE ;
SL_REVERSE:
LD right_motor_direction,#REV ; Set reverse.
LD left_motor_direction,#REV ; Both motors.
CALC_SL_DISTANCE:
JSR CALC_DISTANCE_COUNTS ;
LD BP_right_motor_cnts_to_move.W,A ;
LD BP_left_motor_cnts_to_move.W,A ;
LD BP_right_motor_cnts_moved.W,#0 ;
LD BP_left_motor_cnts_moved.W,#0 ;
LD A,speed ;
DEC A ;
PUSH A ;
MULT A,#CONTROL_SPEED_STEP ;
LD B,#MIN_CONTROL_SPEED_TIME ;
SC ;
SUBC B,A ;
LD BP_right_ramped_set_pt.W,B ;
LD BP_left_ramped_set_pt.W,B ;
POP A ;
MULT A,#SL_SLOW_DOWN_MULTIPLIER ;
COMP A ;
LD B,A ;
LD A,BP_right_motor_cnts_to_move.W ;
SHR A ;
SBIT 7,A_REG_HI.B ;
IFGT A,B ;
LD B,A ;
LD BP_right_motor_slow_down.W,B ;
LD BP_left_motor_slow_down.W,B ;
LD A,#RAMP_STEP_DELTA ;
LD right_ramp_delta_lo,A_REG_LO.B ;
LD right_ramp_delta_hi,A_REG_HI.B ;
LD left_ramp_delta_lo,A_REG_LO.B ;
LD left_ramp_delta_hi,A_REG_HI.B ;
IFGT continue_in_q + 0.B,#0 ;
JMP NO_RAMP_SL_SETUP ;
LD BP_left_set_pt.W,#MIN_CONTROL_SPEED_TIME ;
LD BP_right_set_pt.W,#MIN_CONTROL_SPEED_TIME ;
LD left_control_state,#RAMP_STATE ;
LD right_control_state,#RAMP_STATE ;
JMP START_SL_MOTORS ;
NO_RAMP_SL_SETUP:
LD BP_left_set_pt.W,BP_left_ramped_set_pt.W ;
LD BP_right_set_pt.W,BP_right_ramped_set_pt.W ;
IFGT speed,#BANG_PWM_TRANSITION_SPEED ;
LD left_control_state,#RUNNING_STATE ;
LD right_control_state,#RUNNING_STATE ;
START_SL_MOTORS:
JSR START_RIGHT_MOTOR ;
JSR START_LEFT_MOTOR ;
LD both_motors_on,#0FFH ;
DONE_WITH_START_SL_MOVE:
LD job_state + 0.B,#3 ;
RET ;
;------------- START OF CONTINUE STRAIGHT LINE MOVE ---------------------
.public CONTINUE_SL_MOVE
CONTINUE_SL_MOVE:
; Chk for control & errors.
IFEQ right_motor_on,#0FFH ; Still running?
JMP CHK_SL_PAUSE ;
IFEQ left_motor_on,#0FFH ; Chk both.
JMP CHK_SL_PAUSE ;
; Fall thru means done with move.
LD job_state + 0.B,#0 ;
JMP DONE_WITH_R_N_M ; All done successfully.
CHK_SL_PAUSE:
IFGT pause_job + 0.B,#0 ; Pause commanded?
JMP PAUSE_R_N_M_MOVE ;
IFEQ tilt_enable.B,#0 ;
JMP CHK_SL_SONAR ;
IFGT left_tilt_cnt,#7 ;
JMP MOVE_ZERO_SL_TILT_ABORT ;
IFGT right_tilt_cnt,#7 ;
JMP MOVE_ZERO_SL_TILT_ABORT ;
; IFBIT TILT_BIT,DIGITAL_STATUS_PORT.B ; Tilted?
; JMP CHK_SL_SONAR ;
; JMP MOVE_ZERO_SL_TILT_ABORT ;
CHK_SL_SONAR:
IFEQ sonar_on_off.B,#0FFH ; Sonar turned on?
JMP PAST_SL_SONAR_CHK ;
IFEQ sonar_disable.B,#0FFH ; Sonar enabled?
JMP PAST_SL_SONAR_CHK ;
JSR CHK_SONAR ;
IFGT A,#0 ; Collision?
JMP MOVE_ZERO_SL_COLLISION_ABORT ;
PAST_SL_SONAR_CHK:
IFGT cancel_in_q + 0.B,#0 ; Cancel received?
JMP CANCEL_MOVE_ROBOT ;
IFGT cancel_all + 0.B,#0 ; Cancel everything?
JMP CANCEL_ALL_MOVE_ROBOT ;
IFEQ right_motor_on,#0 ;
JMP CHK_SL_LEFT_STALL ;
IFGT right_motor_stall_cnt.B,right_motor_stall_limit ; Stalled?
JMP SL_STALL_ABORT ;
CHK_SL_LEFT_STALL:
IFEQ left_motor_on,#0 ;
RET ;
IFGT left_motor_stall_cnt.B,left_motor_stall_limit ;
JMP SL_STALL_ABORT ;
RET ;
;-------------- END OF CONTINUE STRAIGHT LINE MOVE ---------------------
;-------------- START OF ROTATE & MOVE PAUSE ROUTINES -------------------
PAUSE_R_N_M_MOVE:
LD stop_flag,#0FFH ;
LD paused_stay_on,stay_on ;
LD move_paused_state,job_state + 0.B ;
LD job_state +0.B,#5 ;
RET ;
WAIT_FOR_PAUSE_STOP:
IFGT right_t0_tic.B,#10 ;
JSR STOP_RIGHT_MOTOR ;
IFGT left_t0_tic.B,#10 ;
JSR STOP_LEFT_MOTOR ;
IFGT right_motor_on,#0 ;
RET ;
IFGT left_motor_on,#0 ;
RET ;
IFEQ move_paused_state,#1 ;
JSR CALC_DEGREES_ROTATED ;
IFEQ move_paused_state,#3 ;
JMP SET_PAUSE_MSG_DISTANCE ;
JMP CONTINUE_PAUSING_R_N_M ;
SET_PAUSE_MSG_DISTANCE:
LD B.W,BP_right_motor_cnts_moved.W ;
LD A,#0 ;
JSR CALC_DISTANCE_MOVED ;
CONTINUE_PAUSING_R_N_M:
LD job_state + 0.B,#4 ;
LD job_paused + 0.B,#0FFH ;
LD move_reply_process_buff + 2.B,#JOB_PAUSED ;
IFGT on_remote_control.B,#0 ;
RET ;
JMP SND_R_N_M_ABORT_MSG_W_O_STATE_RESET ;
R_N_M_PAUSED:
IFGT cancel_in_q + 0.B,#0 ; Cancel received?
JMP CANCEL_MOVE_ROBOT ;
IFGT cancel_all + 0.B,#0 ; Cancel everything?
JMP CANCEL_ALL_MOVE_ROBOT ;
IFGT pause_job + 0.B,#0 ; Still paused?
RET ; Yes - go back.
; Else start back up.
LD stay_on,paused_stay_on ;
LD stop_flag,#0 ;
LD job_paused + 0.B,#0 ;
LD BP_left_set_pt.W,#MIN_CONTROL_SPEED_TIME ;
LD BP_right_set_pt.W,#MIN_CONTROL_SPEED_TIME ;
LD A,#RAMP_STEP_DELTA ;
LD right_ramp_delta_lo,A_REG_LO.B ;
LD right_ramp_delta_hi,A_REG_HI.B ;
LD left_ramp_delta_lo,A_REG_LO.B ;
LD left_ramp_delta_hi,A_REG_HI.B ;
LD left_control_state,#RAMP_STATE ;
LD right_control_state,#RAMP_STATE ;
; SBIT LEFT_MOTOR_TOGGLE_BIT,PORTP_LO.B ;
; SBIT RIGHT_MOTOR_TOGGLE_BIT,PORTP_LO.B ;
IFEQ right_move_complete,#0 ;
JSR START_RIGHT_MOTOR ;
IFEQ left_move_complete,#0 ;
JSR START_LEFT_MOTOR ;
LD both_motors_on,#0FFH ;
LD job_state + 0.B,move_paused_state ;
LD move_reply_process_buff + 2.B,#JOB_RESUMED ;
IFGT on_remote_control.B,#0 ;
RET ;
JMP SND_R_N_M_ABORT_MSG_W_O_STATE_RESET ; Info should still
; be all right.
;--------------- END OF ROTATE & MOVE PAUSE ROUTINES -------------------
.incld ARCMOVES
.incld MOVUTILS
.extrn head_stall_tic
.extrn DO_CONVERSION ;
TST_INT_TIME:
SBIT 2,T0CON.B ;
LD stay_on,#0FFH ;
LD left_control_state,#1 ;
LD right_control_state,#1 ;
JSR START_RIGHT_MOTOR ;
JSR START_LEFT_MOTOR ;
LD X_REG.W,#1000 ;
LD BP_temp_reg.W,I4CR.W ;
LD head_stall_tic.B,#0 ;
TIME_LOOP:
JSR RIGHT_MOTOR_OPTO_INT_HANDLER ;
JSR RIGHT_MOTOR_OPTO_INT_HANDLER ;
; JSR DO_CONVERSION ;
DECSZ X_REG.W ;
JMP TIME_LOOP ;
LD A,I4CR.W ;
LD B,head_stall_tic.B ;
RBIT 2,T0CON.B ;
SC ;
SUBC A,BP_temp_reg.W ;
SUBC B,#0 ;
PUSH A ;
PUSH B ;
JSR STOP_RIGHT_MOTOR ;
JSR STOP_LEFT_MOTOR ;
JSR ONE_LESS_MOVE_JOB ;
POP B ;
POP A ;
LD move_reply_process_buff + 0.B,#5 ;
LD move_reply_process_buff + 1.B,move_process_buff + 1 ;
LD move_reply_process_buff + 2.B,B_REG_LO.B ;
LD move_reply_process_buff + 3.B,A_REG_HI.B ;
LD move_reply_process_buff + 4.B,A_REG_LO.B ;
JMP SND_MOVE_REPLY_MSG ;
.endsect
.end