EE3333 - Section 001
Project MN3005
A Small Autonomous Fire-Fighting Robot System
 Jason Plumb
4/14/98
Instructor:
Dr. Portnoy
Project Advisor:
Dr. Mehrl
Team Members: 
Brent Short and John Walter
Table Of Contents  

Abstract

This paper describes the design and construction of a small autonomous robot for entry in the 1998 Fire Fighting Robot Competition.  At the heart of the system is the 68HC12 microcontroller by Motorola.  Program code to control the fire fighting robot is written in 68HC12 native assembly language.  The system controls two optically isolated stepper motors for precision movement.  Furthermore, the robot performs analog to digital conversion on 6 infrared sensors:  4 for wall proximity detection, one to detect floor markings, and one for candle detection.  The 4 proximity sensors utilize heterodyne modulation of the IR signals to reduce the effects of ambient lighting.  The extinguishing system is comprised of a large fan salvaged from a toy hovercraft, and a 3.5kHz tone decoder circuit is used to start the robot and gain bonus points. 


 
Introduction

The annual Fire Fighting Robot Competition sponsored by Trinity College has been an exciting event for several years.  Robot hobbyists and professionals of all ages from all parts of the world gather to compete and show off their creations.  The goal of the event seems simple:  Navigate a model house floorplan, find a lit candle, and extinguish it.  As simple as this may sound, it is an intricate process to construct a device which can accomplish such a task.  There are a vast number of design options and operating techniques that can be explored.

As the contestís web page states, a primary purpose of the contest is to "provide an incentive for the robotics community to develop what will be a practical application for a real-world robot".  Although the contest is merely a simulation of a real-world scenario, it requires the designers to use practical techniques to create useful designs.  The competition serves as an example of what robots can do on a larger scale.

In the first year of competition, there were only a few robots that were able to successfully find and extinguish the candle reliably.  The more recent events, however, have yielded a larger number of successful entries.  It appears, that the designs are becoming more sound as the robotics community learns which approaches work and which fail.  Because of these improvements, the event has a higher level of competition.  An entry that strives to perform well must be fast and reliable. This designers of this project aim to accomplish both these tasks.

Contest Rules

Although the rules of the contest are lengthy and detailed, an overview will be given to provide a better understanding of the design approach used in this project.  See the Rules Page (Appendix B) on the Trinity College web page for the full list of contest rules.

Overview of Robot System

Figure 1 is a functional block diagram of the robot system.  At the heart of the robot is the 68HC12 microcontroller from Motorolla.  The microcontroller is responsible for sending signals to and receiving signals from the robot hardware.  First, the 68HC12 receives input from the calibration button before each run.  This allows the user to align the robot at a specific distance from the desired wall to be followed.  Once this has occured, the 68HC12 waits for a logic low from the tone-decoder. Then, the controller outputs to the optoisolators to control the motor driver circuits.  The controller also reads values from the IR phototransistors in order to detect walls and search for the candle.
 

Figure 1:  Robot System Block Diagram

Stepper Motors and Stepper Driver Circuit

When deciding how to move the robot through the house, the designers realized that precision movement would be necessary in order to avoid touching the walls and receiving penalties.  In order to achieve the required precision in movement, it was decided that the robot would utilize stepper motors.

The main benefit of stepper motors is that they are able to turn a specific number of degrees for every step.  A four phase stepper motor has four coils that, when energized in a specific sequence, rotate a driving magnetic field which, consequently, rotates a set of permanent magnets.  These permanent magnets are attached to a rotor which drives an output shaft.  Thus, by pulsing the coils in a certain sequence, a clockwise or counterclockwise movement can be attained.

The following table shows a typical stepping sequence for a four phase stepper motor:
 

Step Sequence
 
1
2
3
4
Coil A
on
on
off
off
Coil A'
off
off
on
on
Coil B
on
off
off
on
Coil B'
off
on
on
off
 
Table 1:  Typical Stepping Sequence of a Four Phase Stepper Motor

A change in the coil states (ie. changing from state 2 to state 3 as shown above) results in a single step of the motor shaft.  Direction is easily controlled by running through the above sequence either forward or backward.  It should also be noted that the coils A and A' are always oppositely charged, as are coils B and B'. By inverting the signals going to coils A and B, the corresponding signals A' and B' can be attained.  Thus, only two control lines are required to place the motor into any one of the 4 possible states.  Even though this is an important consideration for certain applications, the controller used in this implementation has a sufficient number of lines to control each coil.  Furthermore, because two of the coils are always energized at any given time, the rotor is held into place by the two magnetic fields and hence will not easily slip -- even when the motor is not turning.  This is another benefit of stepper motors.  Figure 2 provides an internal diagram of a typical four phase stepper motor.

Figure 2:  Internal Diagram of 4 Phase Stepper Motor

The stepper motors used for this project were salvaged from surplus Epson printers.  The steppers are designed to provide 1.8 degrees per step (or 200 steps per revolution) and supply a sufficient amount of torque.  However, the current requirements of almost any motor are more than a digital output can provide.  Because of this requirement, a transistor circuit is needed to drive the motor coils.

The circuit shown in Figure 3 is used to drive the motor coils.  Because there are a total of eight motor coils in the robot, eight of these circuits are needed.  The circuit functions by receiving a digital input from the microcontroller. This signal is fed to an optoisolator in order to separate the low-voltage, low-current microcontroller from potentially dangerous signals in the motor driver circuit.  In other words, the optoisolator allows the 68HC12 to control the motors without any physical connection to the driver circuit.  The output side of the optoisolator then drives the base of the TIP112 driver transistor.  Just as the stepper motors were, the TIP112 transistors were salvaged from the Epson printers.  The TIP112 power transistors are able to supply 50 watts of power, which is more than sufficient to drive the stepper motor coils.
 

Figure 3:  Stepper Motor Driver Circuit

When the microcontroller outputs a digital low signal (logic 0), the output side of the optoisolator acts as an open circuit (ie. no current flows into the collector).  The remaining current path is then from Vcc through the pull-up resistor and into the base of the TIP112 driver transistor.  This effectively turns the transistor on so that current can flow from collector to emitter.  Clearly, current will flow from the 7.2V battery, through the motor coil, into the collector of the TIP112, and to the emitter ground.  Hence, when a logic 0 is sent from the 68HC12 to a given driver circuit, the corresponding coil will become energized.  Inversely, when the microcontroller outputs a digital high signal (logic 1), current will flow from collector to emitter in the output of the optoisolator.  This will restrict current from flowing into the base of the driver transistor -- causing it to be turned "off".  When the driver transistor is "off", there is no path to ground and thus the coil will not be energized.

The current dissipating diode (or "free-wheeling diode")  D1 is used so that the current stored in the motor coil does not damage the power transistor.  When the transistor is turned off, the large magnetic field stored in the motor coil could generate a current spike through the collector to ground.  The diode, however, will allow this potentially dangerous current to flow around the motor coil until it has been dissipated.  The diode also helps to reduce the time it takes for the motor coil to switch from an "on" state to an "off" state.  The speed at which a stepper motor can reliably rotate is relative to the amount of time it takes for a motor coil to switch states.  By reducing the transition time, the stepper motor can be driven at higher frequencies resulting in a higher velocity.  A small resistor in series with diode D1 has also been added to help reduce the transition time of the motor coil.

The shaft of each stepper motor is mounted directly to a 4" diameter rubber wheel.  The wheels were purchased at a local hobby shop and provide a sufficient amount of friction with the floor of the house.  In order to calculate how much the robot will move with each step of the motor, the following equation for arc length can be used:

As one can see, the stepper motors give a wonderful amount of resolution in terms of distance. Additionally, a test was performed to determine how long both steppers could be driven before the supply battery would fail to move the robot.  With a freshly charged 7.2V battery pack, the steppers pulled a total of 4.4A.  This current draw, naturally, was reduced as the battery voltage drained.  After 27 minutes of constant movement under full robot load, the steppers eventually failed to provide enough torque to move the robot.  Prior to stopping, the stepper motors were drawing 3.2A.  Although the steppers draw a serious amount of current from the battery, the test proves that the robot should be able to navigate the model house three times on a single freshly-charged battery.

Infrared Sensors and Sensor Circuits

In order for the robot to determine its position in the house, some kind of proximity sensing device is needed.  After some consideration, an infrared system was chosen for its simplicity.  Other types of sensors (specifically ultrasonic) do give higher degrees of accuracy when measuring distance; however, infrared remains a simple and sufficient solution at a lower price tag.  Additionally, the infrared sensing devices are available locally.

Infrared phototransistors function by using light to stimulate the electrons in the base of a transistor.  Simply put, when light falls onto the device, the transistor becomes "closed" and current is allowed to flow from collector to emitter.  Similarly, when the device is in complete darkness, the transistor is "open" and the collector current is zero.  The amount of collector current is directly related to the amount of light on the device.

Because the infrared component of ambient light (or normal room light) may change depending on several uncontrollable environmental factors, the ambient light alone cannot be used for proximity sensing.  Rather, an infrared light emitting diode (IR LED) is used to provide a more usable level of IR to the phototransistor.  When the IR LED is forward biased, IR light will emit out of the device and reflect off the white walls back to the phototransistor.  Because light intensity falls off as a function of distance, the amount of IR sensed by the transistor is directly related to the distance to the reflecting surface. All transistors, however, have a limited operating region.  Specifically, a phototransistor's collector to emitter voltage will only vary within a certain range of light intensities.

To prove that the IR sensors would be sufficient for proximity detection, a series of tests were performed and measurements were recorded.  In each test, a phototransistor was biased with a different collector resistor to determine an adequate resistance value.  The results of the tests were graphed as transistor voltage as a function of wall distance.  Figure 4 shows these results.

Figure 4:  Phototransistor Sensor Voltage (Vce) vs. Wall Distance

The project nature requires that the sensors have good resolution at relatively close ranges. Although the 10k and 20k Ohm resistors gave a smooth range of values between 3 and approximately 16 inches, it was decided that a low-range cut-off of 3 inches would not suffice.  The 5k Ohm resistor performed better in that it gave a lower cut-off point of approximately 2 inches, while the 1k Ohm resistor failed to detect the wall at approximately 5 inches.  The measurements show that a 3k Ohm resistor provides the perfect bias:  A moderately smooth change in transistor voltage occur between 1.5 and 8 inches.  Because the critical robot movements occur in the 18 inch-wide hallways and the robot is 12 inches wide, the side sensors will be able to accurately determine the distance to each wall.

It should be noted, though, that the above graph reflects sensor measurements taken with a small flashlight illuminating the wall surface.  Because a flashlight bulb can provide a large amount of IR, it was the first choice for the infrared source.  It was later discovered that the flashlight bulbs would be a poor choice for two reasons.  First, the bulbs have a slow on-off time, so slow that it is visible to the human eye.  With the large number of sensor readings that the robot must make, the slow switching becomes intolerable.  Second, the bulbs draw far too much current at approximately 1A per bulb.  With 7 bulbs (one for each corresponding sensor), the current drain would become too much for a small mobile robot of this kind.

The solution was to repeat the above process using IR LEDs instead of flashlight bulbs, take measurements, and produce a similar graph.  From this graph, the designers were able to determine that a 15k bias resistor would be needed to work well with the IR LED.  Because the LEDs emit far less IR light than the flashlight bulbs, the phototransistors must be biased differently.

Initially, a simple proximity sensing solution was implemented using a IR LED and phototransistor pair.  This method was adequate, but performed rather poorly when subjected to differing ambient light conditions.  It was decided that an improved solution must be constructed.  In order to cancel out the effects of ambient light, a form of heterodyne modulation of the IR signal is used.  Figure 5 shows the demodulation circuit.

Figure 5:  Phototransistor Demodulation Circuit Diagram

The circuit theory is straightforward.  The 68HC12 uses one of the built-in pulse-width modulation (PWM) channels to strobe the base of a TIP112 power transistor and flash an IR LED at a frequency of 1kHz.  The signal received at the phototransistor will be comprised primarily of a DC level (from ambient light), a 120Hz signal (from standard room lights), and the 1kHz signal emitted by the IR LEDs.  The idea then is to filter out the unwanted signals and obtain only the 1kHz "carrier".  The intensity of the 1kHz signal can then be used to give an accurate indication of how far the robot is from a certain wall in the house.

A 741 operational amplifier is used to create a bandpass filter for phototransistor signal.  This circuit was designed around a high-pass filter from the AARL handbook.  By adding a capacitor in the positive feedback path, the lower frequencies can be restricted.  The result is a bandpass filter.

The output of the bandpass filter is the recovered 1kHz signal.  However, this signal cannot be read directly by the analog to digital converter on the 68HC12 because it swings to negative voltages.  To make this signal useful, the signal must be converted to a relative DC level (or intensity level).  In order to do this, the output of the 741 is passed through a diode that acts as a half-wave recitifier.  The result is a waveform comprised of only positive levels.  This signal is then passed over a 47uF capacitor to ground in order to smooth the signal to a near-DC level.  The 10k parallel resistor provides a current path for the voltage stored in the capacitor, so that the voltage can change rapidly.  Because the filter may output signals greater than 5V, a 5V zener diode is used to clip the output at 5V.

By connecting the analog to digital input of the 68HC12 to the output of the filter, an accurate measurement of a wall distance can be obtained.  Most importantly, the measurement will not be influenced by the amount of ambient light in the house.

As stated previously, there are 4 sensor channels that utilize the above-described modulation/demodulation technique.  These correspond to the right, left, front, and back wall sensors.  The floor sensor, however, need only be capable of determining if a white line is present.  In other words, the floor sensor measurement is converted to a binary value (present, not present), and therefore modulation is not necessary.

The candle detection circuit also implements a phototransistor. However, because a candle emits a substantially large amount of IR, an IR emitter is not required.  When the phototransistor is directly pointed at a lit candle, the collector to emitter voltage drops approximately 1.7V at a distance of 24 inches.  Even at a distance of 3 feet, there is a significant drop in voltage to be detected by the microcontroller's A/D and to indicate the presence and direction of the candle.  These specifications are within the requirements of the project.  To further improve the directionality of the candle sensor and to improve its distance threshold, a reflective parabolic dish was placed around the candle sensor.  By using a simple phototransistor for candle detection, the robot will be able to determine if the candle is present from the doorway of any room.

Tone Decoder Circuit

In order to obtain a 5% reduction in time, the robot is capable of detecting a 3.5kHz tone to begin its trials.  The project designers hope that such bonuses may help the robot to be more competitive during competition.  The tone decoder circuit is presented in Figure 6.

Figure 6:  3.5kHz Tone Decoder Circuit

The circuit is based around two integrated circuits:  An LM385 audio amplifier and the LM567 tone decoder IC.  The LM385 is used to amplify the weak signal picked up by the condenser microphone, and the LM567 is used to filter the signal and provide a logic low when the signal is present.

An audio signal is picked up by a small condenser microphone and fed to the input of the audio amplifier.  The LM385 is useful in that it requires only a single positive supply voltage, unlike most similar amplifiers.  This reduces the need for an additional power supply -- a serious consideration for a small robot.  The LM385 is designed to give a typical gain of 20, but the use of feedback may increase the gain to more than 100.  The potentiometer between the output pin and the negative input pin supplies this feedback and allows the circuit to be adjusted.

After the audio signal is amplified, it passes through a filter capacitor (C4) and into the input pin of the LM567.  The LM567 is precisely designed for the decoding of tones.  The frequency of an internal local oscillator is set by a timing resistor Rt on pin 5 and a timing capacitor on pin 6.  This operating frequency of the LM567 is approximately equal to this local oscillator frequency.  As the part's data sheet states, the frequency can be obtained by choosing Rt and Ct such that

For this circuit, Rt is comprised of a fixed 5k resistor and a potentiometer so that the frequency of operation can be adjusted, or the circuit can be tuned. After the circuit was constructed and tuned to a relatively low volume input signal, several measurements were recorded.  These measurements show the gain of the amplifier to be approximately 60, and the bandwidth of the tone decoder to be approximately 500Hz (or 14.3% of the operating frequency).

The Motorola 68HC12 Microcontroller

The advisors for this project indicated that the 68HC12 microcontroller by Motorola is to be used as the robot "brains".  The 68HC12 is a 16-bit microcontroller with several useful features relevant to this project, including

As indicated previously, 6 channels of analog to digital conversion are being used to perform sensor readings.  The improved clock speed (compared to the 68HC11 predecessor) will allow more computations per second, resulting in improved navigation control.  The Flash EEPROM can allow larger and more complex programs to be written.

Unfortunately, the Flash EEPROM array on the 68HC12 chip can only be reprogrammed 100 times.  This is a serious limitation when designing, testing, and debugging program code.  Because of this limitation, the project designers have attempted to keep the code simple so that it can fit into the 768 bytes of byte-erasable EEPROM.  The byte-erasable EEPROM is more useful in that it can be reprogrammed 10,000 times.  Ideally, as reliable and stable sections of the robot control code are completed, they could be programmed into the Flash EEPROM and used at a later time.  However, the default programming utility (D-Bug 12) that allows programming of the byte-erasable EEPROM resides in the Flash EEPROM.  Any programming to the Flash EEPROM array will remove the important debugging utility and make code development much more difficult. Ideally, a smaller programming utility could be developed to replace D-Bug12 and coexist in Flash EEPROM with other robot functions.

Program code to control the robot is written in the 68HC12's native assembly language.  Another nice feature is that 68HC11 code can be compiled and executed on the 68HC12.  See Appendix C for a listing of the current source code.

Algorithms for Robot Control 

Because the floorplan is fixed and the placement of the candle can be considered random, the robot can be programmed to traverse the house in a specific room order.  The entire program operation is straightforward:  Wait for the tone, then proceed to first room, if candle is present, locate and extinguish it, otherwise proceed to the next room.  Once the candle has been successfully extinguished, the robot will return to the starting location for a bonus reduction in time.  Figure 7 shows a program flowchart for this operation.

Figure 7:  Program Flowchart Showing Overview of Robot Operations

In order for the robot to accomplish the above tasks, it must first navigate through the hallways in order to find a room.  Ideally, the robot must be able to find its way into a given room from any location in the house.  Figure 8 demonstrates how this is to be accomplished.  Simply put, the robot is able to follow a given wall by comparing sensor readings with the calibration readings.  If the robot determines that it is too close to the specified wall, it sets the motor directions and turns away from the wall.  Similarly, if the robot is too far from a wall, it will set the motor directions and turn toward the wall.  Although this may seem like a poor control method, the small steps of the stepper motors provide a seemingly fluid robot movement.  A relatively simple means of navigating the entire house, then, is to simply follow a given wall until a room has been entered.  As mentioned before, this condition can be detected when the floor sensor picks up the white line marking the room doorway.
 

Figure 8:  Flowchart Wall-Following Algorithm

Once the robot has found its way into a room, it must then determine if the candle is present.  As stated previously, the sensors should be capable of locating the candle from the doorway of any given room.  To ensure that the robot can scan the entire room contents, it first attempts to drive 12 inches into the room.  If it encounters the circle surrounding the candle or travels all 12 inches, it will begin a counterclockwise sweep to "look" for the candle.  If the candle is present in the given room, the robot will move toward it (if it is not already close enough) and blow it out.  Figure 9 demonstrates the Room subroutine and how the robot behaves once it has entered a room.  It should also be noted that if the candle has already been extinguished, the robot will simply stop at the doorway and back up to the facing wall.
 

Figure 9:  Flowchart for Room-Handling Algorithm
 

Once the robot has processed all four rooms, it can then realign with a wall and follow that wall back to the starting point.  Again, during the return trip, the robot does not enter the rooms looking for the candle.

By performing the tasks listed in the above three flowcharts, the robot is able to enter the house, locate the candle, extinguish it, and navigate back to the beginning. A series of tests were perfomed on the robot to see how it would perform with the candle in various locations in the house.  24 fixed candle locations were chosen and a series of 24 trials were conducted. Out of the 24 trials, the robot was able to successfully extinguish the candle 21 times.  At the time of the trials, program code was still under development, and thus the robot only completed its return (without touching any of the walls) on 13 of the 21 attempts.  The time taken for the robot to reach the candle and extinguish it ranged from 14 seconds in the first room to 49 seconds in the fourth room.  With a 55% reduction in time, the worst case scenerio would be approximately 27 seconds per trial.  These results demonstrate that the robot can accomplish all desired tasks while performing reliably.

Conclusion

The annual fire fighting robot competition is an interesting event that challenges the contestants to design a small robot that is capable of finding and extinguishing a lit candle. A solid and reliable design for this competition has been constructed and tested with promising results.

The two-wheel design allows the robot precision movement by using stepper motors while also keeping control simple.  Proximity sensing is accomplished though the use of IR phototransistors.  The devices work well in allowing the robot to determine how far it is from the walls in any given direction up to a distance of approximatley 6 inches.  By modulating the IR emitters and passing the phototransistor output through a bandpass filter, the effects of ambient light can be greatly reduced.  The two rechargable batteries (for the motors and logic circuits, respectively) have been tested under full load and are capable of supplying the robot with enough power to complete all three of its trials.  By using a tone decoder to begin its trials, the robot will receive a 5% reduction in overall time.  Furthermore, the program code written for the 68HC12 microcontroller is capable of moving the robot through the model house without touching any walls.  It can enter each of the rooms, scan for the lit candle, and extinguish it if it is present.  Once is has navigated the entire house and put out the candle, the robot will the navigate back to the starting position.

At this point in time, though, the primary task left to be completed is continued development of program code.  In certain portions of the house, the robot does not move efficiently (that is, it comes quite close to hitting the walls).  As the program code improves, though, the robots performance improves.  The existing code (see Appendix C) does, however, successfully accomplish all necessary tasks for competition.

The existing robot chassis has evolved from an early prototype.  Although the existing chassis serves its basic purpose of supporting the necessary hardware, the design can be improved.  An improved chassis design could be lighter to create less torque on the stepper motors.  This should result in a faster maximum speed and/or a longer battery usability.  Also, an improved chassis could allow better placement of the circuitboards and wiring that is less disorderly.

By developing software to write programs into the byte-erasable EEPROM on the 68HC12, the authors could more effectively utilize the available memory.  This would allow the D-Bug12 software to be replaced and would free up most of the flash EEPROM space for additional code development.  The software listed in Appendix C nearly fills the entire array of byte-erasable memory, and this has become a serious limitation.  With more code space available, the robot would be able to perform more rigorous computations on the incoming sensor data.  Specifically, a more stable control method could be implemented.



References
  1. Benjamin C. Kuo, Step Motors and Control Systems, SRL Publishing Company, Champagne, IL, 1979.
  2. AARL Handbook
  3. Brent Short, Lab Notebook
  4. John Walter, Lab Notebook
  5. Circuit Land Web Page, http://www.uoguelph.ca/~antoon/circ/circuits.htm
  6. Parallax, Inc, Basic Stamp Manual, version 1.8, Parallax, Inc.  1997.
  7. Trinity College, Fire-Fighting Home Robot Contest, http://www.trincoll.edu/~robot

Appendix A - Arena Floorplan


Appendix B - Full List of Contest Rules

The full list of contest rules can be located at the Trinity College Fire Fighting Robot 1998 Contest Rules page.

Appendix C - Program Source Code

The following is a complete listing of the most recent version of the program code used to control the robot.  The program comments in the header describe the robots behavior with this code.

;------------------------------------------------------------------
;
; FIRE FIGHTING ROBOT PROGRAM
;       by Jason Plumb, Brent Short, and John Walter
;       EE3333 - Project Lab 3
;       Spring 1998
;
; 3/24 Goes to every room and looks for the candle.  If the candle
; is present, it will go toward it and look for the circle.  When it
; reaches the circle it will turn the fan on and blow out the candle.
; It will not exit back out of the house once it blows out the candle...
; it will just sit there.
;
; 3/28 - This code has been reduced in size
;        * FWD, BK, and STOP have been replaced with hard coded values
;        * fwd_motors and bk_motors routines have been implemented
;
; 3/31 - This is the reduced code with modulation added in.
;        Specifically, it uses PWM for the LED switch and the logic
;        has been reversed
;
; 4/3  - Candle threshold is set for using flashlight reflector
; 4/4  - This has step count added back in and has been modified
;        so that the robot will detect the candle circle if it is
;        < 12" from the door
;      * This code has been modified so that the robot aligns properly
;        when entering the rooms.  This is done in the go_in routine
;        Note:  This code needs serious reduction - it fills almost
;               all available byte erasable eeprom
; 4/9 - won't hit wall if candle is close -makes sure it has
;        gone a certain distance
;------------------------------------------------------------------
; Begin program equates here
;------------------------------------------------------------------
RAMSTART:  equ   $0800           ; Start of RAM for the HC12
PROGSTART  equ   $0D00           ; Start of program in jump EEPROM
PORTA:     equ   $0000           ; Address of port a on hc12
DDRA:      equ   $0002           ; Data direction register for port A
PORTB:     equ   $0001           ; Address of port b on hc12
DDRB:      equ   $0003           ; Data direction register for port B
PORTE:     equ   $0008           ; Address of port e on hc12
DDRE:      equ   $0009           ; Data direction register for port E
ATDSTAT:   equ   $0066

ADR0H:     equ $70     ;A/D Converter Result Register 0
ADR1H:     equ $72     ;A/D Converter Result Register 1
ADR2H:     equ $74     ;A/D Converter Result Register 2
ADR3H:     equ $76     ;A/D Converter Result Register 3
ADR4H:     equ $78     ;A/D Converter Result Register 4
ADR5H:     equ $7A     ;A/D Converter Result Register 5
ADR6H:     equ $7C     ;A/D Converter Result Register 6
ADR7H:     equ $7E     ;A/D Converter Result Register 7

RAMSTRT:        EQU     $0800   ; start of internal ram
RAMSIZE:        EQU     $0400   ; Size of internal ram
STACK:          EQU     RAMSTRT+RAMSIZE ; top of stack

ATDCTL2:   equ   $0062
ATDCTL3:   equ   $0063
ATDCTL4:   equ   $0064
ATDCTL5:   equ   $0065
ATDSTATH:  equ   $66     ;ATD Status Register High
ATDSTATL:  equ   $67     ;ATD Status Register Low

COPCTL:    equ $16     ;COP Control Register
COPRST:    equ $17     ;Arm/Reset COP Timer register

PWEN:   equ $0042       ; PWM enable register
PWPER0: equ $004C       ; PWM period channel for channel 0
PWCLK:  equ $0040       ; PWM clocks and concatenate (used to concat ch0 and ch1)
PWDTY0: equ $0050       ; PWM duty cycle for channel 1

;----------------------------------------------------------------------
; Begin data segment
;----------------------------------------------------------------------
    org RAMSTART
temp        db $00
tempfw      dw $00
tempw       dw $00
NUM_STEP90  db $87
speed       dw $05FF
led_speed   dw $0000
sensor_diff db $0
rtmtr       db $58,$3D
ltmtr       db $58,$3D
;FWD        db $58
;BK         db $54
;STOP       db $a7

; RAM variables used for holding sensor readings begin here
s0loff  db $FF
s1loff  db $FF
s2loff  db $FF
s3loff  db $FF
s4loff  db $FF
s5loff  db $FF
s6loff  db $FF
s7loff  db $FF

s0lon  db $FF
s1lon  db $FF
s2lon  db $FF
s3lon  db $FF
s4lon  db $FF
s5lon  db $FF
s6lon  db $FF
s7lon  db $FF

s1_close_thresh  dw $0050
s2_close_thresh  db $FF
s3_close_thresh  db $FF
s5_close_thresh  db $FF
s6_close_thresh  db $FF
s2_far_thresh    db $FF
s3_far_thresh    db $FF
s5_far_thresh    db $FF
s6_far_thresh    db $FF
s2_center_thresh db $FF
s3_center_thresh db $FF
s5_center_thresh db $FF
s6_center_thresh db $FF

wall_status  db $00
pwall_status db $00
abs_thresh   db $00             ; The absolute threshold for any given sensor
exist_thresh db $18

sens_addr         dw $0072            ; Parameter to follow_wall function
sens_far_thresh   db $00
sens_close_thresh db $00
follow_wall       db $00              ;0=right, not zero = left
low_step          dw $00              ;step count for lowest candle value
low_value         db $00              ;lowest candle value - init to $FF
old_y             dw $00
old_x             dw $00              ; Artificial stack vars
abovect           db $00
extinguished      db $00
stepct            db $00
old_rt            db $00
in_circle         db $00
;----------------------------------------------------------------------
; Begin main program
;----------------------------------------------------------------------
 org PROGSTART
;----------------------------------------------------------------------
; Initialization stuff here:
;----------------------------------------------------------------------
INIT_STUFF:
 ldaa    #$0
 staa    COPCTL          ; Disable the COP watchdog timer
 staa    ATDCTL3
 staa    extinguished
 staa    stepct
 staa    in_circle
 ldaa #$FF
 staa DDRB               ; Set direction for port B (all output)
 staa PORTB              ; Write initial PORT B value (unlock mot)
 movb #%11101101, DDRA   ; Set direction for A (outputs)
 ;ldaa ;PORTA
 ldaa #%00001101         ; Ensure that calibration LED and IR LEDs on and Fan is off
 staa PORTA              ; Put byte back on port A
 
 movb #$80, ATDCTL2      ; Set up ATD to function normally

 ;movb #$00, ATDCTL3      ; Select continue conversion in BGND Mode
    ; Ignore FREEZE in ATDCTL3
 movb #$01, ATDCTL4      ; Select Final Sample time = 2 A/D clocks
    ; Prescaler = Div by 4 (PRS4:0 = 1)
 ;The next x lines set up RAM variables during init...
 ;that way, they aren't lost when we power down.
 ;(not used, hard coded)movb #$81,   NUM_STEP90
 ;(speed is hard coded in the delay routine)
 ;movw #$1700, speed ;    #$1D10, speed         ; Speed variable
 ldd #$003D
 std rtmtr
 std ltmtr
 ;movw #$003D, rtmtr
 ;movw #$003D, ltmtr

 ;movb #$40, s1_close_thresh  ; Just hard coded for now
 ;movw #$01F6, steps_360 ; (was 1f8)  (was 200)
 ;movb #$55, candle_thresh

 ; The next 4 lines set up and start PWM for modulating IR LEDs
 movb #$40, PWCLK        ; Concat channels 0 and 1
 movb #$03, PWEN         ; Enable PWM
 movb #$1F, PWPER0       ; Set channel 0 period
 movb #$10, PWDTY0       ; Set channel 0 duty cycle
 ;movb #$00, stepct
 
 ;jsr INIT_STUFF          ; Do initialization stuff
;----------------------------------------------------------------------
;end init
;----------------------------------------------------------------------
 jsr WaitForTone         ; Actually, wait for button

 jsr Read
 ldaa ADR6H             ; Get right wall calibration value
 ldab ADR2H
 addd #$0A0A            ; Add for close threshold (due to modulatioN)
 staa s6_close_thresh   ; Calculate and store thresholds
 stab s2_close_thresh
 subd #$1414
 staa s6_far_thresh
 stab s2_far_thresh

 jsr wait_toggle         ; Should turn it back on
 jsr WaitForTone         ; tone or button
 movb #%00110011, PORTB  ; Write initial value to port B
basic:                   ;
 ldy #$00FF
 jsr fwdabit
 movb #$FF, follow_wall  ;left wall
 jsr Follow
 
 jsr RoomL               ; First room (island)
 
 jsr right_90
 movb #$00, follow_wall  ;right wall
 
 jsr Follow
 
 jsr RoomR               ; Second room
 jsr right_90
 jsr Follow
 jsr RoomR               ; Third room
 jsr right_90
 jsr Follow
 jsr RoomR               ; Fourth (last) room
 jsr left_90
 
 ldaa #$FF
 staa count_steps
 
 ldaa #$FF
 staa follow_wall  ; Follow left wall out of house
 staa done
 jsr Follow
 jsr Room
 jsr right_90
 ldy #$00DA
 jsr fwdabit
 jsr Follow
quit:   movb #%11111111, PORTB
stop:   bra stop

;----------------------------------------------------------------------
; This will wait for the tone to be present for a given period of time
; before continuing.  If the button is pressed, it will also exit
;----------------------------------------------------------------------
WaitForTone:
 ;stx old_x
 ldx #$3000
Tone2:  ldaa PORTA
 bita #%00000010         ; Check for button
 bne ToneExit
 bita #%00010000
 beq TonePres            ; The tone is present
 bra WaitForTone
TonePres: dex
   beq ToneExit
   bra Tone2
ToneExit:; ldx old_x             ; Artificial stack
;----------------------------------------------------------------------
; This will toggle the status of the calibration LED
;----------------------------------------------------------------------
ToggleCLED:
 ldaa PORTA
 eora #%00000100
 staa PORTA
 ;ldy old_y               ; Artificial stack
 rts
;----------------------------------------------------------------------
;wait $100 delay cycles - used for calib. LED - then go on to toggle
;----------------------------------------------------------------------
wait_toggle:
 ;sty old_y               ; Artificial stack
 ldy #$0100
wtlp1:  jsr DELAY         ; Wait before turning LED back on
 dey
 bne wtlp1
 bra ToggleCLED
;----------------------------------------------------------------------
Read:   movb  #%01010100 , ATDCTL5    ; Initializes ATD, MULT=1 (do conversions on AN0-AN7)
          ; Run conversions on subsequent channels
WTCONV: BRCLR   ATDSTATH,#$80,WTCONV ; Wait for Sequence Complete Flag
 rts
;-----------------------------------------------------
; This will follow the right or left wall until the floor
; sensor is detected
;-----------------------------------------------------
Follow:
       ldaa stepct              ; Used to go fwd every 3rd step
       cmpa #$03                ; If A is not equal to 3
       bne Follow_1             ; Then jump to follow1
       movb #$00, stepct        ; Otherwise, reset stepct
       bra Follow2              ; And jumpt to follow2 (go 1 step fwd)
Follow_1:
       inca                     ; Increment A
       staa stepct

       jsr Read
       ldaa ADR0H               ;floor sensor
       cmpa #$80
       bhi no_line
       jmp left_motor
no_line:
       ldaa ADR3H               ; Get value of front sensor (1 = 3 temp)
       cmpa #$50 ; was 70        ; Compare to close threshold
       blo  Follow1a            ; If we didnt' "hit" a front wall, branch
       jsr  right_90
       bra  Follow
Follow1a:
       ldaa follow_wall         ; 0 for right wall, 1 for left
       beq  wall_right          ; If zero, follow right wall
       ldab ADR2H               ; Otherwise, get left sensor value
       cmpb s2_far_thresh       ; Compare to the far threshold
       bhi  not_too_far         ; If we're higher than far thresh, we're ok so far
       bra foll_1bC
not_too_far:                    ; Not too far from left wall
       cmpb s2_close_thresh     ; Compare to the left close threshold
       blo  Follow2             ; If lower than close thresh, just go fwd
       bra wall_rtC
wall_right:                     ; If here, we're following right wall
       ldab ADR6H               ; Get front right sensor value
       cmpb s6_far_thresh       ; Compare it with far thresh
       bhi Follow1b             ; If higher, we're ok
wall_rtC: jsr left_motor        ; Otherwise, turn in one step
       bra Follow
Follow1b:
       cmpb s6_close_thresh     ; Compare sensor value to close threshold
       blo Follow2              ; If higher, we're aligned and can go fwd
foll_1bC:
       jsr right_motor          ; Otherwise, we need to turn away
       bra Follow
Follow2:
       jsr fwd_motors           ; Set up motor directions for both fwd
       jsr GO                   ; Go forward 1 step
       bra Follow               ; Keep following
;----------------------------------------------------------------------
; This will set the directions of the motors to pivot left
;----------------------------------------------------------------------
piv_left_motors:
 movb #$58, rtmtr
 movb #$54, ltmtr
 rts
;----------------------------------------------------------------------
; This will set the directions of the motors to pivot right
;----------------------------------------------------------------------
piv_right_motors:
 movb #$54, rtmtr
 movb #$58, ltmtr
 rts
;----------------------------------------------------------------------
; This will set the directions of both motors to fwd
;----------------------------------------------------------------------
fwd_motors:
 ldaa #$58
 staa rtmtr
 staa ltmtr
 rts
;----------------------------------------------------------------------
; This will set the directions of both motors to bk
;----------------------------------------------------------------------
bk_motors:
 ldaa #$54
 staa rtmtr
 staa ltmtr
 rts
;----------------------------------------------------------------------
; This will move just the right motor forward.  Used in turning
;----------------------------------------------------------------------
right_motor:
 movb #$58, rtmtr
 movb #$a7, ltmtr
 bra lmgo
;----------------------------------------------------------------------
; This will move just the left motor forward.  Used in turning
;----------------------------------------------------------------------
left_motor:
 movb #$58, ltmtr
 movb #$a7, rtmtr
lmgo:   jmp GO
 ;rts
;----------------------------------------------------------------------
; This will turn the car 90 degrees left or right
;----------------------------------------------------------------------
left_90:      jsr piv_left_motors
       ;movb #$58, rtmtr
       ;movb #$54, ltmtr
       bra  all_set
right_90:     jsr piv_right_motors
       ;movb #$54, rtmtr
       ;movb #$58, ltmtr
all_set:      ldy #$0081        ; Load Y with # of steps in 90 deg (NUM_STEP90)
loop_90:      jsr GO
       dey
       bne loop_90
       rts
;----------------------------------------------------------------------
; Goes forward or backward a bit depending on the value passed in Y
;----------------------------------------------------------------------
;bakabit: jsr bk_motors
;         bra bakabl
fwdabit: jsr fwd_motors
bakabl:  jsr GO
  dey
  bne bakabl
  rts
;----------------------------------------------------------------------
;Room Routine - This is what the robot does when it enters the room
;----------------------------------------------------------------------
RoomL:  jsr right_motor
 bra into
RoomR:  jsr left_motor
 bra into
Room:   jsr fwd_motors
into:   movb #$00, PWEN         ; Disable PWM (turn off IR LEDs)

rm1:     ;jsr go_in               ; Go into the room
;------------------------------------------------------------------
; go_in will move the robot into the room from the doorway
; It will exit out when it goes 12 inches or detects the candle
; circle on the floor
;------------------------------------------------------------------
go_in:
 ldaa rtmtr
 staa old_rt             ; Preserve the original rt mot direction
go_o_line:
 ldaa old_rt
 cmpa #$58
 bne go_o_lineL
 jsr right_motor
 bra go_o_line1
go_o_lineL:
 jsr left_motor
go_o_line1:
 jsr GO
 jsr fwd_motors
 jsr GO
 jsr Read                ; Read sensors
 ldaa ADR0H              ; Get floor sensor
 cmpa #$80               ; Compare with the floor threshold
 blo go_o_line           ; Keep going fwd until line disappears
 ldaa extinguished       ; Not used (yet)
 beq look
 jmp leave
 

look:      ldy #$00DA       ; Should be 12 inches
;           jsr fwd_motors  ; Set motor direction to fwd
go_in_al:  jsr GO           ; Go fwd 1 step
    jsr Read                ; Read sensors
    ldaa ADR0H              ; Get floor sensor
    cmpa #$80               ; Compare with line
    blo go_bail             ; If line found, we need to bail
    dey
    bne go_in_al
    bra turn
go_bail:
 ldaa #$FF
 staa in_circle
 jsr  GO
 dey
 cpy #$0050
 bhi go_bail
 

 ;ldy  #$006C
 ;jsr fwdabit
 ;rts

turn:   movb #$FF, low_value    ; Init strongest candle reading (lowest #)
 jsr piv_left_motors     ; Set up motor directions for a left pivot
 ldy  #$01F6             ; Y gets number of steps in a 360 deg turn
T360:   jsr  Read        ; Read sensor values
 ldaa ADR7H              ; Get candle sensor value
 cmpa low_value          ; compare with the low_value
 bhi  not_stronger       ; If higher, skip
 staa low_value          ; Otherwise, store as new low value
 sty  low_step           ; Store Y into the low_step count var
not_stronger:
 jsr  GO                 ; Do one step CCW
 dey                     ; Decrement step counter
 bne  T360               ; If we haven't done a 360, keep going
  ; If here, we've done a whole 360
 ldaa low_value          ; Otherwise, get the lowest value
 cmpa #$55               ; Compare to candle threshold
 bhi  leave              ; If higher, then candle not in room
 jsr piv_right_motors    ; Set up motor directions for a right pivot
 ldy  low_step
 ldab #$0C
 aby                     ; Add $0C to Y to "fix" turn
turn_back:
 jsr  GO
 dey
 bne turn_back
 jsr  fwd_motors
 ldx #$0000
FoundIt:
  ldaa in_circle
  bne fan_on
  inx
  jsr GO                 ; Step forward one step
  jsr Read               ; Read sensors
  ldaa ADR0H             ; Check floor sensor
  cmpa #$80              ; Compare with threshold
  bhi FoundIt            ; If higher, need to keep going fwd
fan_on:  ldaa PORTA      ; If we're here, we hit candle circle
  anda #%11110111        ; Make fan turn on (reverse logic)
  staa PORTA
  jsr wait_toggle        ; Keep fan on for ~ 1/2 sec
  ldaa PORTA
  oraa #%00001000
  staa PORTA             ; Turn fan back off
  staa  extinguished     ; a != 0
  jsr bk_motors
  inx
back:    jsr GO
  dex
  bne back
  jsr piv_left_motors
  ldy low_step
straight:
  jsr GO
  dey
  bne straight

  ;jmp quit
leave:
 movb #$03, PWEN         ; Turn IR LEDs on before backing to wall
 ;jmp Back_to_wall
 ;rts
;----------------------------------------------------------------------
;backs until it sees a wall
;----------------------------------------------------------------------
Back_to_wall:
  jsr bk_motors          ; Set up motor directions for backward
no_wall: jsr GO
  jsr Read
  ldaa ADR4H
  cmpa #$70
  blo no_wall
  rts
;----------------------------------------------------------------------
; This routine moves both motors one step
; The current state of the motor does not matter
;----------------------------------------------------------------------
GO:     clc                      ; clear carry bit
 ldab PORTB                      ; read the current motor byte
 andb #%11110000                 ; Mask off right stepper bits
 jsr rtmtr                       ; carry out shift
 bcc NEXT1                       ; If we didn't carry, skip to NEXT1
 orab #%00010000                 ; If we carried, set LSB of this nibble
NEXT1:  bitb #%00001000          ; check if we overflowed right
 beq NEXT3                       ; if not, branch
 eorb #%10001000                 ; if so, fix it
NEXT3:  stab temp                ; store right motor nibble in temp
 clc                             ; clear carry
 ldab PORTB                      ; read the current motor byte
 andb #%00001111                 ; Mask off left stepper bits
 jsr ltmtr                       ; carry out shift
 bcc NEXT4                       ; if didn't carry, skip to NEXT4
 orab #%00001000                 ; if carried, fix it
NEXT4:  bitb #%00010000          ; check if overflowed left
 beq NEXT2                       ; if not, branch
 eorb #%00010001                 ; if so, fix it
NEXT2:  orab temp                ; combine the two motor nibbles
 stab PORTB                      ; send out to motor driver
 jsr DELAY
 rts

;----------------------------------------------------------------------
; This delay routine is used for delaying between steps
;----------------------------------------------------------------------
DELAY:  xgdx             ;(exchange X and D to save X) stx old_x
 ldx #$1200              ; Load X with speed var (used for delaying)
dloop:  dex              ; Decrement X
 cpx #$0000              ; If X != 0
 bne dloop               ; Loop again
 xgdx    ;(exchange X and D to save X) ldx old_x  ; Fake stack
eof:    rts