Pathfinder Mark VII 2000 Edition 3 point...
71
Final Report for the Pathfinder Mark VII 2000 Edition 3 point o TM CPSC 483 Fall 2000 Patrick Barnes Travis Collavo Adam Norgaard
Transcript of Pathfinder Mark VII 2000 Edition 3 point...
Final Report for the
Pathfinder Mark VII 2000 Edition 3 point oTM
CPSC 483Fall 2000
Patrick BarnesTravis CollavoAdam Norgaard
Advisor: Dr. Mahapatra
Table of contents
Acknowledgements 3
Abstract 4
Introduction 5
Individual Achievements 6
Software 10
Hardware 15
Results and Discussions 23
Appendix A: Code 24
Appendix B: Pictures 49
2
Acknowledgements
First we would like to thank Dr. Rabi Mahapatra for his help in getting us started on this project, and for taking the time to sit down and discuss with us the answers to some of our most difficult questions. Next we would like to thank our most excellent TA, Joseph, and our surrogate TA, Jonathan. Joseph performed his part cataloguing diligently, and helped to curb our urge to steal the most expensive equipment in the back closet. Jonathan, the live-in TA in the 483 lab was always there to help out in figuring out both electrical issues and algorithm issues. Without the help of all the above people, our project would have never gotten off the ground. Of course we would like to give a shout out to the family and friends, whose emotional and/or financial support allowed us to get out of the house and get an education. God bless us all, each and every one.
3
Abstract
Our objective is to build a car that can navigate autonomously and intelligently in its surroundings. A user will input a final destination into the robot relative to its current position. It will then be able to traverse the distance to the final destination while avoiding all obstacles in its path. Further improvements may include the ability of the robot to find an optimal path to its final destination.
The car will be constructed primarily of Lego's, and the operation of the car will be controlled by a micro-controller (more specifically the Rice RoboBoard). The user input will be taken in via a keypad, and feedback will be given to the user via a LCD screen. A range finder will be used to gather data points (distances to surrounding walls) for the car to map out its surroundings. It is with this map that the Pathfinder will navigate its environment.
4
I. Introduction
We are currently creating a mobile robot (“Pathfinder”) that is more aware of the geometry of its surroundings than the more simplistic robots existing currently. Current mobile maze navigating robots are generally quite simplistic in design, as most of them have to run into a wall before knowing of the wall’s existence, or be given a preprogrammed route to follow.
Our Pathfinder robot will be able to map out its environment as it moves, and use this map to find a route through its environment. Thus our advanced robot will not be limited to maze environments that have certain assumed geometries, such as those only having ninety-degree angles in corners or obstacles of certain shapes. Instead, our robot will work in most any maze type environment, such as the CPSC 483 lab with a few added obstacles to make things a little more interesting.
Using an ultrasonic range finder, the robot will be able to autonomously create an internal list of ranges (distances) to surrounding obstacles from its current location. The range finder unit will be located on top of the Pathfinder and will be mounted on servos so that it can rotate to find ranges in different directions. The procedure by which a list of ranges will be obtained is outlined in the following:
Mapping Procedure1. Activate range finder2. Store value output from range finder into memory3. Rotate range finder by 3.64. Repeat steps 1 to 3 until 100 times to sweep out 360
When the mapping procedure is complete, the Pathfinder will have compiled a list of ranges (distances) which are then used to create a map of its surroundings. The mapping procedure may be repeated several times to refine the resolution on the Pathfinder’s internal map of its surroundings.
After the map is created, the Pathfinder must determine where to go next. Based on its internal map, it will attempt to find a path around any obstacles that are preventing it from moving directly to its final destination. In order to find this path, we will employ an algorithm of our choosing that chooses an intelligent path (ideally the shortest path). This algorithm may be complicated, as the shortest path will involve travelling near obstacles, yet it will need to keep some distance from the obstacles to avoid running into them.
Regardless, we must account for the fact that the map of the surroundings will be imprecise due to imprecise range finder measurements, imprecise alignment/bearing, and imprecise movement. The algorithm will help prevent the robot from approaching any detected surface too closely.
5
II. Individual Achievements
Our design and implementation methodology so far has been fairly simple. We have attempted to follow the initial timeline as closely as possible, but we have encountered several obstacles, which will be discussed later. Tasks were evenly divided each day, with each person working towards a separate goal. In order to allow two of us to be working with RoboBoard related tasks simultaneously, we have acquired another computer, and another corresponding RoboBoard and serial interface cable. We have collaborated on some of the more difficult tasks, and have kept each other up to date on work we have done individually. We have recorded accomplishments for our biweekly reports, and this data has been compiled in the following list:
Travis’ Contributions 9/18/2000 - 10/02/2000
-Created about 33% of the proposal.-Checked out the Rice RoboBoard manuals.-Gave approximately 40% of the presentation.-Checked out the RoboBoard.-Purchased a key component for the project.-Researched the capabilities of the RoboBoard.-Researched the Range finder.
10/02/2000 – 10/18/00-Created 2 serial interface cables-Figured out how to interface stepper motors with RoboBoard-Partially figured out how to control motors-Found correct Pcode to download to RoboBoard-Got all library functions working with RoboBoard-Assisted in figuring out how to handle interrupts on the RoboBoard-Obtained new parts needed for Pathfinder construction-Created nifty theme song for our project
10/19/00 - 10/24/00-Got servos and servo control functions working with RoboBoard-Created program to test motor/chassis interaction
10/24/00-11/08/00-Created basic function for range reading-Interfaced range finder with the RoboBoard-Created servo control functions-Created function for manipulating servo via a knob on the RoboBoard
11/08/00 – 11/22/00
6
-Rewrote (improved) stepper motor function-Finished function to return environment map array from scan-Found a workaround for array size restrictions in Interactive C-Rewrote some motor testing code-Created function to test range finder-Found the cause of some of the board failures-Figured out how to use the escape button on the RoboBoard.
11/22/00 – 12/03/00-Rewrote range finder function to improve accuracy-Modified scan function to center range finder at preset position-Created external 12V to 5V power supply for range finder-Tested range finder to discover that external power supply was needed-Corrected errors with getarray() function-Tested range finder in premade environment-Discovered conversion factor to convert range finder values to inches
Adam’s Contributions 9/18/2000 - 10/02/2000
-Created about 33% of the proposal.-Assisted with the creation of the PowerPoint presentation.-Gave about 35% of the presentation.-Found Rice’s web page pertaining to the RoboBoard.-Assisted with the testing of the RoboBoard and the IC software.-Created the parts list that was submitted to the TAs.-Created both versions of the project sign, even the color one.-Researched the Range finder.
10/02/2000 – 10/18/00-Obtained Parts for IR sensing-Obtained bumper sensors to be used with robot.-Constructed chassis for robot.-Constructed and tested steering mechanism. -Constructed holding bay for the RoboBoard-Incorporated batteries and their charger use with the RoboBoard.-Constructed gear box to step down the motor for driving the main wheels.-Constructed main axle differential to reduce torque.-Also, I am the group photographer.
10/19/00 - 10/24/00-Increased gearbox efficiency-Reduced gearbox noise-Incorporated additional motor to drive gearbox-Added battery holder to chassis
7
10/24/00-11/08/00-Assisted in the implementation of the range finder. (connecting wires…)-Mounted the stepper motor to the Legos-Mounted the shaft encoder to the stepper motor.-Mounted the Range finder to the stepper motor.-Mounted the Range Finder controller board to the Legos.-Assisted in getting the compass to work.-Started documenting the input and output pins for accountability.-Redesigned the rear axle support to prevent bending.-Began gluing Legos at key locations to prevent catastrophic collapse.
11/08/00 – 11/22/00-Mounted shaft encoder on gear box to assist in determining distance traveled by car.-Remounted shaft encoder to increase the accuracy of distance traveled.-Mounted Keypad to Robot.-Mounted the compass on movable extendo-rods attached to battery basket.-Reduced the noise level generated by motors. (Somewhat)-Fixed the breakage of the range finder wires. (The range finder movement would weaken the wires.)-Began testing the gear box shaft encoder accuracy and results.-Assisted in determining the conversion of gear box shaft to feet. -Reduced the number of input and output pins which external devices are using.-Decoded the output from the range finder into a radar graph to produce understandable output.
11/22/00 – 12/03/00-Assisted in the creation of the Voltage regulator-Assisted in testing the calibration of the movement and steering functions.-Assisted in testing the range finder.-Fixed the Pathfinder whenever it broke.-Reinforced rear axle to prevent bending by supporting axle on other side of the
wheels.-Mounted Voltage regulator.-Assisted in PJ’s creation of the Movement algorithm.
PJ's contributions9/18/2000 - 10/02/2000
- Created most of the PowerPoint slides for the presentation.- Presented roughly 25% of the presentation.- Helped the team find equipment already available in the lab that would be useful
for our project.- Helped the team locate parts that need to be ordered (different range finders,
electric compasses, etc.).
8
- Helped the team research how other people have communicated with the RoboBoard via hardware (serial adapters) and software (Interactive C).
- Helped the team troubleshoot communicating with the RoboBoard (eliminating possible causes of error).
- Moral support, dinner suggestions, and other general team support.
10/02/2000 – 10/18/00-Figured out how to interface with the digital input and output pins-Keypad interface and functions-Began figuring out how to handle interrupts on the RoboBoard-Began figuring out how to compile assembly programs-Helped with making the serial interface cables-Helped figure out how to interface stepper motors-Created group website
10/19/00 - 10/24/00-Got basic interrupts working-Began working on executing a C function from with an assembly function
10/24/00-11/08/00-Researched how compass works by reading included specifications-Implemented compass with RoboBoard via breadboard, and tested-Optimized compass implementation (still in progress)-Experimented with assembly (timing of instructions, accumulator usage, etc.)-Wrote custom assembly program to help with range finder operation (measuring
time)
11/08/00 – 11/22/00-Updated custom assembly program to make range finder operation accurate-Brainstormed pathfinding algorithms-Brainstormed how to interpret range finder data-Brainstormed how to implement memory-Assisted in algorithm for saving range finder sweep data-Assisted in measuring shaft-encoder-to-wheel ratio-Began writing routines for converting coordinates from polar to Cartesian-Helped discover that angles of deflection affect range finder readings
11/22/00 - 12/03/00-Modified car movement functions to accelerate and decelerate-Recalibrated movement functions (converting shaft-encoder value to distance)-Recalibrated the servo turning the wheels-Calculated the turn radius-Created the Movement algorithm to account for car's non-zero turn radius-Implemented Movement algorithm to move car to another location directly-Organized the memory map of our system-Contributed to the final report and presentation
9
III. Software
A large volume of software must be used to interact with the RoboBoard. This includes several pre-written library files and a number of modified or custom made files. This section will enumerate and explain all the programs used in the creation of the Pathfinder. A full listing of the code from each program may be found in the appendix. All files ending with a .icb extension are unreadable as-is, but have a corresponding .asm file which is readable (.asm files are compiled into .icb files).
Descriptions
The main files used in interacting with the RoboBoard are as follows:
Analog.icbThis file simply contains a list of constants representing the analog related ports on the RoboBoard.
Car.cThis file contains many functions that perform basic car operations, such as turning the wheels, moving forward or backward, or moving along a certain path.
Cmotor.cThis file is used to test the motors and servo. When the program is executed, it goes into an infinite loop, turning motors on ports 5 and 6 whenever the choose button is pressed on the RoboBoard. Servo control is also initiated when the choose button is pressed, and the position of the servo may be set by rotating a potentiometer on the RoboBoard. All the motors and the servo may be toggled on and off with the choose button.
Compass.cThis file contains the functions necessary for initializing and operating the electronic compass, which involves clocking the compass through software and reading in bearing values.
Encoders.cThis pre-written library file contains high level functions for interaction with a shaft encoder. Shaft encoder operation is explained in the Hardware section.
Encoders.icbLow level assembly functions for controlling a shaft encoder are contained within this file. Interrupts are used to increment a counter whenever the shaft encoder circuit returns a high signal.
Getarray3.cThis file uses the peekword function 50 times to return an array of 50 integers that are located in memory starting at a user-definable offset. These integers are returned to the IC prompt and are readable by the user.
Keypad.cThis file contains functions to operate the keypad. A general function gets a single keypad press from the user; other higher-level functions retrieve multi-digit numbers
10
and even multiple parameters (such as inches and feet). It involves asserting column pins and checking for connections with row pins.
Main.cThis c file is currently unused. It will house the main program which will call
Math.cThis file contains some math functions that didn’t come with the Interactive version of C (such as fabs()).
Range finder2.cThis file contains 2 functions, read_range and range_finder. The read_range function de-asserts the init input of the Polaroid 6500 Series
Sonar Ranging Module (SRM), and then calls the rfecho function (see rfecho details). The init input is then de-asserted again to reset the SRM.
The range_finder function takes 3 readings using the read_range function. If any of these readings are suspiciously high, the 3 readings are retaken up to 20 times, until a more reasonable value is obtained. When 3 reasonable values are obtained, the value that is most different from the other 2 values is discarded, and the remaining 2 values are averaged. This average is returned. This throwing out of values helps to eliminate erroneous range finder readings.
Rfecho.icbThis file contains the compiled object code for handling a time-critical routine used by the range finder.
Robogeo.cThis file contains functions to find solutions for moving the car to an immediate sub-destination, based on the current position, turn radius, and final sub-destination. For example, if the car is to move about yea far ahead and to the right, then it turns its wheels to the right, moves in an arc for a short distance until it is facing its sub-destination, then straightens its wheels and moves forward. See figures 9.4g through 10.88z for illustration.
Robolib.cThis file contains basic, frequently used functions. Most of these functions contain high level calls to assembly level functions located in the Pcode (basic RoboBoard operating system stored in an EEPROM). These functions include the following: Peek, peekword, poke, and pokeword – used to look at a value in the memory of
the RoboBoard (peek), or to place a value in memory (poke) Seconds, mseconds – returns the elapsed system time since the last
reset_system_time() execution Sleep, msleep – causes program execution to pause for a set amount of time
(mseconds() takes milliseconds as an argument, seconds() takes seconds. Beep, tone, beeper_on, beeper_off, set_beeper_pitch – used to make sounds on
the RoboBoard speaker. Forward, reverse, brake, off, alloff, ao, motor, _set_motor – used to control the
motor ports of the RoboBoard. Led_out0, led_out1 – used to toggle LED’s on the RoboBoard on or off. Choose_button, escape_button, dip_switches, dip_switch – used to read the values
of various buttons and switches on the RoboBoard.
11
Analog, digital, digital_out – used to read and write values to the digital and analog inputs and outputs on the RoboBoard.
Motor_force – returns a number representing the force being exerted by the motors
Robo_knob – returns a value based on the position of a potentiometer on the board.
Voltage – returns the voltage value of the power supply of the board (in the 12V range)
Hog_processor – causes a process to gain exclusive use of the processor system_pwm_on, system_pwm_off, system_print_on, system_print_off – turns on
and off various abilities of the RoboBoard (such as the ability to print to the LCD screen on the board) in order to reduce unnecessary CPU usage.
Servo.cContains simple, high-level functions that call lower level servo-related assembly functions, and that can convert degrees to radians and vice-versa.
Servo.icbUses the internal timer of the Motorola 68HC11 microprocessor to create clock pulses of varying length that determine the position of the servo.
Stepscanmem3.cThis function is used to make a 360° scan of the room from the Pathfinder’s current position and place 100 values representing range finder readings taken at 3.6° increments. A stepper motor is controlled from this function, and set to move in the necessary 3.6° increments clockwise, and then to activate the range finder on each increment and store the values in consecutive memory addresses (range finder details found in range finder2.c description). The getarray function is used to retrieve the values stored in memory.
Theme.cOutputs a theme song from the speaker to verify that the RoboBoard is functioning.
12
Software Diagrams
Software Problems
Range finder related problemsOne very vexing problem that we encountered was how to deal with erroneous range finder readings. The range finder readings can very by about 10% or more between readings from the same object. Through thorough testing, we found that generally, one out of every three range finder readings would be significantly different from the other two surrounding readings (on average of about 10% different, while the two other readings would be almost exactly the same). Thus the range_finder function was modified to throw out the reading which was the most different from the other two readings in a series of three consecutive readings. This helped to eliminate the occurrence of erroneous readings to some degree
Another property of the range finder which makes it difficult to work with from a software perspective is the fact that the reading returned varies with the relative angle of the object that the range finder is pointing at. For example, if you take a reading of the distance to a piece of cardboard, and then rotate the piece and take another reading without changing the distance of the piece to the range finder, the reading can as much as double, depending on the angle at which the cardboard is held. We have been able to filter out readings that were taken of objects that were
13
Figure 1: Geometric theory behind pathfinding algorithm
at a high angle of deflection relative to the range finder by filtering out range finder readings which were suspiciously high. Unfortunately, this only helps with this problem in a small number of cases, and further error correction must be taken as we finalize the pathfinding algorithms.
Data type related problemsInteractive C cannot handle arrays of more than about 72 integers, which was a major problem we had to circumvent. In order to get around this problem, we just decided on a set memory address on the RoboBoard in which to store the array in question, and used pokeword to store values in consecutive memory locations starting at the chosen memory address. Peekword could then be used to retrieve these values at a later time.
Possible Software ImprovementsThere are several possible improvements that could be made to our software. The following list outlines several of these potential improvements:
Incorporate multitaskingMultitasking is supported by the RoboBoard, but we are currently not utilizing it. Some of our code may operate more efficiently if multitasking were used.
Increase modularityThe Stepscanmem3.c function is not very modular, and thus may be difficult to understand by other users (it could be made more modular). In addition, the cmotor.c file should separate the servo testing and the motor testing sections of code more clearly.
Write our own Pcode fileMany low-level functions that we use are listed in unreadable machine code in the file “pcode.s19.” It would be helpful to rewrite the Pcode file to remove functions we don’t use and to modify functions in ways that would make high level RoboBoard programming easier.
Improve range finder error correctionThe read_range function could possibly be further modified to include more error-correction capabilities.
Improve pathfinding algorithms The robogeo.c file could be further modified to improve the pathfinding ability of the Pathfinder.
14
15
IV. Hardware
Hardware DescriptionsINTROPolaroid Ultra-Sonic 6500 Ranging Module
The range finder is comprised of a controller board and a transducer. To function, the transducer creates an ultra-sonic sound pulse which then progresses through the air till it collides with the nearest object in its path; the reflected pulse is then detected by the transducer. The time elapsed (counted by the RoboBoard) from the production of the signal and its return can then be converted to distance traveled; the distance and time are directly proportional. To initiate a pulse we assert the range finder’s input, INIT. We then measure the time elapsed till the range finder’s output, ECHO, is asserted; this indicates the signal has been reflected and detected by the transducer.Tandon KP4M4-001 Stepper Motor
The ultra-sonic range finder is attached to a stepper motor, which has a movement range of 360+ degrees in 3.6 degree increments. The implementation of the stepper motor with the Pathfinder has the stepper motor rotating from 0 to 360 degrees, all the while taking readings, and then return to 0 degrees via its former path. This keeps the transducer wires from bending too much or even breaking. The stepper motor has 5 wires. Since we did not have any significant documentation on the stepper motor’s operation, we had to determine, for ourselves, exactly how these wires were used. By examining the pre-written library file for the stepper motor, and observing how changes to this file affected the operation of the stepper motor, we were able to discern the correct way to connect the stepper motor to the motor ports on the RoboBoard. The black wire is a ground wire. Labeling the other wires as follows, white=1, green=2, brown=3, red=4, the wires are asserted to create the following signal:
16
Stepper Motor Control SignalsWire 1
Wire 2
Wire 3
Wire 4
IR Shaft EncoderA shaft encoder has the ability to determine and tally the movement of any shaft. The shaft encoder is used to calibrate the stepper motor and the range finder to a predetermined position. The reason for this is because the stepper motor has no way of knowing its absolute position. It can only ascertain its relative position from its previous position. With the inclusion of the shaft encoder we will ensure that the range finder will always initiate its scan at the proper location
Rice RoboBoard V 4.0This is wonderfully versatile micro-controller that is specifically designed for the operation of custom made robots. It has many features: 32K ram, 8 digital inputs, 7 digital outputs, 8 analog inputs, 6 motor ports, 1 servo port, LCD output screen, 3 push buttons, 4 dip switches, 1 potentiometer. This board is also used to attach and control numerous other devices.
Vector™ CompassThis is an electronic compass that is able to report the angle of the device to within two degrees accuracy. It is used to orient the Pathfinder to its goal. The compass can work in several modes, such as master vs. slave (to dictate how clocking is done), binary versus BCD (to dictate how bearing information is outputted, and a special calibration mode.
Steering servoThis servo is used to control the Pathfinder’s steering mechanism. The servo has a range of about 180 degrees, and is accurate to every degree. It also has a very high torque.The steering servo connection is quite simple. The red wire connects to VCC, black wire to GND, and the white wire to the timer output of the RoboBoard (Port 30).
MotorsThe motors are connected to specially designed motor ports on the RoboBoard. These ports are connected through H-bridges that allow a large amount of current to be drawn by the motors without causing power problems with the RoboBoard. The voltage provided to each motor is pulse width modulated (turned on and off rapidly) by the motor functions in order to prevent the motors from constantly accelerating while active. The rate of modulation determines the velocity of the motor shaft.The motors operate at a very high speed, but have almost no torque. To rectify this predicament we decided to incorporate a gear box.
Gear Box
17
Due to the nature of the motors, there was not enough torque to move the Pathfinder. So, our solution to this problem was to step the motors down 225 times to reduce the speed and increase the torque. See diagram below.
Shaft EncoderLater in the implementation of the Pathfinder we decided that we needed a better way of determining the distance traveled by the Pathfinder. The old way, timing the motor-on duration, was not accurate enough. To fix this, we incorporated a shaft encoder on the gear box, specifically on the third axle from the motors. This allowed for a resolution of 3.1 ticks per inch. For example, if we wanted the path finder to go 24 inches we would program the motors to run until 74 ticks were counted. This has proven to be very accurate. As an alternative we had thought to use the shaft encoder on the rear axle, but this proved to be futile due to the differential used on the rear axle. If we had used it in this manor we would have had different values for the distance traveled depending on how much it was turning.
18
Hardware Diagrams
19
Hardware Layout
Robot Car
Micro controllerRoboBoard – 68HC11
Memory
Range finder mount
Map of surroundings
Path finding algorithm-interpret data from sensors/rangefinders/compass -determine path to take-determine how-far/direction to move -update old map data with new data-account for inaccuracies from range finder-account for imprecise movements-find shortest path (optional)
Movement Hardware
Servo for steering
Wheels
Wheel Motors
I/O
Keypad
Speaker Serial Port
Stepper motor
Sonar range finder
Electronic CompassBumper sensor
Human/Computer Interface
Output pertinent data
Input movement commands
Output
Output
Input
Outside world
Shaft Encoder
Gear Box
Shaft Encoder
LCD
Output
Input
InputInput
20
Shaft Encoder
Axle8 spokes24 spokes40 spokes
Gear Box
RearDifferential
Rear Axle
Polaroid 6500 Ranging Module Mounted on Stepper Motor
Hole
DiskWithHole
Transducer
ShaftEncoder
SteppermotorRange finder Scans 0° to 360°
then returns to 0°
21
Steering Mechanism
Wheel
Axle
8 spokes
16 spokes
40 SpokesTracks
Servo
MoveRight
MoveLeft
Key PadConnect to RoboBoard
1 2 3 A
4 5 6 B
7 8 9 C
* 0 # D
Hardware Problems
Noise level of gear boxBecause the motors can only run at an extremely high velocity, it is almost impossible to eliminate all the noise generated by the fast moving parts. However, in the beginning of the motor implementation the noise of the motors was very obtrusive. To fix this problem we tried many things to lessen the noise. The things that seemed to work were: strengthening the Legos of the gear box (eliminate the bending of parts), sanding the Lego gear shaft, and loosening the Lego gears so they were not tightly fastened against the Lego supports.
Spontaneous destruction of steering mechanismThe steering Legos comprised of 4 rotating 2x2 parts which were not very stable. During the testing the Pathfinder’s chassis, the steering mechanism frequently fell apart. To fix this, we sanded and glued every weak point in the entire steering mechanism.
Rear axle bendingWhen the Pathfinder is completely loaded, it weighs about 10 to 15 pounds. This is slightly heavier then the “ideal” case the Legos company had in mind. To fix this, we incorporated supports for the rear axle that surrounded the rear wheels. This provided the necessary support to prevent the rear wheels from bending uncontrollably.
Random resetting of the RoboBoard due to power drains from range finderThe Range finder must have 2 amps of current to initiate the ultra-sonic pulse. This current drain is more than the RoboBoard is able to handle, thus causing the RoboBoard to promptly reset. To fix this, we created a voltage regulator that was powered directly from the external battery.
Wobbly steering mechanismAs it stands now, the steering mechanism is very wobbly. This is due to use of 2x2 rotating pieces. These are inherently not stable Lego pieces. Also, the Lego gears have some give and take that allow for imprecise steering controls.
Possible Hardware ImprovementsImprove the noise level of the gearbox
To decrease the noise generated by the gearbox we had the idea of using a steel rod in the place of the plastic Lego axle. We believe that this would make it much more quite. Another idea that we had was to put oil on the axles that are making the most noise. Since the noise is generated by friction of the plastic pieces contacting each other at high velocities, we believe that lubricating the axle would also reduce the noise level.
Improve general structural integrityAs is the case with all Legos, our Pathfinder tends to fall apart on occasion. To improve the general structural integrity of the robot we
22
could glue every Lego piece. This is not a valid solution due to the fact that people will have to use these Legos in future.
23
V. Results and Discussions
As of writing this final report, our project is partially complete. We have rudimentary path finding capabilities (i.e. enter a final coordinate and go there without avoiding obstacles), but we still need to integrate the current path finding algorithm with the rangefinder. This involves creating the algorithm to analyze the current environment map (which we have functions to create already) and integrate it with an overall environment map. Given that we have 4 days until the project demonstration is due, we feel that we will have enough time to create this algorithm to some degree. We have all the functions created to actually move our car, including the ability to go forward a set number of inches, and to turn the car and go in a well-defined circular path. We also have functions completed that take a complete scan of the local environment and to store this scan in the Roboboard’s memory. We have come a long way, but we still have some work cut out for us.
The difficultly level if this project was fairly high, mainly because we were constantly encountering various problems with the hardware or the software that we had to circumvent before proceeding. If we do not finish our project, we will at least have some good code for interacting with the new hardware that we purchased for our project, and a future team would easily be able to finish what we started and expand upon our original design.
We feel that our project allowed us to increase our experience level both with hardware and software design. Understanding the workings of the Roboboard, the rangfinder hardware, the shaft encoder hardware, the stepper motors, and the custom-made power supply was necessary to get to where we are now with our project. In creating the power supply, and allowing the rangefinder circuitry to work with the Roboboard, we increased our experience level in electrical engineering. In learning how the stepper motors, the compass, and the shaft encoder hardware work, we increased our understanding of how to interface proprietary hardware with a system for which it was not specifically designed to work. The algorithms we created to control the hardware was equally difficult to construct. In order to get the car to move from point A to point B, we had to review a decent volume of trigonometry material and incorporate this into our code. Coding the environmental scanning algorithm also involved some innovation, as the library code we were given to operate the stepper motors created an incorrect waveform for operating these motors, and the correction of this code was fairly challenging. Coding software to control the range finder and compass was difficult, and required hours of troubleshooting and error-compensation to arrive at a satisfactory program. Overall, we feel that this was a challenging project that improved our skills with hardware and software equally.
24
Appendix 1
Code
analog.icb<the machine code section has been cut out of this printing><the symbol section has been included here>ADCTL 1030 *0049 0052 0053 ADR1 1031 *0050 0054 ADR2 1032 *0051 ADR3 1033 *0052 ADR4 1034 *0053 BADOPINT 00f8 *0086 BASE 1000 *0006 0025 0033 BAUD 102b *0044 BPROT 1035 *0054 CFORC 100b *0018 CMEINT 00fc *0088 CONFIG 103f *0064 COPRST 103a *0059 DDRC 1007 *0014 DDRD 1009 *0016 HPRIO 103c *0061 INIT 103d *0062 IRQINT 00f2 *0083 NOCOPINT 00fa *0087 OC1D 100d *0020 OC1M 100c *0019 OPTION 1039 *0058 PACNT 1027 *0040 PACTL 1026 *0039 PAIINT 00da *0071 PAOVINT 00dc *0072 PIOC 1002 *0010 PORTA 1000 *0008 PORTB 1004 *0012 PORTC 1003 *0011 PORTCL 1005 *0013 PORTD 1008 *0015 0041 0044 PORTE 100a *0017 PPROG 103b *0060 RESETINT 00fe *0089 RESV1 1001 *0009 RESV2 1036 *0055 RESV3 1037 *0056 RESV4 1038 *0057 RTIINT 00f0 *0082 SCCR1 102c *0045 SCCR2 102d *0046 SCDR 102f *0048 SCIINT 00d6 *0069 SCSR 102e *0047 SPCR 1028 *0041 SPDR 102a *0043 SPIINT 00d8 *0070 SPSR 1029 *0042 SWIINT 00f6 *0085 TCNT 100e *0023 TCTL1 1020 *0033 TCTL2 1021 *0034 TEST1 103e *0063 TFLG1 1023 *0036 TFLG2 1025 *0038 TI4O5 101e *0031
25
TIC1 1010 *0024 TIC1INT 00ee *0081 TIC2 1012 *0025 TIC2INT 00ec *0080 TIC3 1014 *0026 TIC3INT 00ea *0079 TMSK1 1022 *0035 TMSK2 1024 *0037 TOC1 1016 *0027 TOC1INT 00e8 *0078 TOC2 1018 *0028 TOC2INT 00e6 *0077 TOC3 101a *0029 TOC3INT 00e4 *0076 TOC4 101c *0030 TOC4INT 00e2 *0075 TOC5INT 00e0 *0074 TOINT 00de *0073 XIRQINT 00f4 *0084 read_analog_direct 874c *0051 0028 subroutine_analog_header_port 872b *0018 subroutine_read_analog 8733 *0029
car.c/* move forward "inches" inches */void car_forward(long inches) {
int d, dfinal, direction; /* -1=backward, 1=forward */long reading; /*shaft encoder reading */dfinal = (int) inches * 10 / 3;if (dfinal<0) {
direction = -1;dfinal = -dfinal;
}else
direction = 1;if (dfinal>10) dfinal = dfinal - 5;enable_encoder(1);reset_encoder(1);motor(4,6*direction);motor(5,6*direction);while ((d < dfinal) && (!choose_button())) {
d = read_encoder(1);printf("%d ", d);printf("(%din)\n", d*3/10);
}motor(4,0);motor(5,0);disable_encoder(1);
}
/* move car forward "ticks" shaft encoder ticks at "speed" speed */void car_forward_ticks(long ticks, int speed) {
int direction;long d, dfinal, reading;dfinal = ticks;if (dfinal<0L) {
direction = -1;dfinal = -dfinal;
}else
direction = 1;enable_encoder(1);reset_encoder(1);motor(4,speed*direction);motor(5,speed*direction);while ((d < dfinal) && (!choose_button())) {
d = (long) read_encoder(1);printf("%d ", d);printf("(%din)\n", ((int)d)*3/10);
26
}motor(4,0);motor(5,0);disable_encoder(1);
}
/* enhanced forward function; it accelerates and deccelerates to minimize jerk */void car_fsmart() {
int direction, speed;long d=0L, dfinal, dhalf, reading, ticks;
servo_on(); car_turn_straight(); while (!choose_button()) { printf("dlast=%d ticks?", d); d=0L; ticks = getintfromkeypad(); /* input distance */
dfinal = ticks;dhalf = (long) ((int) dfinal / 2);if (dfinal<0L) {
direction = -1;dfinal = -dfinal;
}else
direction = 1;enable_encoder(1);reset_encoder(1);
while ((d < dfinal) && (!choose_button())) {/* decide speed */if (d < dhalf) { /* accelerate */
speed = (int)d/10+2;if (speed>8) speed=8;
} else { /* deccelerate */speed = (int)(dfinal-d)/10+2;if (speed>8) speed=8;
}motor(4, speed*direction);motor(5, speed*direction);d = (long) read_encoder(1);printf("%d ", d);printf("(%din)\n", ((int)d)*3/10);
}motor(4,0);motor(5,0);disable_encoder(1);
} servo_off();}
void car_turn(float value) {servo_deg(value);msleep(500L);
}
void car_turn_right() {servo_pulse_wavetime=600; /* calibrated by hand */msleep(500L);
}
void car_turn_left() {servo_pulse_wavetime=4010; /* calibrated by hand */msleep(500L);
}
void car_turn_straight() {servo_pulse_wavetime=2350; /* calibrated by hand */msleep(500L);
}
27
/* move circle in left arc until compass indicates a full circle */void car_circle_left() { int b0, bf, d, done=0; while (!choose_button()) {
while (!choose_button()) {}servo_on();servo_deg(180.0);msleep(1500L);b0 = compass_read();enable_encoder(1);reset_encoder(1);motor(4,6);motor(5,6);while(done<2) {
if (choose_button())done=2;
bf = compass_read();/* make sure it turns at least 8 degrees */if (done==0) {
if ((bf-b0>7) || (bf-b0<-7))done=1;
}/* stop when init and final bearings are within 4 degrees of each other */if (done==1) {
if ((bf-b0<5) && (bf-b0>-5))done=2;
}printf("bearing=%d", bf);
}d = read_encoder(1);motor(4,0);motor(5,0);printf("\n");printf("i%d ", b0);printf("f%d ", bf);printf("d%d\n", d);servo_off();
}}
/* some specific route */void car_route2() { int d, done=0; servo_on(); while (!choose_button()) {
while (!choose_button()) {}car_turn(180.0);car_forward(18L);car_turn(0.0);car_forward(36L);car_turn(180.0);car_forward(18L);car_turn(90.0);car_forward(24L);
} servo_off();}
/* input x,y from user on keypad, and move car there! */void gouserxy() {
long x, y;int dir;float d1, d2;char dirchar[4] = "RSL";
while (!choose_button()) {printf("\n");x = keypadgetinches();printf("\n");y = keypadgetinches();printf("\n");
28
printf("x=%d ", x);printf("y=%d\n", y);
while (!choose_button()); while (choose_button());
/* run geosolve to find the best direct path to x,y */geosolve((float) x, (float) y, &dir, &d1, &d2);
/* display solution */printf("%c", dirchar[dir+1]);printf("%f ", d1);printf("S%f\n", d2);
while (!choose_button()); while (choose_button());
servo_on();if (dir == -1) car_turn_right();if (dir == 1) car_turn_left();if (dir != 0) {
car_forward((long) d1);car_turn_straight();
}car_forward((long) d2);servo_off();
}}
/* turn car r(ight), go distance, then go straight for one foot *//* used to calibrate right turning and right turn radius */void carr(long radiusinches) { int d, done=0, value; servo_on(); while (!choose_button()) {
while (!choose_button()) {}car_turn_right();car_forward(radiusinches);car_turn_straight();car_forward(12L);
} servo_off();}
/* turn car l(eft), go distance, then go straight for one foot *//* used to calibrate left turning and left turn radius */void carl(long radiusinches) { int d, done=0, value; servo_on(); while (!choose_button()) {
while (!choose_button()) {}car_turn_left();car_forward(radiusinches);car_turn_straight();car_forward(12L);
} servo_off();}
/* turn car s(traight), input distance, then go straight for that distance *//* used to calibrate distance (convert shaft encoder count to distance) */void carst() { int d, done=0, speed=6; long value; servo_on(); car_turn_straight(); while (!choose_button()) {
printf("\ndist?");value = getintfromkeypad();if (value == 0L) {
printf("speed? ");speed = (int) getintfromkeypad();
} else car_forward_ticks(value, speed); } servo_off();
29
}
cmotor.cvoid slowa(int port, int delay){
int i;for (i=4;i<=8;i++){
motor(port, i);msleep((long) delay);delay=(delay*2)/3;
}
}
/* this function lets the user control the wheels interactively *//* Roboboard's choose button toggles starting and stopping motors *//* Roboboard's potentiometer turns the wheels left and right */void cmotor(int port1, int port2){
int state=1; /* state is 0 when motors are off, 1 when motors are on */
float pmaxs;float deg;int spd;int prev, cur;float del;float prevd;int statechange=1;
prevd=0.0;del=0.01;servo_on();
while (1) { /* infinite loop (used for testing purposes) */
pmaxs=((float)robo_knob())/255.0;deg=(pmaxs*180.0);spd=(int)(pmaxs*8.0);if (state==1&&!choose_button()){
/* turn on motors */motor(port1,6);motor(port2,6);if ((deg-prevd)>2.0||(prevd-deg)>2.0){
if ((prevd-deg)>1.0){
while ((prevd-deg)>0.3){
servo_deg(prevd); /* turn wheels */prevd-=del;del=del*1.1;
}prevd=deg;servo_deg(deg);del=0.01;
}
else if ((deg-prevd)>1.0){
while ((deg-prevd)>0.3){
servo_deg(prevd); /* turn wheels */prevd+=del;del=del*1.1;
}prevd=deg;
30
servo_deg(deg);del=0.01;
}}
}
cur=spd;if (cur!=prev||statechange){if (state==1)printf("On, speed=%d\n", spd);if (state==0)printf("Off, speed=%d\n", spd);prev=cur;statechange=0;} if (choose_button()&&!state)
{ while (choose_button()) {}
state=1;statechange=1;
} else if (choose_button()&&state) { while (choose_button()) {} ao(); /* turn motors off */
state=0;statechange=1;
}
}}
compass.c/* compass pin numbers */#define COMPASS_SCLK 1#define COMPASS_SDO 2#define COMPASS_SDI 3#define COMPASS_NOTSS 4#define COMPASS_NOTP_C 5#define COMPASS_NOTCAL 6#define COMPASS_NOTRES 7#define COMPASS_NOTM_S 8#define COMPASS_NOTBCD_BIN 9#define COMPASS_YFLIP 10#define COMPASS_NOTXFLIP 11#define COMPASS_CI 12#define COMPASS_EOC 13#define COMPASS_NOTRAW 14#define COMPASS_VCC 15#define COMPASS_GND 16#define COMPASS_NOTRESET 17
/* convert compass pin number to corresponding Roboboard digital pin */int c2r(int cpin) {
/* NOTE: table[0] represents pin 1, table[1] is pin 2, etc. */int table[17]={14,2,0,13,13,12,0,0,0,0,11,0,0,0,0,0,0};return table[cpin-1];
}
/* send signal to compass */void compass_out(int cpin, int value) {
digital_out(c2r(cpin), value);}
/* input signal from compass */int compass_in(int cpin) {
return (!digital(c2r(cpin)));
31
}
void compass_waituser0() {printf("Point at 0 deg & push choose.\n");while (!choose_button());while (choose_button());
}
void compass_waituser1() {printf("Point at 180 deg& push choose.\n");while (!choose_button());while (choose_button());
}
/* Some of these routines are unnecessary for the Robobard, since default Roboboard settings are predictable. However, they are included in case others need them. */
/* Compass poweron routine, see pg9 sec2.10 of compass manual *//*void compass_poweron() {
compass_out(COMPASS_NOTP_C, 1);compass_out(COMPASS_NOTSS, 1);compass_out(COMPASS_NOTCAL, 1);compass_out(COMPASS_NOTRESET, 1);msleep(50L);compass_out(COMPASS_VCC, 1);
}*/
/* Compass reset routine, see pg18 of compass manual *//*void compass_reset() {
compass_out(COMPASS_NOTP_C, 1);compass_out(COMPASS_NOTSS, 1);compass_out(COMPASS_NOTCAL, 1);compass_out(COMPASS_NOTRESET, 0);msleep(10L);compass_out(COMPASS_NOTRESET, 1);msleep(500L);
}*/
/* other functions to put the compass in different modes *//*void compass_binarymode() {
compass_out(COMPASS_NOTBCD_BIN, 1);}
void compass_bcdmode() {compass_out(COMPASS_NOTBCD_BIN, 0);
}
void compass_slavemode() {compass_out(COMPASS_NOTM_S, 1);compass_out(COMPASS_NOTRAW, 1);
}*/
/* calibrate the compass *//* where a comment says "expect ...", should expect response from compass; we do not use this, because we have tested and determined that it is unnecessary since we are familiar with Roboboard interaction timing */int compass_calibrate() {
compass_out(COMPASS_NOTCAL, 1);/* expect CI low */compass_waituser0();compass_out(COMPASS_NOTCAL, 0);msleep(10L);compass_out(COMPASS_NOTCAL, 1);msleep(10L);
32
/* expect CI high */compass_waituser1();compass_out(COMPASS_NOTCAL, 0);msleep(10L);compass_out(COMPASS_NOTCAL, 1);msleep(250L);/* expect CI low */return 0;
}
/* make sure proper pins are set */void compass_prepare_read() {
compass_out(COMPASS_NOTRESET, 1);compass_out(COMPASS_NOTP_C, 1);compass_out(COMPASS_NOTSS, 1);compass_out(COMPASS_NOTCAL, 1);compass_out(COMPASS_EOC, 1);compass_out(COMPASS_SCLK, 1);
}
/* get a compass reading in slave mode */int compass_read() {
int i,b[18],d=0;compass_out(COMPASS_NOTP_C, 0);msleep(12L);/* expect EOC low */compass_out(COMPASS_NOTP_C, 1);msleep(110L); /* or wait until EOC hi */compass_out(COMPASS_NOTSS, 0);msleep(30L);for (i=0; i<18; i++) {
compass_out(COMPASS_SCLK, 0);msleep(10L);compass_out(COMPASS_SCLK, 1);b[i] = compass_in(COMPASS_SDO);
}compass_out(COMPASS_NOTSS, 1);for (i=8; i<=16; i++) {
d = 2*d + b[i];printf("%d", b[i]);
}printf("=%d\n", d);return (d-1);
}
/* no routine has been made yet for taking a compass reading in master mode, because we do not use this mode. Exercise is left to the reader. */
/* end of compass routines */
encoders.c/* Encoders.c -- encoder code for ic for RoboBoard rev 2.2. *//* Shaft encoders on digital ports 0 and 1 should be used first because they are more efficient. *//* Shaft encoders on digital ports 2 and 3 are less efficient and should only be used after 0 and 1 *//* In order to use encoders, plug a digital shaft encoder into digital port 0 -> 3, and call enable_encoder() with the port number. Now you can call read_encoder() with the port number to read the number of shaft counter ticks since the last reset, and reset_encoder() to reset that number. The counter overflows after around 32000 ticks */
void enable_encoder(int i){ if(i==0) { bit_set(0x1022,1); port0_shaft_count=0; }
33
else if(i==1) { bit_set(0x1022,2); port1_shaft_count=0; } else enable_cheesy_encoders(i);}
void disable_encoder(int i){ if(i==0) { bit_clear(0x1022,1); } else if(i==1) { bit_clear(0x1022,2); } else disable_cheesy_encoders(i);}
void reset_encoder(int i){ if(i==0) port0_shaft_count=0; else if(i==1) port1_shaft_count=0; else if(i==2) port2_shaft_count=0; else if(i==3) port3_shaft_count=0;}
int read_encoder(int i){ if(i==0) return(port0_shaft_count); else if(i==1) return(port1_shaft_count); else if(i==2) return(port2_shaft_count); else if(i==3) return(port3_shaft_count); return(0);}
getarray3.c/* used by us to debug memory and analyze map data */int getarray[](int half){
int i;int tempi;
int steps[50];
for (i=0;i<50;i++){
steps[i]=peekword(50000+(i*2)+half);}
return steps;}
keypad.c
34
/* loop and check keypad until a key is pressed, then return value based on key */int getkeypadpush(){
int i,j,row,col,done=0;
/* clear all the keypad digital outputs */for (j=0; j<3; j++)
digital_out(j+8,!0);
while (!done) {for (j=0; j<3; j++) {
digital_out(j+8,!1); /* assert column */msleep(33L);for (i=0; i<4; i++) { /* check rows */
if (digital(i+4)) { /* key was pressed */done=1;row=i;col=j;
}}digital_out(j+8,!0);
}}/* printf("Row=%d Col=%d\n", row, col); */digital_out(col+8,!1); /* deassert column */while (digital(row+4)) /* wait until user releases key */
msleep(25L);/* printf("Let go.\n"); */return (row*3+col+1); /* return value based on column and row */
}
/* uses the above function, and returns a more meaningful value *//* instead of row and column, it returns 0-9 or 10 (for *) or 11 (for #) */int getkey() {
int keypaddata, row, col, val;int intmap[13] = {0,1,2,3,4,5,6,7,8,9,10,0,11};char charmap[13] = {'0','1','2','3','4','5','6','7',
'8','9','*','0','#'};
keypaddata = getkeypadpush();printf("%c", charmap[keypaddata]);
return intmap[keypaddata];}
/* get multidigit number from keypad (* = negative, # = stop) */long getintfromkeypad() {
long total=0L, sign=1L;int val=0;while (val != 11) { /* repeat until user hits pound key */
val=getkey();if ((val>=0) && (val<=9))
total = total*10L + (long) val;if (val==10) /* user hit star key */
sign = -sign;}/* printf("\nValue=%d.\n", sign*total); */return sign*total;
}
/* test program; input two numbers from keypad and add them */void keypadadd() {
long a, b, c;printf("\n");printf("a=");a = getintfromkeypad();printf("\n");printf("b=");b = getintfromkeypad();printf("\n");printf("a+b=%d\n", (a+b));
35
}
/* get two parameters from keypad (feet and inches), convert to inches */long keypadgetinches() {
long ft, in;printf("ft: ");ft = getintfromkeypad();printf("in: ");in = getintfromkeypad();return (ft*12L + in);
}
main.c/* as of the time of this writing, the team hasn't put everything together yet *//* this will eventually put everything together, including pathfinding */int oldmain(){theme();}
math.c#define PI 3.14159265
long abs(long i) {if (i>=0L) return i;return -i;
}
/* float absolute value */float fabs(float f) {
if (f>=0.) return f;return -f;
}
/* convert radians to degrees */float r2d(float rad) {
return rad*180./PI;}
rangefinder2.c/* ask range finder to report distance to object straight ahead */int read_range(){
digital_out(11,0);
rfecho(0); /* see assembly function */
digital_out(11,0);
return peekword(0xd002);
}
int range_finder(){int time1=3000, time2=3000,time3=3000, dt1, dt2, dt3;int state=0;int x=0;int temp=0;
/* the range finder sometimes does not report a reasonable reading *//* make sure that we get reasonable readings */while (temp<20&&(time1>=950||time2>=950||time3>=950)){
36
time1=read_range();time2=read_range();time3=read_range();if(time1>=1000||time2>=1000||time3>=1000){ digital_out(11,0); msleep(100L);}
temp++;}
printf("%d ",time1);printf("%d ",time2);printf("%d\n",time3);printf("t=%d",temp);
dt1=abs(time1-time2);dt2=abs(time2-time3);dt3=abs(time1-time3);if (temp>=20)return 2500;if (dt1>=dt2&&dt3>=dt2)return (int)(((float)time2+(float)time3)/2.0);else if (dt1>=dt3&&dt2>=dt3)return (int)(((float)time3+(float)time1)/2.0);else if (dt2>=dt1&&dt3>=dt1)return (int)(((float)time1+(float)time2)/2.0);}
rfecho.asm ORG MAIN_STARTsubroutine_rfecho: LDA $0052 ; load register with data containing dig port 11 ORA #$08 ; set dig port 11 to high; NOTE: set back to low outside STA $0052 ; write register back STA $7800 ; write this register, too
LDD #$0000 ; load doublebyte (zero) into D STD $D000 STD $D002
label: LDD $D002 ; load current count into D ADDD #$0001 ; add to D STD $D002 ; store doublebyte D in memory
CPD #2500 ; compare D to see if it reached the maximum BNE skipa ; if D<max, don't skip to end BRA skipz ; skip to end
skipa: LDA $7800 ; load register with data containing dig port 3 ANDA #$08 ; isolate port 3 CMPA #$08 ; compare port 3 is high BNE label
skipz: RTS
robogeo.c#define ALLOWBACKWARDS 1
/* this function finds the shortest direct path from (0, 0) to (destx, desty) within this contraint: car does not have 0 turn radius, so must follow arc initally */
/* this is done by determining the circular path on which the car can travel, then finding a line that is tangent to this circle and that passes through (destx, desty). Then, the car turns and moves along the circle to the point of tangency, then moves straight towards (destx, desty).
37
*/
void geosolve(float adx, float ady, int *bestdir, float *besttdist, float *bestsdist) {float alpha, beta, gamma, theta, r, dx, dy, h, L, xp, yp, x, y;float turndist, straightdist, D, bestD;int i, j;int angle;bestD=99999.0; /* shortest distance */if (fabs(adx)<0.01) { /* delta x is zero */
if (fabs(ady)<0.01) { /* destination is (0,0) */*bestdir = 0;*besttdist = 0.0;*bestsdist = 0.0;
}else { /* destination is directly ahead or behind */
*bestdir = 0;*besttdist = 0.0;*bestsdist = ady;
}}else for (i=-1; i<=1; i+=2) { /* i=-1 is right circle, i=1 is left circle */
r = 10.8; /* turn radius *//* find distance from circle center to dest */dx = adx + ((float)i * r);dy = ady;alpha = atan(dy/dx);h = sqrt(dx*dx + dy*dy);/* if h<r, no solution possible; can can't turn sharp enough */if (h>r) {
/* L = distance from point of tangency to dest */L = sqrt(h*h - r*r);beta = atan(r/L);for (j=-1; j<=1; j+=2) { /* two points of tangency per circle */
/* gamma is either alpha+beta or alpha-beta */gamma = alpha + ((float)j * beta);xp = L * cos(gamma);yp = L * sin(gamma);if (dx<0.) {
xp = -xp;yp = -yp;
}/* (x,y)=point of tangency relative to center of circle */x = dx - xp;y = dy - yp;theta = atan(y/x);if (x<0.) theta = PI+theta;/* theta is angle to (x,y) */
/* car begins positioned at 180deg for right circle */if (i<0) theta = PI-theta;angle = (int) (theta * 180. / PI);angle = (angle + 360) % 360;if (angle>180) angle=angle-360; /* shortest delta angle */theta = (float)angle * PI / 180.;turndist = r * theta; /* arc length */straightdist = L; /* dist from tangency point to dest *//* if i!=j, then car will do straight portion backwards */if (i!=j) straightdist = -straightdist;if (turndist<0.0)
D = L-turndist;else
D = L+turndist;if (D<bestD) {
if (ALLOWBACKWARDS || (straightdist >= 0.)) {*bestdir = i;*besttdist = turndist;*bestsdist = straightdist;bestD = D;
}}
}
38
}}
}
void geotest(float x, float y) {int dir;float d1, d2;char dirchar[4] = "RSL";geosolve(x, y, &dir, &d1, &d2);printf("%c", dirchar[dir+1]);printf("%f ", d1);printf("S%f\n", d2);
}
robolib.c/* These constitute a lot of basic functions of the Roboboard, and have been documented in our lab report. The ELEC201 manual explains these in even greater detail. */
void reset_system_time(){ pokeword(0x14, 0); pokeword(0x12, 0);}
float seconds(){ return ((float) mseconds()) / 1000.;}void sleep(float seconds){ msleep((long)(int)(seconds * 1000.));}
void msleep(long msec){ long end_time= mseconds() + msec;
while (1) {
long done= mseconds()-end_time;if (done >= 0L && done <= 1000L) break;
}}
void beep(){ tone(2500.0, .3);}
void tone(float frequency, float length){ set_beeper_pitch(frequency); beeper_on(); sleep(length); beeper_off();}
void beeper_on(){ bit_set(0x1020, 0b00000001); bit_set(0x1022, 0b00001000);}
void beeper_off(){ bit_clear(0x1022, 0b00001000);
39
bit_clear(0x1020, 0b00000001); bit_clear(0x1000, 0b00001000); }
void set_beeper_pitch(float frequency){ pokeword(0x26, (int)(1000000.0 / frequency));} /* special pulses are used to control wheel motors */int _speed_table[]= {
0b00000000, 0b00000001, 0b00010001, 0b01001001, 0b01010101, 0b01011011, 0b01110111, 0b01111111,
0b11111111 };
void forward(int motor){ _set_motor(motor, 0b00000010, 8); }
void reverse(int motor){ _set_motor(motor, 0b00000001, 8); }
void brake(int motor){ _set_motor(motor, 0b00000000, 8);}
void off(int motor){ if (motor < 0) return; if (motor < 2) bit_clear(0x47, 1 << (7 - motor)); else if (motor < 4) bit_clear(0x48, 1 << (9 - motor)); else if (motor < 6) bit_clear(0x49, 1 << (11 - motor)); else return;}
void alloff(){ bit_clear(0x47, 3 << 6); bit_clear(0x48, 3 << 6); bit_clear(0x49, 3 << 6);}
void ao(){ alloff();}
void motor(int m, int speed){ if (speed >= 0) _set_motor(m, 0b00000010, speed); else _set_motor(m, 0b00000001, -(speed));}
void _set_motor(int motor, int dir, int speed)
40
{ if ((speed < -8) || (speed > 8)) { printf ("Illegal speed %d", speed); return; } if (motor < 0) return; if (motor < 2) { bit_set(0x47, 1 << (7 - motor)); if (motor==0)
{ bit_clear(0x47, 0b00001100); bit_set(0x47, (dir << 2)); poke(0x4a, _speed_table[speed]);}
if (motor==1){ bit_clear(0x47, 0b00110000); bit_set(0x47, (dir << 4)); poke(0x4b, _speed_table[speed]);}
} else if (motor < 4) { bit_set(0x48, 1 << (9 - motor)); if (motor==2)
{ bit_clear(0x48, 0b00001100); bit_set(0x48, (dir << 2)); poke(0x4c, _speed_table[speed]);}
if (motor==3){ bit_clear(0x48, 0b00110000); bit_set(0x48, (dir << 4)); poke(0x4d, _speed_table[speed]);}
} else if (motor < 6) { bit_set(0x49, 1 << (11 - motor)); if (motor==4)
{ bit_clear(0x49, 0b00001100); bit_set(0x49, (dir << 2)); poke(0x4e, _speed_table[speed]);}
if (motor==5){ bit_clear(0x49, 0b00110000); bit_set(0x49, (dir << 4)); poke(0x4f, _speed_table[speed]);}
} else return;}
void led_out0(int s){ if (s) bit_set(0x49, 0b00000010); else bit_clear(0x49, 0b00000010);}
void led_out1(int s){ if (s) bit_set(0x49, 0b00000001); else bit_clear(0x49, 0b00000001);}
41
int choose_button(){ return ! (2 & (peek(0x7800)));}
int escape_button(){ return !(1 & (peek(0x7800)));}
int dip_switches(){ int i,value; for(i=0,value=0;i<4;value+=dip_switch(++i)) value<<=1; return value;}
int dip_switch(int which){ if (which > 4 || which < 1) return -1; else return(analog_mux(((which-1)<<8)+7)<128);}
int analog(int port){ if (port<0 || port>30) { printf("Analog port out of range\n"); beep(); return -1; } if (port<8) if (digital(port)) return 0; else return 255; if (port<20) return -1; if (port<28) return(analog_mux(((port-20)<<8)+6)); if (port<30) return(analog_mux(((port-22)<<8)+7)); if (port<31) if (digital(port)) return 0; else return 255;}
int digital(int port){ if (port<0 || port>30) { printf("Digital port out of range\n"); beep(); return -1; } if (port==0) return !(peek(0x1000)&1); if (port==1) return !(peek(0x1000)&2); if (port<8) return !((peek(0x7800)>>port)&1); if (port<20) return -1; if (port<30) return analog(port)<127; if (port<31) { bit_clear (0x1026, 0b10000000); return !(peek(0x1000)&128); }
42
}
int digital_out(int port, int value){ int current; if (port<8 || port>19 && port!=30) { printf("Port %d not a digital output\n", port); beep(); return -1; } if (port < 16) { if (value==0)
bit_clear(0x52, 1 << (port-8)); else
bit_set(0x52, 1 << (port-8)); current=peek(0x52); poke(0x7800, current); return 0; } if (port < 18) { if (value==0)
bit_clear(0x48, 1 << (17-port)); else
bit_set(0x48, 1 << (17-port)); return 0; } if (port < 20) { if (value==0) bit_clear(0x47, 1 << (19-port)); else
bit_set(0x47, 1 << (19-port)); return 0; } if (port==30) { bit_set(0x1026, 0b10000000); if (value==0)
bit_clear(0x1000, 0b10000000); else
bit_set(0x1000, 0b10000000); return 0; }}
int motor_force(int port){ return analog_mux(port);}
/* potentiometer */int robo_knob(){ return analog_mux(0x0407); }
int voltage(){ return ((analog_mux(0x0507)*100) / 17);}
void hog_processor(){ poke(0x0a, 0);}
void system_pwm_on() {bit_set(0x39, 0b00000100);}
43
void system_pwm_off() {bit_clear(0x39, 0b00000100);}
void system_print_on() {bit_set(0x39, 0b00000001);}void system_print_off() {bit_clear(0x39, 0b00000001);}
servo.c/************************************************************************//* *//* "servo.c" for the 6.270 'bot *//* servo support code *//* by Anne Wright *//* Sat Jan 11 *//* *//************************************************************************/
/* The values MIN_SERVO_WAVETIME and MAX_SERVO_WAVETIME given here can only be approximate because the appropriate range depends on the servo. These values should be conservative, and therefore may not allow the full range of movement from your servo.
To customize this for a given servo, set the value of MIN_SERVO_WAVETIME to 0, and MAX_SERVO_WAVETIME to 10000 at the IC interaction prompt. Then call servo(x); at the prompt where x gets slowly smaller until you find the lower limit. You can tell when it hits the limit because the body will heat up and it will buzz.
The appropriate value of MIN_SERVO_WAVETIME is the smallest value of x which does not cause the servo to heat up. Set it to this value in your initialization routine.
Servos are usually able to go a little more than 180 degrees. The accuracy of the servo_rad and servo_deg routines depend on the position set by MAX_SERVO_WAVETIME being close to 180 degrees away from the position set by MIN_SERVO_WAVETIME. If you want those routines to be accurate, call servo(x); to get 180 degrees off. If instead you want the maximum possible excursion, follow the same routine as above to find the largest value of x which does not cause the servo to heat up. Again, set MAX_SERVO_WAVETIME in your initialization routine to the value you choose.
Note that floating point is not very efficient, so it is better to use integers when you can. Therefore it is most efficient to use the integer pulse widths directly rather than to call servo_rad and servo_deg. You can use the conversion functions radian_to_pulse or degree_to_pulse to initially calculate the appropriate values, then use those integers.*/
int MIN_SERVO_WAVETIME = 1400;int MAX_SERVO_WAVETIME = 4530;
#define SERVO_RANGE (MAX_SERVO_WAVETIME-MIN_SERVO_WAVETIME)
#define rexcursion 3.14159#define dexcursion 180.
void servo_on(){ asm_servo_on(0);}void servo_off(){ asm_servo_off(0);}/************************************************************************//* Servo movement commands */int servo(int period) /* argument in clock cycles of pulse, moves servo */{ if(period>MAX_SERVO_WAVETIME)
44
return (servo_pulse_wavetime=MAX_SERVO_WAVETIME); else if(period<MIN_SERVO_WAVETIME) return (servo_pulse_wavetime=MIN_SERVO_WAVETIME); else return(servo_pulse_wavetime=period);}int servo_rad(float angle) /* argument in radians, moves servo */{ return servo(radian_to_pulse(angle));}int servo_deg(float angle) /* argument in degrees, moves servo */{ return servo(degree_to_pulse(angle));}/************************************************************************//* Pulse width calculations */int radian_to_pulse(float angle) /* argument in radians, returns pulse width */{ return ((int)(angle*((float)SERVO_RANGE)/rexcursion)+MIN_SERVO_WAVETIME);}
int degree_to_pulse(float angle) /* argument in degrees, returns pulse width */{ return ((int)((angle*((float)SERVO_RANGE))/dexcursion)+MIN_SERVO_WAVETIME);}
servo.icb<the machine code section has been cut out of this printing><the symbol section has been included here>ADCTL 1030 *0049 ADR1 1031 *0050 ADR2 1032 *0051 ADR3 1033 *0052 ADR4 1034 *0053 BADOPINT 00f8 *0086 BASE 1000 *0006 0036 0049 0061 0084 BAUD 102b *0044 BPROT 1035 *0054 CFORC 100b *0018 0091 CMEINT 00fc *0088 CONFIG 103f *0064 COPRST 103a *0059 DDRC 1007 *0014 DDRD 1009 *0016 HPRIO 103c *0061 0015 INIT 103d *0062 IRQINT 00f2 *0083 NOCOPINT 00fa *0087 OC1D 100d *0020 0053 0090 0093 OC1M 100c *0019 0039 OPTION 1039 *0058 PACNT 1027 *0040 PACTL 1026 *0039 0038 PAIINT 00da *0071 PAOVINT 00dc *0072 PIOC 1002 *0010 PORTA 1000 *0008 0009 PORTB 1004 *0012 PORTC 1003 *0011 PORTCL 1005 *0013 PORTD 1008 *0015 PORTE 100a *0017 PPROG 103b *0060 REPEAT_PERIOD 0014 *0011 0065 0095 RESETINT 00fe *0089 RESV1 1001 *0009 RESV2 1036 *0055 RESV3 1037 *0056 RESV4 1038 *0057
45
RTIINT 00f0 *0082 SCCR1 102c *0045 SCCR2 102d *0046 SCDR 102f *0048 SCIINT 00d6 *0069 SCSR 102e *0047 SERVO_ADDRESS 1000 *0009 SERVO_MASK 0080 *0010 SPCR 1028 *0041 SPDR 102a *0043 SPIINT 00d8 *0070 SPSR 1029 *0042 SWIINT 00f6 *0085 SetPulse 8785 *0082 0075 TCNT 100e *0023 0086 TCTL1 1020 *0033 TCTL2 1021 *0034 TEST1 103e *0063 TFLG1 1023 *0036 TFLG2 1025 *0038 TI4O5 101e *0031 TIC1 1010 *0024 TIC1INT 00ee *0081 TIC2 1012 *0025 TIC2INT 00ec *0080 TIC3 1014 *0026 TIC3INT 00ea *0079 TMSK1 1022 *0035 TMSK2 1024 *0037 TOC1 1016 *0027 0088 TOC1INT 00e8 *0078 TOC2 1018 *0028 TOC2INT 00e6 *0077 TOC3 101a *0029 TOC3INT 00e4 *0076 TOC4 101c *0030 TOC4INT 00e2 *0075 0029 0034 TOC5INT 00e0 *0074 TOINT 00de *0073 XIRQINT 00f4 *0084 interrupt_code_exit 879e *0098 0030 0072 0080 interrupt_code_start 8774 *0070 0033 servo_count 872f *0022 0066 0074 0078 0096 subroutine_asm_servo_off 8758 *0048 subroutine_asm_servo_on 8765 *0060 subroutine_initialize_module 8730 *0024 variable_servo_enable 872d *0019 0052 0063 0071 variable_servo_pulse_wavetime 872b *0018 0043 0087
stepscanmem3.c/* Stepper motor control functions *//* The following code will set up the stepper motors to run in high torque mode - see documentation for details */
/* The step table just has one entry per motor port, to hold thestate of that port */
int _step_table[]= { 0b00000001, 0b00000001, 0b00000001, 0b00000001, 0b00000001, 0b00000001
};
/* Stepper motors have four control wires. Normally these are turned on and off one at a time in sequence to make the stepper turn. This is called fourphase stepping.
46
__ __Wire 1 | |________| |____ __ __Wire 2 ___| |________| |_
__ _Wire 3 _____| |________|
__Wire 4 ________| |_______
The following functions use a different method. They keep two wires high at all times instead of one like the four phase control uses. This provides moretorque. The control looks as follows:
____ ____Wire 1 | |____| |____
____ ____Wire 2 |____| |____|
__ ____ __Wire 3 |____| |____|
____ ____Wire 4 __| |____| |__
Thus, for each pair, one is high and one is low at all times, with the two pairsout of phase.*/
/* This will take a "radar sweep" of the surroundings with the range finder */int stepscan(int port1, int port2, int encodport){
int i;int temp;int tempi;long timed2;int enco;int state=0;int direction=0;int index=0;int count=0;int x=5;int pdir=1;int count2=0;int range1;int range2;int mem1;int mem2;
int on=0;int tempo=0;
enable_encoder(encodport);reset_encoder(encodport);
for (tempo=0;tempo<100;tempo++)range_finder();
while (1){
if ((choose_button()&&index<15000)||direction==1){
while(choose_button()){}reset_encoder(encodport);printf("\nSwitching dir=%d",direction);
msleep(50L);temp=port1; /*switch directions*/port1=port2;port2=temp;direction=!direction;
printf("\nDirection switched");
47
if (on==0){
printf("\nMotors on"); msleep(50L);
_set_motor(port1,_step_table[port1],8); /* turn motor on */_set_motor(port2,_step_table[port2],8);on=1;
}
while(!read_encoder(encodport)) /* find encoder hole */{
printf("\nFinding encoder"); _step_table[port2]=_step_table[port2]^0b00000011;
_set_motor(port2,_step_table[port2],8);msleep(5L);_step_table[port1]=_step_table[port1]^0b00000011;_set_motor(port1,_step_table[port1],8);count++;
if (count>x) /* when x is reached, switch directions and increment x */
{temp=port1;port1=port2;port2=temp;direction=!direction;x=x+2;count=0;
}
} /* end while(!read_encoder(encodport)) */
count=0; /* re-initialize counters */x=5;
reset_encoder(encodport);
printf("\nEncoder found");
if (pdir==direction) /* make sure motor is going the right direction */
{printf("\nDirection not correct");
msleep(50L);temp=port1;port1=port2;port2=temp;pdir=!pdir;
}
printf("\nDirection corrected");bit_set(0x47,1<<7);for (i=0; i<=100; i+=2){
_step_table[port2]=_step_table[port2]^0b00000011;_set_motor(port2,_step_table[port2],8);if (direction==1){
range1=range_finder();mem1=(50000+(i*2)+index*200+((index!=0)&&(index
%2==0))*4);pokeword(mem1,range1);
}_step_table[port1]=_step_table[port1]^0b00000011;_set_motor(port1,_step_table[port1],8);if (direction==1){
range2=range_finder();
48
mem2=(50000+((i*2)+2)+index*200+((index!=0)&&(index%2==0))*4);
pokeword(mem2,range2); }count2++;
if (count2==25){
printf("\nEncoder reset"); reset_encoder(encodport);
}
} /* end for (i=0; i<=100; i+=2) */
count2=0;pdir=!pdir;msleep(1000L);
enco=read_encoder(encodport);
if (direction==1)index=index+1;
} /* end if (choose_button()&&index<600) */
printf("\nScan number: %d",index);msleep(50L);
if (escape_button()){
ao();disable_encoder(encodport);return enco;
}
}/* end while(1) */
}/* end stepper */
theme.c/* play three notes, pause, play low note, then play parabola of notes */void theme(){
int i=0;float d;float e;
{tone(450.0,0.2);tone(510.0,0.2);tone(410.0,0.2);}sleep(0.3);tone(204.0,0.2);
d=1.0;
for(i=0;i<30;i++){
d+=0.05;tone(sin(d)*700.0,0.04);
}
}
49
Appendix 2
Pictures
Bottom view of the pathfinder. Notice the two motors powering the gear box which in turn powers the rear differential and then the rear axle.
50
Front view of the Pathfinder.
Angled view of the Polaroid Ultra-Sonic Ranging Module attached to the stepper motor. Notice the Shaft encoder wheel just below the transducer.
51
Top view of the RoboBoard connected to the range finder and powered through the voltage regulator.
The voltage regulator. The input power is on the left, output power is on the right. Positive is on the top and common is on the bottom.
52
53
54
55
