Tuco - University of Florida · PDF fileTuco is an autonomous slave RC that has the ability to...

TucoAutonomous/Remote

Controlled Robot Project Report

Carlos Jaime CollazoEEL5666Dr. Antonio ArroyoIntelligent Machines Design LaboratoryUniversity of FloridaAugust 5, 1998

Table of Contents

Abstract ........................................................................................................................3

Executive Summary ..................................................................................................... 4

Introduction .................................................................................................................. 4

Integrated System ......................................................................................................... 4

Mobile Platform ........................................................................................................... 4

Actuation ...................................................................................................................... 5

Sensors .........................................................................................................................5

Behaviors...................................................................................................................... 6

Conclusion.................................................................................................................... 6

Appendix A: Behavior Code and Figures...................................................................... 7

Abstract

Following is a discussion on developing an obstacle avoidance system, sonar guided system, andremote control and autonomous control mode switching capabilities for a high speed hobby racecar (RC) using infrared emitter and sensors, sonar transducers and a 75MHz AMtransmitter/receiver pair. The RC called Tuco uses a commercial speed control servo to controlthe high current motor. RF noise produced by the motors and the micro-controller complicated theautonomous mode of operation.

Executive Summary

Tuco is an autonomous slave RC that has the ability to travel at high speeds. A commercial RCplatform was used to mount a Motorola 68HC11 microprocessor (MP), the motor, speed controldevice (SC), steering servo (SS) and differential gear. Tuco has actuators such as RC size shockabsorbers to cushion the frame when in motion. Although Tuco is capable of running at about50mph, the speed controller was used in conjunction with the MP board to maintain low speedsallowing Tuco perform obstacle avoidance operations with IR emitters and receivers. The speedcontroller allows for forward and reverse motor direction control modes. Two IR receivers areplaced at the front for forward motion and two in the back for obstacle avoidance when reversing.

At lower speeds, Tuco behaves more efficiently when utilizing the sonar system capabilities.Three sonar receivers are installed in the platform for Tuco to follow a moving beacon: a hand held40kHz sonar transmitter. The receivers are placed at the front of the RC in a triangular form. Therobot translates the loudness (signal amplitude) of the three received signals and follows thedirection of the loudest of the three. The idea behind the inclusion of a sonar tracking device is tohave Tuco follow the transmission in the way a canine would follow his/her master when called.

The MP utilizes its input capture features to process the two control pulses received by the 75MHzAM receiver installed in the robot. One of the signals controls the steering servo and the othercontrols the speed and motion direction. Upon reception of the pulse width modulated controlsignals, the MP decides whether or not the remote control signal has changed in pulse widthbefore changing the mode of operation (remote control and autonomous).

Introduction

The primary concern with Tuco is to have it find a sonar transmission beacon like a canine wouldfollow the master during autonomous mode. In order to accomplish such a task, three sonartransducers connected to a 40kHz band pass filter (BPF) are used. Another concern is to haveTuco respond to the RF transmission as it would normally do without any micro-controllers in thesystem.

Integrated System

Tuco is a four wheel RC utilizing the MP 32Kbyte memory expansion and motor port boarddesigned by Scott Jantz. To avoid the noise coming from the motors and the MP a FaradayShield composed of a copper sheet connected to the common ground of the system was used. Asthe SC and the SS are connected to the Output Compare, and the SC and SS signals coming fromthe RF receiver are connected directly into the Input Capture ports of the MP, if a change in thesignal pulse widths are detected beyond (10 clock cycle counts, the MP overrides the autonomousoperation and gives to remote control mode.

The four IR receiver and the three sonar transducer processed analog signals are converted todigital by seven of the Analog to Digital (A/D) ports of the MP. When the digital signal translatedas a numerical value is greater than a threshold, the MP is programmed to respond and behave incertain ways. For instance, when the two from IR receiver signals pass the threshold, the MPstops, steers against the obstacle and reverses for about 2 seconds. After that, Tuco searches fora change in threshold in the two rear IR receivers for obstacle avoidance. Once this happens,

Tuco resumes its normal mode of autonomous sonar guided operation. When the sonar signalssurpass the threshold, the MP determines the maximum signal out of the three transducers andfollows the direction of the source. The transducers are placed in the following order: left, centerand right.

Mobile Platform

Tuco is a four wheel RC and has the layout shown in Figure 1.

Figure 1

Actuation

Tuco uses two commercial actuators: Futuba steering servo and speed control. Both response topulse modulated signals driven by TTL level (+5V) amplitudes. The period for each is around20ms and the pulse widths range from 0.7ms for left or reverse and 2ms for right or forward. TheMP controls these devices directly from Output Compare ports two and three.

Sensors

The IR receiver analog outputs are connected directly into the analog ports of the MP asmentioned above. The receivers are the hacked version of the Sharp GP1U5 IR receivers.Figure 2 shows the design and hacking method of the Sharp IR receivers.

Figure 2

MPSonar SystemSpeed ControlIR ReceiverIR Emitter

Basically, trace 3 above was cut and a wire was soldered to the top of box 1 which is a 0.1uFcapacitor to have access to analog readings from the IR receivers. Box 2 is the integrated section.

The sonar system design is shown in the Appendix.

Behaviors

As mentioned above, in autonomous mode Tuco finds the maximum value out of the three signalscoming from the sonar transducers and follows the direction of the transducer in the correspondingposition. Simultaneously, Tuco avoids obstacles when following the beacon. Observe thesubroutines AVOID and SONAR in theAppendix.

Conclusion

Tuco follows the moving beacon, avoids obstacles and switches between operation modes asexpected. All systems performed properly.

2

1

DIGITALANALOG Vcc GROUND

3

Appendix

Sonar Transmitter Circuitry

Sonar Receiver Circuitry

Behavior Code***********************************************************

* EEL5666* Tuco's Brain* Carlos Jaime Collazo** OC2=Steering Servo=PIN28* OC3=Speed Control=PIN29* A1=Used by Buffalo!!* A2=GREEN=RIGHT_IR=PIN45=PE1* A3=BLUE=LEFT_IR=PIN47=PE2* A4=Sonar Output2=Center=PE3=PIN49* A5=Sonar Output3=Left=PE4=PIN44* A6=Right Rear IR=PE5=PIN46* A7=Left Rear IR=PE6=PIN48* A8=Sonar Output1=Right=PE7=PIN50* IR => Vref High = 5V, Vref Low = 0V* VRHIGH=PIN52;VRLOW=PIN51* IC1=PIN32=CHANNEL1=STEERING, IC2=PIN33=CHANNEL2=SPEED C.** SAMPLE IC OUTPUTS** IC1=CHANNEL1* PW1 : L1: P1:CTR1:MIN1:MAX1* 0BA7:8A5D:9606:0BAB:087C:0EE2* IC2=CHANNEL2* PW2 : L2: P2:CTR2:MIN2:MAX2* 0BEC:8A17:9605:0BF1:0852:0F69* CENTER VALUES BEFORE ANY STEERING OR SPEED CHANGE:* 0C14:8A03:9615:0C0A:0C08:0C20* 0C0C:8A13:9618:0C21:0C00:0C13* range: ee7-c12=2d5=725base10***********************************************************

*----------------------------------------* EQUATES/REGISTERS*----------------------------------------

*_____MEMORY_____________________________TSTACK EQU $01FF ;Top of StackSTACK EQU $0041 ;STACKBASE EQU $1000 ;BEGINNING OF REGISTERSRAM EQU $0100 ;RAM addressSRAM EQU $8000 ;Low Addr. Static RAMEEPROM EQU $B600 ;EEPROMSTART EQU $F800 ;PCBUG11 START*_____TIMER______________________________TMSK1 EQU $22 ; TIMER MASK1 REGISTERTFLG1 EQU $23 ; TIMER FLAG1 REGISTERTMSK2 EQU $24 ; TIMER MASK2 REGISTERTFLG2 EQU $25 ; TIMER FLAG2 REGISTERTCTL1 EQU $20 ; TIMER CONTROL 1TCTL2 EQU $21 ; TIMER CONTROL 2TCNT EQU $0E ; TCNT High byte*_____I/O PORTS__________________________PORTA EQU $00PORTB EQU $04PORTC EQU $03PORTCL EQU $05PORTD EQU $08PORTE EQU $0ADDRC EQU $07DDRD EQU $09ME11IO EQU $7000*_____INPUT CAPTURE______________________TIC1 EQU $10 ; Timer Input Capture 1TIC2 EQU $12 ; Timer Input Capture 2TIC3 EQU $14 ; Timer Input Capture 3*_____OUTPUT CAPTURE______________________TOC1 EQU $16 ;OUTPUT COMPARE 1 REGISTERTOC2 EQU $18 ;OUTPUT COMPARE 2 REGISTER

TOC3 EQU $1A ;OUTPUT COMPARE 3 REGISTERTOC4 EQU $1C ;OUTPUT COMPARE 4 REGISTER*_____A/D SYSTEM__________________*VRHIGH=PIN52;VRLOW=PIN51OPTION EQU $39 ;SYSTEM CONFIG. OPTIONSADCTL EQU $30 ;A/D CONTROL/STATUSADR1 EQU $31 ;ANALOG PORT1:ADR2 EQU $32 ;ANALOG PORT2:ADR3 EQU $33 ;ANALOG PORT3:ADR4 EQU $34 ;ANALOG PORT4:ADR5 EQU $31 ;ANALOG PORT5:ADR6 EQU $32 ;ANALOG PORT6:ADR7 EQU $33 ;ANALOG PORT7:ADR8 EQU $34 ;ANALOG PORT8:* AN0=PE0=PIN43; AN1=PE1=PIN45* AN2=PE2=PIN47; AN3=PE3=PIN49* AN4=PE4=PIN44; AN5=PE5=PIN46* AN6=PE6=PIN48; AN7=PE7=PIN50*_____PULSE ACCUMULATOR__________________PACTL EQU $26 ; PA ControlPACNT EQU $27 ; PA Counter*_____SCI________________________________BAUD EQU $102B ; BAUD rate control register to set the BAUD rateSCCR1 EQU $102C ; Serial Communication Control Register-1SCCR2 EQU $102D ; Serial Communication Control Register-2SCSR EQU $102E ; Serial Communication Status RegisterSCDR EQU $102F ; Serial Communication Data Register*_____Interrupt Vectors__________________BIC1I EQU $E8 ;BUFFALO INT. VECTOR IC1BIC2I EQU $E5 ;BUFFALO INT. VECTOR IC2BOC2I EQU $DC ;BUFFALO INT. VECTOR OC2BOC3I EQU $D9 ;BUFFALO INT. VECTOR OC3BATODI EQU $EB ;BUFFALO INT. VECTOR RTI FOR A/DOC2I EQU $FFE6 ;INT. VECTOR OC2OC3I EQU $FFE4 ;INT. VECTOR OC3ATODI EQU $FFF0 ;RTI FOR A/DRESET EQU $FFFE ;Reset interrupt vector

*----------------------------------------* CHARACTERS/CONSTANTS*----------------------------------------

EOS EQU $04 ; User-defined End Of* ; String (EOS) characterCR EQU $0D ; Carriage Return CharacterLF EQU $0A ; Line Feed CharacterESC EQU $1B ; Escape CharracterQTE EQU $27 ; QUOTE 'SCLN EQU $3B ; SEMICOLON ;ZERO FCB '0'

FCB '1'FCB '2'FCB '3'FCB '4'FCB '5'FCB '6'FCB '7'FCB '8'FCB '9'FCB 'A'FCB 'B'FCB 'C'FCB 'D'FCB 'E'FCB 'F'

ANA0 FCC @Left:@FCB EOS

ANA1 FCC @Right@FCB EOS

COL FCC @:@

FCB EOSCRLF FCB CR, LF, EOSCLR FCB ESC,$5B,$32,$4A ; ANSI sequence to clear screen

FCB EOS ; EOS characterPOS FCB ESC,$5B,$3B,$48 ; and move cursor to home

FCB EOS ; EOS characterPW1PRNT FCC @PW1=@

FCB EOS ; EOS characterL1PRNT FCC @L1=@

FCB EOS ; EOS characterP1PRNT FCC @P1=@

FCB EOS ; EOS characterPW2PRNT FCC @PW2=@

FCB EOS ; EOS characterL2PRNT FCC @L2=@

FCB EOS ; EOS characterP2PRNT FCC @P2=@

FCB EOS ; EOS character

*----------------------------------------* BIT MASKS*----------------------------------------

BIT0 EQU %00000001BIT1 EQU %00000010BIT2 EQU %00000100BIT3 EQU %00001000BIT4 EQU %00010000BIT5 EQU %00100000 ;OC3BIT6 EQU %01000000BIT7 EQU %10000000 ;OC2INV0 EQU %11111110INV1 EQU %11111101INV2 EQU %11111011INV3 EQU %11110111INV4 EQU %11101111INV5 EQU %11011111 ;OC3INV6 EQU %10111111INV7 EQU %01111111 ;OC2BIT74 EQU %10010000BIT65 EQU %01100000BIT54 EQU %00110000

*----------------------------------------* Data*----------------------------------------

MID EQU $71 ;Middle: ~(2^8)/2ADF EQU 0IOPAT EQU $FF ;All PORTD outputsDUMMYP EQU $0C20 ;DUMMY CENTERDUMMYL EQU $9606-$0C20 ;DUMMY CENTER DIFFDUMMYMIN EQU $900 ;DUMMY MINIMUMDUMMYMAX EQU $F90 ;DUMMY MAXIMUM*TOLLERANCE EQU $A ;TOLLERANCE VALUETOLLERANCE EQU $14 ;TOLLERANCE VALUETHRESHOLD EQU $18 ;SERVO CONTROL THRESHOLD*OFFSET EQU $38OFFSET EQU $D

SLOWF EQU $DF+OFFSET ;ADD OR SUB FOR SLOW MOTION (FWD)*decrease pulsewidth to go fwd, incrase to go backwards.SLOWR EQU $57+OFFSET ;ADD OR SUB FOR SLOW MOTION (REV)LEVEL EQU $A8 ;STEER LEVEL UNIT (5 LEVELS FROM CENTER* TO AN EXTREME).SONART EQU $F

*----------------------------------------* INTERRUPT VECTOR ADDRESSES*----------------------------------------

ORG BOC2I* FDB OC2IS

JMP OC2IS

ORG BOC3I* FDB OC3IS

JMP OC3IS

ORG BIC1IJMP IC1ISR

ORG BIC2IJMP IC2ISR

* ORG BATODI* JMP ADSERV

* ORG RESET* FDB MAIN

*----------------------------------------* Variables*----------------------------------------

ORG RAMJMP MAIN

ORG SRAMA1 RMB 1 ; BUFFALO ANALOG PORTRFA2 RMB 1 ; RF ANALOG PORTLFA3 RMB 1 ; LF ANALOG PORTSI2A4 RMB 1 ; SI2 ANALOG PORTSI3A5 RMB 1 ; SI3 ANALOG PORTRRA6 RMB 1 ; RR ANALOG PORTLRA7 RMB 1 ; LR ANALOG PORTSI1A8 RMB 1 ; SI1 ANALOG PORTPAN2 RMB 1 ; PREVIOUS ANALOG PORT TMPPAN3 RMB 1 ; PREVIOUS ANALOG PORT TMPADCNT RMB 1 ; A/D System CounterPW1 RMB 2 ; Pulse Width Register 1PW2 RMB 2 ; Pulse Width Register 2L1 RMB 2 ; Low part of pulse 1L2 RMB 2 ; Low part of pulse 2P1 RMB 2 ; PERIOD 1P2 RMB 2 ; PERIOD 2PPW1 RMB 2 ; PREVIOUS PULSE WIDTH 1PPW2 RMB 2 ; PREVIOUS PULSE WIDTH 2PWA1 RMB 2 ; Pulse Width Register 1PWA2 RMB 2 ; Pulse Width Register 2LA1 RMB 2 ; Low part of pulse 1LA2 RMB 2 ; Low part of pulse 2PA1 RMB 2 ; PERIOD 1PA2 RMB 2 ; PERIOD 2T RMB 2 ; Delay maxRTMP RMB 2 ; Temporary registerLTMP RMB 2 ; Temporary registerFTMP RMB 2 ; Forward tempRVTMP RMB 2 ; Reverse tempACCELF RMB 2 ; ACCELERATION TEMPDECELF RMB 2 ; DECELERATION LOOP FACTORSTEER RMB 2 ; STEERING TEMPORARYACCEL RMB 2 ; ACCELERATION TEMPTURN RMB 2 ; TURN TIME RANGE FACTORDBUFR RMB 5 ; 5 BYTE VARIABLE FOR HTOD SRTINE.

WORD RMB 2 ; TEST WORDBYTE RMB 1 ; TEST BYTE*INPUT CAPTURE VALUESMAX1 RMB 2 ; MAX PULSE WIDTH CHANNEL1 (FULL RIGHT)MAX2 RMB 2 ; MAX PULSE WIDTH CHANNEL2 (FULL REV)MIN1 RMB 2 ; MIN PULSE WIDTH CHANNEL1 (FULL LEFT)MIN2 RMB 2 ; MIN PULSE WIDTH CHANNEL2 (FULL FWD)CENTER1 RMB 2 ; CENTER CHANNEL1CENTER2 RMB 2 ; CENTER CHANNEL2SLOWFWD RMB 2 ; SLOW FORWARD VALUESLOWREV RMB 2 ; SLOW REVERSE VALUETOHI1 RMB 2 ; IC1 LO TO HI TCNT VALUETOHI2 RMB 2 ; IC2 LO TO HI TCNT VALUETOLO1 RMB 2 ; IC1 HI TO LO TCNT VALUETOLO2 RMB 2 ; IC2 HI TO LO TCNT VALUEPTOHI1 RMB 2 ; PREVIOUS IC1 LO TO HI TCNT VALUEPTOHI2 RMB 2 ; PREVIOUS IC2 LO TO HI TCNT VALUEREMOTE RMB 1 ; REMOTE ON/OFF SWITCHREMOTE1 RMB 1 ; REMOTE ON/OFF SWITCHREMOTE2 RMB 1 ; REMOTE ON/OFF SWITCHTEMP1 RMB 2TEMP2 RMB 2OC2LTH RMB 2 ; OC2 LO TO HI TCNT VALUEOC3LTH RMB 2 ; OC3 LO TO HI TCNT VALUELTHDIFF2 RMB 2 ; OC2 LO TO HI DIFF. = PERIODLTHDIFF3 RMB 2 ; OC3 LO TO HI DIFF. = PERIODOC2HTL RMB 2 ; OC2 HI TO LO TCNT VALUEOC3HTL RMB 2 ; OC3 HI TO LO TCNT VALUEMIDL RMB 1 ; LEFT ANALOG MIDDLE VALUE (A2)MIDR RMB 1 ; RIGHT ANALOG MIDDLE VALUE (A3)AVDL RMB 1 ; AVOID LEFT SWITCHAVDR RMB 1 ; AVOID RIGHT SWITCHTOLLERANCE1 RMB 2 ; PULSE 1 TOLLERANCETOLLERANCE2 RMB 2 ; PULSE 2 TOLLERANCESI1MIN RMB 1 ; SONAR INPUT 1 MINSI2MIN RMB 1 ; SONAR INPUT 2 MINSI3MIN RMB 1 ; SONAR INPUT 3 MINS1 RMB 1 ; SONAR TEMP 1S2 RMB 1 ; SONAR TEMP 2S3 RMB 1 ; SONAR TEMP 3PS1 RMB 1 ; SONAR PREVIOUS TEMP 1PS2 RMB 1 ; SONAR PREVIOUS TEMP 2PS3 RMB 1 ; SONAR PREVIOUS TEMP 3TS1 RMB 1 ; SONAR PREVIOUS TEMP 1TS2 RMB 1 ; SONAR PREVIOUS TEMP 2TS3 RMB 1 ; SONAR PREVIOUS TEMP 3SCTRL RMB 1 ; SONAR CONTROL VARIABLESMAX RMB 1 ; SONAR MAX

************************************************************* MAIN************************************************************

MAIN LDS #TSTACK ;Init. stack at $1ff ($ff total)* ; need more than $41 for printing

LDX #BASE ;Init. register base* JSR InitSCI ;Init. SCI Port

JSR TIMER ;Init. TimerJSR IC12INIT ;Init. IC1 and IC2JSR DELAYJSR OC2INIT ;" OC2JSR OC3INIT ;" OC3JSR ADINIT ;Init. A/D systemJSR INIT_COUNT ;INITIALIZE COUNTERJSR CLRS ;Clear screenJSR DUMMYC ;INIT DUMMY CONTROLS

CLI ;Enable Interrupts

JSR LDELAY ;ALLOW DATA STORINGJSR ADNOI ;GET A/D VALUESJSR DELAY ;ALLOW DATA STORINGJSR MINMAX1 ;CALCULATE INITIAL CHNL. VALUESJSR MINMAX2JSR INIT_SERVOS ;INITIALIZE SERVOSJSR INIT_IR ;INITIALIZE IR SYSTEMJSR INIT_SONAR ;INITIALIZE SONAR REGISTERS

*----------------------------------------* BODY*----------------------------------------

CLRA REMOTE ;INITIALIZE CONTROL TO AUTONOMOUS

PARALLEL NOP ;CHECK IF BOTH ARE RECEIVING

JSR SMAXCALJSR SONARCHK

* JSR SPRINT

BRA PARALLEL

*----------------------------------------* CONTROL SUBROUTINES*----------------------------------------

*-----------------------* SONAR SYSTEM*-----------------------

*-----------------------SONARCHK NOP

LDAA SCTRLCMPA #1BEQ TRNRLDAA SCTRLCMPA #3BEQ TRNLBRA SCNTR

TRNR NOPJSR FCTOR ;STEER RIGHTJSR SFWDJSR DELAYJSR DELAYJSR AVOIDJSR DELAY

* JSR SPRINTJSR RTOCJSR DELAYJSR SREVJSR STOPREVBRA SCHKDONE

TRNL NOPJSR FCTOL ;STEER RIGHTJSR SFWDJSR DELAYJSR DELAYJSR AVOID

* JSR DELAY* JSR SPRINT

JSR LTOCJSR DELAYJSR SREV

JSR STOPREVBRA SCHKDONE

SCNTR NOPJSR SFWDJSR AVOID

* JSR DELAY* JSR SPRINT

JSR SREVJSR STOPREV

SCHKDONE RTS

*-----------------------SPRINT NOP

LDAA #'S'JSR ocharLDAA #'1'JSR ocharCLRALDAB S1JSR HXTOALDX #COLJSR PrintS

*****************************************LDAA #'S'JSR ocharLDAA #'2'JSR ocharCLRALDAB S2JSR HXTOALDX #COLJSR PrintS


*****************************************LDAA #'S'JSR ocharLDAA #'C'JSR ocharCLRALDAB SCTRLJSR HXTOA

*****************************************LDX #CRLFJSR PrintSLDX #POSJSR PrintSRTS

*-----------------------* SONAR MIN. VALUES*-----------------------

SMINS LDAA SI1A8CMPA SI1MINBHS CHKSI2STAA SI1MIN

CHKSI2 LDAA SI2A4BHS CHKSI3STAA SI2MIN

CHKSI3 LDAA SI3A5BHS SMINDONESTAA SI3MIN

SMINDONE RTS

*-----------------------* SONAR DIFFERENCES*-----------------------

SDIFF NOPLDAA SI1A8STAA TS1SUBA PS1STAA S1BLE NEXTS1BRA SDONE

NEXTS1 LDAA SI2A4STAA TS2SUBA PS2STAA S2BLE NEXTS2BRA SDONE

NEXTS2 LDAA SI3A5STAA TS3SUBA PS3STAA S3

SDONE LDAA TS1STAA PS1LDAA TS2STAA PS2LDAA TS3STAA PS3RTS

*-----------------------* SONAR MAX CALC*-----------------------

SMAXCAL LDAA #0STAA SMAXLDAA S1CMPA SMAXBLT NM2STAA SMAXLDAA #1STAA SCTRL

NM2 LDAA S2CMPA SMAXBLT NM3STAA SMAXLDAA #2STAA SCTRL

NM3 LDAA S3CMPA SMAXBLT NMDONESTAA SMAXLDAA #3STAA SCTRL

NMDONE NOPRTS

*-----------------------* OBSTACLE AVOIDANCE*-----------------------

AVOID LDAA AVDLADDA AVDRBEQ AVDDONEJSR STOPFWD ;Stop motorLDAA AVDLBEQ CHKR

JSR AVOIDLBRA AVDDONE

CHKR LDAA AVDRBEQ AVDDONEJSR AVOIDR

AVDDONE RTS

*-----------------------* OBSTACLE DETECTION* CHECK*-----------------------

CHECK LDAA LFA3 ;CHECK LEFTCMPA MIDL ;COMPARE WITH MIDDLEBLO NOLEFT

AVOID_LEFT LDAA #1STAA AVDLBRA CHK_R

NOLEFT CLRASTAA AVDL

CHK_R LDAA RFA2CMPA MIDR ;COMPARE WITH MIDDLEBLO NORIGHT

AVOID_RIGHT LDAA #1STAA AVDRBRA CHKDONE

NORIGHT CLRASTAA AVDR

CHKDONE RTS

*-----------------------* OBSTACLE AVOIDANCE* INDEPENDENT CONTROLS*-----------------------

AVOIDR NOPJSR FCTOR ;STEER RIGHTJSR SREVJSR LDELAY ;REVESE DELAYJSR STOPREVJSR RTOC

* JSR SFWDRTS

AVOIDL NOPJSR FCTOL ;STEER LEFTJSR SREVJSR LDELAYJSR STOPREVJSR LTOC

* JSR SFWDRTS

*-----------------------* MOTOR CONTROL*-----------------------

SFWD LDD SLOWFWDLDX ACCELFJSR FWDJSR DELAYJSR DELAYRTS

STOPFWD LDD CENTER2LDX DECELFJSR REV

RTS

SREV LDD SLOWREVLDX ACCELFJSR DELAYJSR DELAYJSR REVRTS

STOPREV LDD CENTER2LDX DECELFJSR FWDRTS

FWD STD FTMPFLOOP NOP

JSR XDELAYLDD PWA2SUBD ACCELSTD PWA2JSR L2CALCPD FTMPBNE FLOOPRTS

REV STD RTMPRLOOP NOP

JSR XDELAYLDD PWA2ADDD ACCELSTD PWA2JSR L2CALCPD RTMPBNE RLOOPRTS

L2CAL PSHAPSHBLDD P2SUBD PWA2STD LA2PULBPULARTS

*-----------------------* STEER CONTROL*-----------------------

CTOL ADDD CENTER1 ;CENTER TO LEFT SPECIFIED IN DJSR LEFTRTS

CTOR ADDD CENTER1 ;CENTER TO RIGHT SPECIFIED IN DJSR RIGHTRTS

RTOC LDD CENTER1 ;RIGHT TOJSR LEFT ;CENTERRTS

FCTOL LDD MIN1 ;CENTER TOJSR LEFT ;FULL LEFTRTS

LTOC LDD CENTER1 ;LEFTJSR RIGHT ;TO CENTERRTS

FCTOR LDD MAX1 ;CENTER TOJSR RIGHT ;FULL RIGHTRTS

LEFT STD LTMPLLOOP LDD PWA1

SUBD STEERSTD PWA1JSR L1CALCPD LTMPBNE LLOOPRTS

RIGHT STD RTMPRLOOP LDD PWA1

ADDD STEERSTD PWA1JSR L1CALCPD RTMPBNE RLOOPRTS

L1CAL PSHAPSHBLDD P1SUBD PWA1STD LA1PULBPULARTS

*-----------------------* SERVO MIN AND MAX* VALUES CALCULATIONS*-----------------------

MINMAX1 LDD PW1CPD MIN1BHI CMM11STD MIN1

CMM11 CPD MAX1BLO CMM12STD MAX1

CMM12 RTS

MINMAX2 LDD PW2CPD MIN2BHI CMM21STD MIN2

CMM21 CPD MAX2BLO CMM22STD MAX2

CMM22 RTS

*----------------------------------------* PRINT IC AND OC VALUES*----------------------------------------

SEGMENT1 NOPLDD PW1JSR HXTOALDX #COLJSR PrintSLDD L1JSR HXTOALDX #COLJSR PrintSLDD P1

JSR HXTOALDX #COLJSR PrintSRTS

SEGMENT2 NOPLDD CENTER1JSR HXTOALDX #COLJSR PrintSLDD MIN1JSR HXTOALDX #COLJSR PrintSLDD MAX1JSR HXTOALDX #CRLFJSR PrintSRTS

SEGMENT3 NOPLDD PW2JSR HXTOALDX #COLJSR PrintSLDD L2JSR HXTOALDX #COLJSR PrintSLDD P2JSR HXTOALDX #COLJSR PrintSRTS

SEGMENT4 NOPLDD CENTER2JSR HXTOALDX #COLJSR PrintSLDD MIN2JSR HXTOALDX #COLJSR PrintSLDD MAX2JSR HXTOALDX #CRLFJSR PrintSRTS

*----------------------------------------* Print analog port values*----------------------------------------

SEGMENT5 NOPLDX #BASE

*****************************************LDAA #'R'JSR ocharLDAA #'F'JSR ocharCLRALDAB RFA2 ;RIGHTJSR HXTOA ;INPUT IN DLDX #COLJSR PrintS

*****************************************LDAA #'L'JSR ocharLDAA #'F'

JSR ocharCLRALDAB LFA3 ;LEFTJSR HXTOA ;INPUT IN DLDX #COLJSR PrintS

*****************************************LDAA #'R'JSR ocharLDAA #'R'JSR ocharCLRALDAB RRA6JSR HXTOALDX #COLJSR PrintS

*****************************************LDAA #'L'JSR ocharLDAA #'R'JSR ocharCLRALDAB LRA7JSR HXTOALDX #CRLFJSR PrintS

*****************************************LDAA #'S'JSR ocharLDAA #'I'JSR ocharLDAA #'1'JSR ocharLDAA #'_'JSR ocharCLRALDAB SI1A8JSR HXTOALDX #COLJSR PrintS

*****************************************LDAA #'S'JSR ocharLDAA #'I'JSR ocharLDAA #'2'JSR ocharLDAA #'_'JSR ocharCLRALDAB SI2A4JSR HXTOALDX #COLJSR PrintS

*****************************************LDAA #'S'JSR ocharLDAA #'I'JSR ocharLDAA #'3'JSR ocharLDAA #'_'JSR ocharCLRALDAB SI3A5JSR HXTOALDX #CRLFJSR PrintS

*****************************************LDAA #'S'JSR ochar

LDAA #'I'JSR ocharLDAA #'1'JSR ocharLDAA #'M'JSR ocharCLRALDAB SI1MINJSR HXTOALDX #COLJSR PrintS

*****************************************LDAA #'S'JSR ocharLDAA #'I'JSR ocharLDAA #'2'JSR ocharLDAA #'M'JSR ocharCLRALDAB SI2MINJSR HXTOALDX #COLJSR PrintS

*****************************************LDAA #'S'JSR ocharLDAA #'I'JSR ocharLDAA #'3'JSR ocharLDAA #'M'JSR ocharCLRALDAB SI3MINJSR HXTOALDX #CRLFJSR PrintS



*****************************************LDAA #'S'JSR ocharLDAA #'3'JSR ocharCLRALDAB S3JSR HXTOALDX #CRLFJSR PrintS

*****************************************LDAA #'M'

JSR ocharLDAA #'L'JSR ocharCLRALDAB MIDL ;LEFT CENTERJSR HXTOA ;INPUT IN DLDX #COLJSR PrintS

*****************************************LDAA #'M'JSR ocharLDAA #'R'JSR ocharCLRALDAB MIDR ;RIGHT CENTERJSR HXTOA ;INPUT IN DLDX #CRLFJSR PrintS

*****************************************RTS

SEGMENT6 NOPLDAA #'A'JSR ocharLDAA #'L'JSR ocharCLRALDAB AVDL ;AVOID LEFT CONTROLJSR HXTOA ;INPUT IN DLDX #COLJSR PrintS

*****************************************LDAA #'A'JSR ocharLDAA #'R'JSR ocharCLRALDAB AVDR ;AVOID RIGHT CONTROLJSR HXTOA ;INPUT IN DLDX #COLJSR PrintS

*****************************************LDAA #'R'JSR ocharLDAA #'M'JSR ocharLDAA #'T'JSR ocharCLRALDAB REMOTE ;REMOTE ON/OFFJSR HXTOA ;INPUT IN D

*****************************************LDX #CRLFJSR PrintSRTS

SEGMENT7 JSR PRNTPWA2LDX #POSJSR PrintS

*****************************************RTS

*----------------------------------------* Print PWA2 Data*----------------------------------------

PRNTPWA2 LDD PWA2

JSR HXTOALDX #COLJSR PrintSLDD LA2JSR HXTOARTS

*----------------------------------------* Print COLON*----------------------------------------

PRNTCOLON LDX #COLJSR PrintSRTS

*----------------------------------------* Print CRLF*----------------------------------------PRNTCRLF LDX #CRLF

JSR PrintSRTS

*----------------------------------------* Print OC2-3 VALUES*----------------------------------------

PRNTOC23 NOPLDD OC2LTHSUBD OC2HTLJSR HXTOALDX #COLJSR PrintSLDD LTHDIFF2JSR HXTOALDX #COLJSR PrintS

LDD OC3LTHSUBD OC3HTLJSR HXTOALDX #COLJSR PrintSLDD LTHDIFF3JSR HXTOARTS

*-----------------------* DELAY FUNCTIONS*-----------------------

PRNTPOS LDX #POSJSR PrintSRTS

*-----------------------* DELAY FUNCTIONS*-----------------------

XDELAY PSHYLDY TSTX TJSR DELAYSTY T ;RESTORE ORIGINAL TPULYRTS

DELAY PSHAPSHB

LDD #0DCOUNT ADDD #1 ;14 cycles x factor in D

CPD T ;before call to delayBNE DCOUNTPULBPULARTS

LDELAY JSR DELAYJSR DELAYJSR DELAYJSR DELAYRTS

************************************************************* INITS************************************************************

*-----------------------* INIT SONAR SYSTEM*-----------------------

INIT_SONAR LDAA SI1A8ADDA #2STAA SI1MINLDAA SI2A4ADDA #2STAA SI2MINLDAA SI3A5ADDA #2STAA SI3MINNOPRTS

*-----------------------* INITIALIZE IR SYSTEM*-----------------------

INIT_IR LDD #$ffSTD ME11IOLDAA LFA3ADDA #TOLLERANCESTAA MIDLLDAA RFA2ADDA #TOLLERANCESTAA MIDRRTS

*-----------------------* INITIALIZE SERVOS*-----------------------

INIT_SERVOS PSHAPSHBLDD #1 ;init steering factorsSTD STEERSTD ACCEL ;init acceleration factorsLDD #100STD ACCELFLDD #4STD DECELFLDD #$FFFFSTD TLDD #$FFSTD TURN ;Init. turn time factor

JSR PW1INIT ;Init. Steering pulse to centerJSR PW2INIT ;Init. Speed Control pulse to haltPULBPULARTS

*-----------------------* INITIALIZE COUNTER*-----------------------

INIT_COUNT PSHAPSHBLDX #BASELDD #0STD TCNT,X ;RESET COUNTERPULBPULARTS

*-----------------------* PULSE INITS*-----------------------

* Initialize OCx pulses

PW1INIT LDX #BASELDD PW1STD TOC2,X ;first transition Hi->LoSTD CENTER1 ;CENTERRTS

PW2INIT LDX #BASELDD PW2STD TOC3,X ;TEST CODESTD CENTER2 ;HALTADDD #SLOWR ;INIT. SLOW MOTIONSTD SLOWREVLDD CENTER2SUBD #SLOWFSTD SLOWFWDRTS

DUMMYC LDD #DUMMYPSTD PW1STD PW2STD CENTER1STD CENTER2LDD #DUMMYLSTD L1STD L2LDD #DUMMYPSTD PWA1STD PWA2LDD #DUMMYLSTD LA1STD LA2LDD #DUMMYMINSTD MIN1STD MIN2LDD #DUMMYMAXSTD MAX1STD MAX2RTS

*-----------------------* TIMER INIT

*-----------------------

* Set the Pre-Scaler Frequency for TCNT--first 64 cycles

TIMER LDX #BASEBCLR TMSK2,X BIT1 ;PR1:PRO=00 FOR 32.768 msecBCLR TMSK2,X BIT0 ;New count every 500nsRTS

*-----------------------* IC1-2 INIT*-----------------------

IC12INIT LDX #BASEBSET TCTL2,X BIT2 ;EDG2B:EDG2A=01BCLR TCTL2,X BIT3 ;CAPTURE RISING EDGE FIRSTBSET TCTL2,X BIT4 ;EDG1B:EDG1A=01BCLR TCTL2,X BIT5 ;CAPTURE RISING EDGE FIRSTBSET TMSK1,X BIT1 ;ENABLE IC2 INTERRUPTBSET TMSK1,X BIT2 ;ENABLE IC1 INTERRUPTLDD #0STD MAX1 ;INITIALIZE MAX/MINSTD MAX2 ;CALCULATIONS FOR ICLDD #$FFFFSTD MIN1STD MIN2RTS

*-----------------------* OC2-3 INITS (HI=11, LO=10)*-----------------------

* INITIALIZE OC2 (Bit7:6=OM2:OL2)

OC2INIT LDX #BASEBSET TCTL1,X BIT7 ;OM2:OL2=11BSET TCTL1,X BIT6 ;FOR SET TO HIGHBSET TMSK1,X BIT6 ;ENABLE OC2RTS ; INTERRUPT (Pulse 1)

* INITIALIZE OC3 (Bit5:4=OM3:OL3)

OC3INIT LDX #BASEBSET TCTL1,X BIT5 ;OM3:OL3=11BSET TCTL1,X BIT4 ;FOR SET TO HIGHBSET TMSK1,X BIT5 ;ENABLE OC3RTS ; INTERRUPT (Pulse 2)

*-----------------------* A/D INIT*-----------------------

ADINIT PSHALDX #BASEBSET OPTION,X BIT7 ;TURN A/D ONLDAA #40

WAIT DECABNE WAIT ;200 ECYCLES FOR STABILITY

PULARTS

*-----------------------* RTI INIT*-----------------------

INITRTI BSET PACTL,X BIT0 ;8.192ms RTI

BCLR PACTL,X BIT1BSET TMSK2,X BIT6 ;Enable RTI interruptsRTS

************************************************************* INTERRUPT SERVICE SUBROUTINES************************************************************

*--------------------------------------------------* INPUT COMPARE 1*--------------------------------------------------IC1ISR LDX #BASE

BRCLR TFLG1,X BIT2 IC1EXIT ;BRANCH AT ILLEGALBCLR TFLG1,X INV2 ;CLEAR FLAGBRCLR TCTL2,X BIT4 IC1TOLO

IC1TOHI BSET TCTL2,X BIT5 ;EDG1B:EDG1A=10BCLR TCTL2,X BIT4 ;CAPTURE FALLING NEXTLDD TIC1,XSTD TOHI1 ;PERIOD1 = CURRENT LO TO HISUBD PTOHI1 ; TRANSITION -STD P1 ; PREVIOUS LO TO HI TRANS.BRA IC1EXIT

IC1TOLO BCLR TCTL2,X BIT5 ;EDG1B:EDG1A=01BSET TCTL2,X BIT4 ;CAPTURE RISING NEXTLDD TIC1,XSTD TOLO1SUBD TOHI1 ;PW1 = TOLO1 - PTOHI1STD PW1

JSR RMTECHK1 ;DETECT PULSE WIDTH CHANGE

LDD TOHI1STD PTOHI1 ;UPDATE PREVIOUS TO CURRENTLDD P1 ;L1 = P1 - PW1SUBD PW1STD L1

IC1EXIT NOP*------------------------------------------* USE OF IC1 FOR A/D CHECKING* AND NIMMAX CALCULATIONS

JSR CHECKJSR MINMAX1JSR MINMAX2

*------------------------------------------RTI

RMTECHK1 LDD CENTER1SUBD PW1BLT TOPOS1

RCHKCMP1 CPD #THRESHOLDBLO NORMT1LDAA #1STAA REMOTE1BRA RCHKEXIT1

TOPOS1 STD PPW1LDD #0SUBD PPW1BRA RCHKCMP1

NORMT1 CLR REMOTE1RCHKEXIT1 JSR REMCHK

RTS

*--------------------------------------------------* INPUT COMPARE 2*--------------------------------------------------

IC2ISR LDX #BASEBRCLR TFLG1,X BIT1 IC2EXIT ;BRANCH AT ILLEGALBCLR TFLG1,X INV1 ;CLEAR FLAGBRCLR TCTL2,X BIT2 IC2TOLO

IC2TOHI BSET TCTL2,X BIT3 ;EDG2B:EDG2A=10BCLR TCTL2,X BIT2 ;CAPTURE FALLING NEXTLDD TIC2,XSTD TOHI2 ;PERIOD2 = CURRENT LO TO HISUBD PTOHI2 ; TRANSITION -STD P2 ; PREVIOUS LO TO HI TRANS.BRA IC2EXIT

IC2TOLO BCLR TCTL2,X BIT3 ;EDG2B:EDG2A=01BSET TCTL2,X BIT2 ;CAPTURE RISING NEXTLDD TIC2,XSTD TOLO2SUBD TOHI2 ;PW2 = TOLO2 - PTOHI2STD PW2

JSR RMTECHK2

LDD TOHI2STD PTOHI2 ;UPDATE PREVIOUS TO CURRENTLDD P2 ;L2 = P2 - PW2SUBD PW2STD L2

IC2EXIT NOP*------------------------------------------* USE OF IC2 FOR A/D SAMPLING

JSR ADNOI*------------------------------------------

RTI

RMTECHK2 LDD CENTER2SUBD PW2BLT TOPOS2

RCHKCMP2 CPD #THRESHOLDBLO NORMT2LDAA #1STAA REMOTE2BRA RCHKEXIT2

TOPOS2 STD PPW2LDD #0SUBD PPW2BRA RCHKCMP2

NORMT2 CLR REMOTE2RCHKEXIT2 JSR REMCHK

RTS

*--------------------------------------------------* REMOTE CONTROL CHECK*--------------------------------------------------

REMCHK LDAA REMOTE1ADDA REMOTE2CMPA #0BHI REMOTE_ONCLRASTAA REMOTEBRA REMDONE

REMOTE_ON LDAA #1STAA REMOTE

REMDONE RTS

*--------------------------------------------------* OUTPUT COMPARE 2*--------------------------------------------------

OC2IS LDX #BASEBRCLR TFLG1,X BIT6 EXITOC2 ;IGNORE ILLEGAL INTERRUPT

BCLR TFLG1,X INV6 ;Clear flagBRSET TCTL1,X BIT6 TOLOW2 ;End of positive pulse partBSET TCTL1,X BIT7 ;SET TO HIGH: OM2:OL2=11BSET TCTL1,X BIT6LDD TOC2,X ;LOAD CURRENT OC VALUESUBD OC2LTH ;SUBTRACT PREVIOUS LO TO HISTD LTHDIFF2 ;STORE DIFFERENCE IN TEMPLDD TOC2,X ;STORE NEW LO TO HISTD OC2LTH

JSR OC2CTRL1

BRA EXITOC2TOLOW2 BSET TCTL1,X BIT7 ;SET TO LOW: OM2:OL2=10

BCLR TCTL1,X BIT6LDD TOC2,X ;LOAD CURRENT HI TO LOSTD OC2HTL ;STORE IN TEMP

JSR OC2CTRL2

EXITOC2 RTI

OC2CTRL1 LDAA REMOTEBEQ AUTO21LDD OC2LTHADDD L1BRA CTRL21

AUTO21 LDD OC2LTHADDD LA1

CTRL21 STD TOC2,X ;SET NEXT (PERIOD BEGINS)RTS

OC2CTRL2 LDAA REMOTEBEQ AUTO22LDD OC2HTLADDD PW1BRA CTRL22

AUTO22 LDD OC2HTLADDD PWA1

CTRL22 STD TOC2,XRTS

*--------------------------------------------------* OUTPUT COMPARE 3*--------------------------------------------------

OC3IS LDX #BASEBRCLR TFLG1,X BIT5 EXITOC3 ;IGNORE ILLEGAL INTERRUPTBCLR TFLG1,X INV5 ;Clear flagBRSET TCTL1,X BIT4 TOLOW3 ;End of positive pulse partBSET TCTL1,X BIT5 ;FOR SET TO HIGH: OM3:OL3=11BSET TCTL1,X BIT4LDD TOC3,XSUBD OC3LTH ;D = CURRENT LO TO HI - OC3LTHSTD LTHDIFF3 ;STORE DIFFERENCE IN TEMPLDD TOC3,X ;LOAD CURRENTSTD OC3LTH ;STORE AS PREVIOUS

JSR OC3CTRL1

BRA EXITOC3TOLOW3 BSET TCTL1,X BIT5 ;FOR SET TO LOW: OM3:OL3=10

BCLR TCTL1,X BIT4LDD TOC3,XSTD OC3HTL

JSR OC3CTRL2

EXITOC3 RTI

OC3CTRL1 LDAA REMOTEBEQ AUTO31LDD OC3LTHADDD L2BRA CTRL31

AUTO31 LDD OC3LTHADDD LA2

CTRL31 STD TOC3,X ;SET NEXT (PERIOD BEGINS)RTS

OC3CTRL2 LDAA REMOTEBEQ AUTO32LDD OC3HTLADDD PW2BRA CTRL32

AUTO32 LDD OC3HTLADDD PWA2

CTRL32 STD TOC3,XRTS

*----------------------------------------* A/D SERVICE/NO INTERRUPTS*----------------------------------------

ADNOI NOPLDX #BASELDAA #%00010000STAA ADCTL,X

CONV1_4 BRCLR ADCTL,X BIT7 CONV1_4 ;Wait until flag is set

LDAA ADR2,XSTAA RFA2LDAA ADR3,XSTAA LFA3LDAA ADR4,XSTAA SI2A4

LDAA #%00010100STAA ADCTL,X

CONV5_8 BRCLR ADCTL,X BIT7 CONV5_8 ;Wait until flag is set

LDAA ADR5,XSTAA SI3A5LDAA ADR6,XSTAA RRA6LDAA ADR7,XSTAA LRA7LDAA ADR8,XSTAA SI1A8

ACONT2 NOPJSR SMINSJSR SDIFF

RTS

*----------------------------------------* ichar*----------------------------------------

ichar LDAA SCSR ; Check status reg.* ; (load it into A reg)

ANDA #$20 ; Check if receive buffer fullBEQ ichar ; Wait until data presentLDAA SCDR ; SCI data ==> A register

RTS ; Return from subroutine

*----------------------------------------* ochar; Input in A.*----------------------------------------

ochar PSHB ; Save contents of B registerLoop1 LDAB SCSR ; Check status reg (load it into B reg)

ANDB #$80 ; Check if transmit buffer is emptyBEQ Loop1 ; Wait until emptySTAA SCDR ; Register A ==> SCI dataPULB ; Restore B registerRTS ; Return from subtoutine

*----------------------------------------* PrintS; Index x[0]=first char.*----------------------------------------

PrintS PSHA ; Save contents of A register* SEI ; Disable interruptsLoop2 LDAA 0,X ; Get a character (put in A register)

CMPA #EOS ; Check if it's EOSBEQ Done ; Branch to Done if it's EOSJSR ochar ; Print the character by calling ocharINX ; Increment indexBRA Loop2 ; Branch to Loop2 for the next char.

Done NOP ; Dummy line* CLI ; Enable interrupts;

PULA ; Restore A register;RTS ; Return from subtoutine;

*----------------------------------------* CLEAR SCREEN*----------------------------------------

CLRS LDX #CLRJSR PrintSLDX #POSJSR PrintSRTS

*----------------------------------------* PRINT HEX IN ASCII (IN D)*----------------------------------------

HXTOA STD WORD ;STORE NO. IN TEMPANDA #%11110000 ;AND A WITH MASKLSRALSRALSRALSRAJSR ADJUSTJSR ocharLDD WORDANDA #%00001111JSR ADJUSTJSR ocharANDB #%11110000 ;AND B WITH MASKLSRBLSRBLSRBLSRBTBAJSR ADJUSTJSR ocharLDD WORDANDB #%00001111

TBAJSR ADJUSTJSR ocharRTS

ADJUST INCA ;ADJUST A FIRST TO ACC. FOR DECALDX #ZERO-1 ;ZERO TABLE INDEX FACTOR IN A

NUM INXDECABNE NUMLDAA 0,XRTS

*----------------------------------------* HEX TO DEC CONVERSION* Index of input's address in X*----------------------------------------

Convert JSR HTOD ;Convert to dec -> ASCIILDX #DBUFR ;Load converted no's indexCLRB

Digits LDAA 0,X ;Load byte X [0:4]JSR ochar ;Output Byte in AINX ;X = X+1, next digitINCB ;Increment BCMPB #5 ;Compare with totalBNE Digits ;Branch if != 5RTS

*----------------------------------------* SCI Initialization*----------------------------------------

InitSCI PSHA ; Save contents of A registerLDAA #$30 ; Set BAUD rate to 9600STAA BAUDCLR SCCR1 ; Set SCI Mode to 1 start bit,

* ; 8 data bits, and 1 stop bit.LDAA #$0C ; Enable SCI TransmitterSTAA SCCR2 ; and ReceiverPULA ; Restore A registerRTS ; Return from subtoutine

*--------------------------------------------------* PORTD INIT*--------------------------------------------------

PDINIT LDX #BASELDAA #IOPATSTAA DDRD,XRTS

****************************************************************************** HTOD - Subroutine to convert a 16-bit hex number to a* 5 digit decimal number.** Uses 5 byte variable "DBUFR" for decimal ASCII result* On entry X points to hex value to be converted & displayed* All registers are unchanged upon return*****************************************************************************

HTOD PSHX ;Save registersPSHBPSHALDD 0,X ;D=hex value to be converted

LDX #10000IDIV ;freq+10,000 -> X; r -> DXGDX ;Save r in X; 10,000s digit in D (A:B)ADDB #$30 ;Convert to ASCIISTAB DBUFR ;Store in decimal bufferXGDX ;r back to DLDX #1000IDIV ;r+1,000 -> X; r -> DXGDX ;Save r in X; 1,000s digit in D (A:B)ADDB #$30 ;Convert to ASCIISTAB DBUFR+1 ;Store in decimal bufferXGDX ;r back to DLDX #100IDIV ;r+100 -> X; r -> DXGDX ;Save r in X; 100s digit in D (A:B)ADDB #$30 ;Convert to ASCIISTAB DBUFR+2 ;Store in decimal bufferXGDX ;r back to DLDX #10IDIV ;r+10 -> X; r in D (B is units digit)ADDB #$30 ;Convert to ASCIISTAB DBUFR+4 ;Store to units digitXGDX ;10s digit to D (A:B)ADDB #$30 ;Convert to ASCIISTAB DBUFR+3 ;Store in decimal bufferPULA ;Restore registersPULBPULXRTS ;Return

*****************************************************************************

END

Tuco - University of Florida · PDF fileTuco is an autonomous slave RC that has the ability to...

Documents

Transcript of Tuco - University of Florida · PDF fileTuco is an autonomous slave RC that has the ability to...