Tallinn University of Technology
Department of Mechatronics
Firefighter SAM
Mechatronics (MHK0030)
Instructor:
Raivo Sell
Developers:
Märt Pikkani (Software)
Margus Lehtme (Electronics)
Sulev Eesmaa (Mechanics)
Tallinn 2006
Table of contents
Table of contents ................................................................................................................. 2
1 Abstract ....................................................................................................................... 3
2 Kokkuvõte eesti keeles ............................................................................................... 4
3 Introduction ................................................................................................................. 5
4 The Task...................................................................................................................... 5
5 Ideas ............................................................................................................................ 5
6 Problems ..................................................................................................................... 5
7 Specification ............................................................................................................... 5
7.1
Product idea .......................................................Error! Bookmark not defined.
7.2
Functional requirements.....................................Error! Bookmark not defined.
7.3
Behavior .............................................................Error! Bookmark not defined.
7.4
Limitation ...........................................................Error! Bookmark not defined.
7.5
Time schedule ..................................................................................................... 5
7.6
Block diagram ..................................................................................................... 7
7.7
Data flow ............................................................................................................. 8
8 Mechanics and Robot Body ........................................................................................ 9
9 Motors ....................................................................................................................... 12
9.1
Servo parameters ............................................................................................... 12
9.2
Motor configuration .......................................................................................... 13
10
Sensors .................................................................................................................. 13
11
Electronics............................................................................................................. 16
11.1 Connection schematics...................................................................................... 17
11.2 Bill of Materials ................................................................................................ 17
12
Programming......................................................................................................... 18
12.1 Programming environment ............................................................................... 18
12.2 Algorithm .......................................................................................................... 18
12.2.1
Moving ...................................................................................................... 18
12.2.2
Finding the candle ..................................................................................... 20
12.2.3
Blowing the candle ................................................................................... 21
12.3 Code .................................................................................................................. 21
13
Conclusions ........................................................................................................... 22
13.1.1
Conclusion of Märt ................................................................................... 22
13.1.2
Conclusion of Margus ............................................................................... 22
13.1.3
Conclusion of Sulev .................................................................................. 23
14
References ............................................................................................................. 23
Appendix 1: Source code .................................................................................................. 24
2
1 Abstract
Current documentation is compiled as a part of mechatronics course held in Tallinn
University of Technology. Main objective of the course was to build a firefighting robot,
which is able to find candle from known maze and stop the fire. To see exact task, please
the appendix. Team consisted of three persons - one was responsible of software, second
was responsible for mechanics and third was responsible of electronics. Important
milestones were 21.09.2006 for idea presentation. 12.10.2006 for presentation of first
solution, where our robot was able to move. 09.11.2006 was milestone for prototype
presentation where nothing much was different from 12.10.2006. At the same date with
Robotex contest on 24.11.2006 there was held contest between course members. Robot
was able to move in the maze but it couldn’t find the candle.
Team met at least twice a week. Protocol was written after meeting where important
decisions were marked down (given also in appendix). Team members used forum for
communication. For creating the documentation there was WIKI style homepage used.
At first the task didn’t seem very difficult. After we have started working on the system
we found out that algorithm for moving along the wall was already a difficult task.
It must mentioned, that teamwork is also very important. Although each team member
worked hard for the target, robot couldn’t succeed with the task on 24-th of November.
3
2 Kokkuvõte eesti keeles
Tallinna Tehnikaülikoolis, Mehhatroonika (MHK0030) aine raames tuli luua
“Firefighter” robot. Vaata ülesande püstitust lisast, töö lõpus. Meeskond roboti loomiseks
koosnes kolmest liikmest, kus üks oli vastutav tarkvara, teine mehhaanika ja kolmas
elektroonika eest. Olulisemad tähtajad olid 21.09.2006, kus toimus ideede presentatsioon.
12.10.2006 toimus esmalahenduse demonstratsioon, kus käesoleva töö käigu valmistatav
robot suutis liikuda. 09.11.2006 toimus prototüübi presentatsioon, kus roboti tarkvara oli
pisut täiendatud, kuid midagi suurt ei olnud lisatud. Robotexi võistlusega samal päeval,
24.11.2006 toimus võistlus kursuses osalenute vahel, kus loodud robot suutis ruumides
ringi liikuda, kuid küünalt leida ei suutnud.
Meeskonnatöö käigus saadi vähemalt 2 korda nädalas kokku. Kohtumiste kohta kirjutati
tavaliselt ka protokoll, kust oli võimalik välja lugeda kokkulepped jms. Suhtlemiseks
kasutati foorumit, kuhu said kõik meeskonnaliikmed enda ideed kirja panna.
Dokumentatsiooni loomiseks kasutati WIKI keskkonda.
Küünlakustutamise ülesanne ei tundunud esialgu väga keeruline. Olles aga reaalselt
süsteemi looma hakanud selgus, et näiteks algoritm roboti mööda seinaäärt liikumiseks
on juba raske luua.
Suur roll kogu ülesande edukaks täitmiseks on meeskonnatööl. Iga meeskonna liige andis
endast kõik, et robot täidaks talle ette antud ülesande. Kahjuks 24-ndal novembril temale
roboti jaoks seatud ülesanne jäi täitmata.
4
3 Introduction
Current documentation is compiled as a part of mechatronics course held in Tallinn
University of Technology. Main objective of the course was to build a firefighting robot,
which is able to find candle from known maze and stop the fire.
4 The Task
The task for robot is to find a candle in a maze and blow it. Exact maze layout is known
and given in the specific task description (see appendix). Money which can be used for
robot parts is limited. Size of the robot is also limited.
5 Ideas
Idea of our team about the robot is as following:

Move along walls using IR distance sensors and Ultra Sonic sensors;

Find the candle, using IR photo diodes;

When candle has been found then blow it with ventilator.
6 Problems
Following problems occurred when robot was implemented:





It is not easy to make 90 degree turns as motors act differently in different
situations;
Finding candle with an IR photo diode is not possible because of IR noise from
surrounding environment (people, light bulbs etc)
It is difficult to find a room as there is no wall which leads into the room;
Time planning is more important then it may seem, even when we thought we
have working and realistic time schedule, at the end of the course it just stopped
working.
Temporary things tend to live forever. Until they break, stop working or just
cause major loss of time when debugging (bad cable connectors, lots of open
wires)
7 Specification
7.1 Time schedule
Group gathered together at least once a week, usually 2-3 times per week. On every first
meeting of the week we set up tasks and checkpoints for upcoming week and analyzed
last week's progress.
Tasks and actions of the 1. week (week 38, 18-24 September):
5





Set up communication channel (phpBB forum)
Familiarize with AVR Kit and Atmega128
Check out possible motor variants
First tests with fan and candle
Set deadline to 8th of November
Tasks and actions of the 2. week (week 39, 25 September - 1 October)


Modify servos & first experiments with PWM and servo motors
Make robot body and wheels out of acrylic
Tasks and actions of the 3. week (week 40, 1 - 8 October)




Motors, wheels and body assembled
Robot is able to move, no intelligence or sensors yet
Software development
First problems with supply voltage, common ground and servo interference
Tasks and actions of the 4. week (week 41, 9 - 15 October)




Read and act according to sonars and IR sensors
Balance and stabilize robot
Common ground and supply voltage problems somehow solved
Find and order IR sensors for flame detection
Tasks and actions of the 5. week (week 42, 16 - 22 October)



First signs of stagnation, quite quiet week, mostly some brainstorming
Some supply board improvements (5V regulated voltage)
Software development
Tasks and actions of the 6. week (week 43, 23 - 29 October)

Some software developement
Tasks and actions of the 7. week (week 44, 30 October - 5 November)

Software development, dealing with serial communication for debugging
Tasks and actions of the 8. week (week 45, 6 - 12 November)




Robot presentation
Dealing with I2C communication (sonar uses I2C)
Serial communication working
Mount and wire all the sensors
6
Tasks and actions of the 9. week (week 46, 13 - 19 November)


Software development
Fan switching circuit
Tasks and actions of the 10. week (week 47, 20 - 26 November)






Changed most of connectors
New supply and fan switching circuit
Fan mounted
Tried several wall-fallowing algorithms
Robot is now in needed dimensions
Demo run at Robotex - did not find the candle
Tasks and actions of the 11. week (week 48, 27 November - 3 December)


Touch sensors mounted
Code improvements and documentation
7.2 Block diagram
Simple block diagram of created robot is given on picture below.
Figure 1 Block diagram of robot
Where abbreviations of inputs are as following:

US - Ultrasonic (distance sensor)
7

IR dist. - Infrared (distance sensor)

IR - Infrared photodiode for candle detection

Touch - touch sensors for impact detection
Abbreviations of outputs are as following:

servo - servo motors used for moving

LED - light emitting diode for showing that robot is alive

UART - RS-232 port used for debugging the SW

LCD - liquid crystal display for debugging purposes

VENT - fan to extinguish the fire
7.3 Data flow
Input dataflow diagram is given on the picture be
low.
In robot SW the timer is generating interrupt after 1000 controller ticks. When the
interrupt has been generated then time of the timer is looked. When the timeout for IR
distance, IR photo diode or ultrasonic sensor has been reached then the measuring of the
signal is started. ADC input values are pushed into channel_buffer array which is
working like FIFO. Use of the buffers is as following:




Channel_buffer[0] – LEFT_DIST_SENSOR;
Channel_buffer[1] – RIGHT_DIST_SENSOR;
Channel_buffer[2] – LEFT_CNDL_SENSOR;
Channel_buffer[3] – RIGHT_CNDL_SENSOR.
Ultrasonic measurement values are inserted into FIFO uh_channel_buffer.
8
When touch sensors indicate collision then left_touch or right_touch values is set to
TRUE.
When timer generates interrupt it always increases the time in time value buffers
time_high and time_low. There are two buffers used for counting time as one value. Time
low counts the tick of micro-controller. Time_high is increased after time_low buffer
overflow.
PE6 port is set high for starting the ventilator to blow the candle off.
For controlling the motors the different PWM duty cycle is set using following registers:


OCR1A – RIGHT_SERVO;
OCR1B – LEFT_SERVO.
8 Mechanics and Robot Body
The main idea behind developing the robot mechanics was to keep everything as simple
as possible. Also all mechanics had to be improvable (adding and removing different
parts with no further damage to the rest of mechanics).
Robot main body parts were built of plexiglas which were cut out with the help of the
CNC machine. CNC was used because of the good output quality. CNC used CAD
drawings as the input and this made easier to get quickly new body if necessary. The
body drawing is shown on the Picture 1, which shows the body from top view. All the
necessary holes for different sensors, AVR, servos and fan are shown on this picture.
The initial competitions rules made some boundaries for developing the mechanics, the
robot had to fit in the tube of diameter 150mm and height 100mm. Also the weight had to
be less than 1200 grams. The robot bottom side was round but the sides were cut off,
otherwise the robot would not fit to the boundaries. Sides cut also made connecting the
wheels easier. The wheels are made of plexiglas and are controlled by the servo motors.
The wheels were covered with tape which removed the slickness. Servo motors came
with special connectors so the connecting motor with wheel was easy. The motors were
connected under the bottom of robot with the help of plexiglas joints which were fitted to
special holes in the robot bottom (Picture 1). The joints and motors were connected with
bolt and nut. Robot body was supported with two static feet. With the help of these feet,
the robot would always be in balance. The AVR kit was connected to the body with 4
spacers so the kit is about 10mm from bottom of the robot.
9
Picture 1. The top view of the robots body
Picture 2. The side view of the robots body with sensors, AVR and fan
On picture 2, it is shown the placement of the sensors. This picture is from one side and
gives overview of the placement of different parts of the robot. The IR distance sensors
on the sides were connected with metal strip which had holes in it so the position of the
IR sensors is easy to change by connecting the sensor with bolts to different holes.
Ultrasonic sensor on the front of the robot is connected with spacer. The candle detection
IR sensors were built in little pipes. The pipes were connected with special metal strip.
The IR sensors were built in pipes because the idea was to get only direct IR light from
10
candle all the other IR sources light would not reach to the end of the pipes, where the IR
diodes were built in. The primal plan was connect the fan (for distinguishing the candle)
on the top of the ultrasonic sensor but then the robot would not fit in the tube. So the fan
had to connect to the back of the robot, so when the robot detects the candle it has to turn
around. The fan propeller is moving in the special gap which was cut inside the
Plexiglas(Picture 1), otherwise the robot would not fit to the limits. There were also used
touch sensors which were little micro buttons, they were connected to metal strip so that
when robot hits the wall on front or on the sides, then the metal strip with button is
pressed against the robot body so that the button is switched on and the AVR knows that
the robot has rammed the wall.
Picture 3. The real robot
The batteries were in two packages one was for supplying the AVR and the other was for
motors. The motor supply package was connected to the bottom of the body between the
servo motors, by placing the batteries under the body, the robot is more stable and
11
doesn’t loose it balance so easily. The replacing of motors battery pack is made easy by
the basic holding connections. The AVR battery was connected on the side of the AVR
with double-sided tape. Because sensors, motors and controller all need different supply
voltage a special supply board, this compact board was connected on the other side of the
AVR. All the mechanics and joints were as basic as possible. The main reason why we
had to keep the mechanics as simple as possible was that there were no mechanical
engineer in our team , so we tried to keep everything as compact as we could
The Picture 3 shows the real output of our mechanical engineering skills, this picture is
not the final look of our robot but it is almost the same.
9 Motors
Servo motors were chosen because they come in full package and it is easy to control
them. The full package means that there is not necessity for extra electronics like Hbridge. By connecting the supply wires and the PWM wire from AVR, there is no more
further connections needed. The easiness behind controlling servos movements is quite
basic. The motors are controlled by the changing PWM. The servo motor has some
control circuit and a potentiometer that is connected to the output shaft. The
potentiometer gives information about the current angle of the motor. If the motor is on
the needed angle then the motors shuts off. But when the angle is not right then the
control circuit commands motor to turn as long as the right angle is achieved. The angle
which has to achieve is given by the PWM from AVR. The servo expects to see a pulse
every 20 millisecond. The length of the pulse determines how far the motor has to turn. A
1.5 millisecond pulse means that motor wants to achieve 90 degree. If the pulse is shorter
than 1.5 millisecond then the motor has to move to 0-90 degrees, if pulse is longer than
1.5 the achievable angle will be between 90-180 degrees. But the problem is that the
standard servo is not meant to use to make a full turn, so it would not have any use in our
project, if it wouldn’t be hackable. The idea behind hacking servo is to make the
potentiometer to be always with one position so the control circuit would think that right
angle is not achieved and so the motor goes around. The servos potentiometers were
replaced with voltage divider (two same resistors, this means that the achievable angle is
90 degrees), so the control circuit thinks that 90 degree is not achieved yet, so it
commands the motor to go around. Also a tab on the gear surface was removed to make
possible for gear to make a full turn. By hacking we can use servos like usual DC motor,
but with the difference that the motor is controlled by the PWM.
9.1 Servo parameters
To find out the duty cycle for stop, there were several attempts executed.
RIGHT_SERVO : it was found out that motor stopped when OCR1A was set to 2484.
LEFT_SERVO : it was found out that motor stopped when OCR1B was set to 2510.
12
9.2 Motor configuration
Idea was to find out timeouts, how long robot has to turn to get 180 degrees. Another
target was to find out the motors speed.
It is possible to control the robot using HyperTerminal on PC. Feedback can be read from
the HyperTerminal. I sent the command (TURN_RIGHT) to show the time to USART
and start turning. When I sent command (STOP) then another timeout was shown on
USART and robot stops. Time between these two events shows us the required time for
180 degrees turn.
'''Measure, HOW fast robot turns.'''
1) Press Forward ('a') - TIMER = 241,
2) Press STOP ('x') - TIMER = 241,
241
639
This means that 398 units. The angle was 180 degrees.
'''Measure, HOW fast robot moves.'''
1) Press Forward ('w') - TIMER =
2) Press STOP ('x') - TIMER =
6,
12,
853 - 00:00:00 (mm:ss:msec)
122 - 00:23:20 (mm:ss:msec)
This means that 6853 units = 00:00:00 and 12122 units = '''00:23:20'''. The length of the
road was 281,5 cm. This means that robot moves 0,12 m/sec.
10 Sensors
The final version of our robot uses 4 types of sensors.
1)IR-distance sensors: The plan was to use IR-distance sensors for detecting how far the
robot is from the side walls. With the help of metallic strips two IR Sharp Long Distance
GP2Y0A02YK (Picture 4-1) were mounted on the both sides of robot. These IR distance
sensors consist of IR transmitter and IR receiver. The transmitter sends all the time out an
IR beam which reflects from the walls, and the reflected IR is received by the receiver.
The length between the wall and receiver is detected by the reflected beams angle(Picture
4-2).. The less the angle is the nearer the wall is and vice versa. The output of the sensors
read by the A/D input of AVR and we can detect how far the wall is. These sensors give
out creditable results between 20 and 150cm. These sensors were very useful until the
robot was so far that we could put it in the maze, the problem was that in some corridors
the walls were too close (less than 20cm), and so the output of a sensor is not accurate.
Also we managed on the last week to supply one of the sensors with reverse voltage and
after that the sensor was out of order. Fortunately we managed to get new Sharp IR
sensor GP2D12(Picture 4-3) which saved our both problems, because this sensor range is
between 10- 80cm, so we could even move in this narrow corridor without any big
problems. The GP2D12 works on the same principle only in other distances.
13
Picture 4. All the sensors
2)Ultrasonic sensors: The plan was to use ultrasonic sensor in the front of the robot, so it
detects how far it is from the front wall. We had two ultrasonic sensors to choose from.
One was Devantech SFR04 and the other SRF08. The difference between them is that
SRF08(Picture 4-4) works on IIC protocol when communicating with AVR and also it
has extra photo resistor on it that we could use for candle detection. The detecting of
obstacles on front of the sensors is same on both of these. The sensors consist of
transmitter and receiver, the transmitter sends out ultrasonic wave which bounce back
when it hits an obstacle and the bounced signal is received by the receiver which
measures the time how long took the wave to go from transmitter against obstacle and
there to receiver. By the delay we can calculate the distance. The primal plan was to get
work the IIC sensor but this plan didn’t work on some unknown reasons, the oscilloscope
showed that the sensor worked but the AVR didn’t think so. After weeks of trying to get
14
work IIC we couldn’t so we had to use typical SFR04(Picture 4-5) which just sends out
info to the A/D input of AVR and this worked like charm.
3)Candle detection IR sensors: From doing some look up from google.com we got
information that there are usually two ways for detecting fire (or candle fire). The most
typical is use IR detectors, because fire emits IR light and by detecting it we could find
the candle. Also candle emits some little UV light in very narrow range and there is this
popular Hamamatsu UVtron which should detect fire from 5m away from fire source.
The problem with IR detectors is that there are so many different sources which emits IR
light beside the visible light. So these other sources will disturb our candle detectors. This
problem is removed with UV detectors because there is not much UV sources which
would disturb the candle detectors. But the Uvtron couldn’t fit to our budget which was
fixed by the competition rules, so we had to go for cheaper solution – IR detectors. For
candle detection we decided to use two IR photodiodes (TSL261, Picture 4-7) which
reacts when it “sees” IR light. The competition rules said that the candle fire is height
between 2 and 4cm. We thought that if we want to get less noise to IR diodes we had to
put them on the same height and direct them straight on candle, so they wouldn’t see any
other IR light sources. The IR diodes were mounted at the end of little black plastic pipes
which were pointed so that candle fire is directly on the same line with the pipes.
Theoretically this means that we can detect candle with no problems, because the only
light which comes at the end of pipes is the candle fire, but in reality it didn’t look so. But
generally these IR diodes worked normally and could detect candle somewhere up to
40cm or so.
4)Touch sensors: In theory it should be possibly to use Ultrasonic and IR distance
sensors so, that we should not be afraid that robot hits the wall. But we can’t always trust
them because with some minor mistake in our program code or with supply of sensors,
the sensor could give out not correct information to the AVR and when this happens the
AVR can lead the robot to the walls. Also it is possible that robots goes to this “dark”
corner in room so that the sensors can’t help out the AVR to lead the way out, this is the
one of biggest problems when building robot, because when robot goes to this corner and
never gets out, then everything is ruined and you don’t want to eat at least 4 weeks.
That’s why there are touch sensors which detects any hit with the walls and give this
information to the AVR and then AVR knows that robot should go back or turn away
from the wall. Because our robot moves only forward we used two touch sensors (Picture
4-6), these touch sensors were two micro switches, when the switch is closed then AVR
knows that the robot has hit the wall. The micro switches were connected with the help of
two metal strips so that when robot hits wall with front or sides, then these metal strips
with switches are pushed against the robot and the switch at close state as long as AVR
thinks out how to get out this messy situation. With the help of the touch sensor all the
phobias with being afraid of the “dark” corner should be removed.
15
11 Electronics
Electronics is kept as simple as possible. Robot had also extra 5V regulator, but AVRKit
is bundled with LM1086 which can provide 1,5A. This is more than enough and last
revision of schematic basically consists of power switch and external motor driving
circuit implemented on n-channel power MOSFET. Two different voltage sources are
used:


9V from 9V block
6V from 4 x AA
AVRKit & sensors are powered from 9V source, voltage is regulated down to 5V. Servos
and fan motor are powered directly from 6V source, large capacitance is placed into the
schematic to minimize the effect of inductive loads. D1 across fan motor terminals shortcircuits reversed voltage peaks that may occur when motor is switched off and protects
switching transistor Q1. Transistor Q1 opens when its gate-drain voltage goes high, gate
can be driven directly from ATMega128 UC.
Picture 5. Switching circuit
16
11.1 Connection schematics
Illustration describes robot's functional blocks. As one can see, almost all inputs and
actuators are directly connected to ATMega128 controller, only exception is fan motor,
which needs its own switching circuit to draw needed amount of current from 6V source.
Actually there were two more blocks but they were only used in development process for
debugging: small LCD screen and UART port with TTL to RS-232 conversion. Serial
communication over UART provided us with valuable almost real-time data.
Picture 6. Connection diagramm
11.2 Bill of Materials
Batteries
AA
4
1€
4€
Batteries
9V block
1
2€
2€
Controller
AVRKit+ATMega128 1
30 €
30 €
IR Range detector
OBP
1
10 €
10 €
IR Range detector
SHARP
GP2Y0A21YK
1
10 €
10 €
Sonar
SRF04
1
15 €
15 €
Servo
Robbe
2
12 €
24 €
IR Detector
TSL261
2
7€
14 €
Acryl
.
~40cm2
1€
1€
10 €
10 €
Wire, solder, different recycled components
.
and connectors, screws etc
.
Total 120 €
17
12 Programming
12.1 Programming environment
During the project there was professional integrated development environment “AVR
Studio 4” used for programming the robot. For compiling the SW there was WinAVR
suite used. It is open source development tool for Atmel AVR series of RISC
microprocessors hosted on Windows platform. It includes GCC compiler for C and C++.
For SW debugging “AVR® JTAGICE mkII” emulator was used. For more information
about software development tools see table below.
Table 1 SW development tool web links
AVR Studio
http://www.atmel.com/dyn/products/tools_card.asp?tool_id=2725
WinAVR suite
http://winavr.sourceforge.net/WinAVR-user-manual.html
AVR® JTAGICE
mkII
http://www.atmel.com/dyn/products/tools_card.asp?tool_id=3353
12.2 Algorithm
12.2.1
Moving
In programming algorithm our team has decided that robot moves mainly along right
wall. The maze was divided into different areas to make it easier to know where the robot
is and where it is heading. There are also cardinal points used. Figure 2 gives overview of
the marking. This kind of marking gives more information about current location.
In normal moving along right wall the transition decisions are made by the following
table:
Table 2 Transition decisions
Nr.
Old status
Transition condition
Action
New status
1
1 / NORD
Peak in left IR dist.
Move forward
4 / NORD
4 / NORD
Peak in right IR dist.
Turn right and move
forward
9 / OST
9 / OST
Peak in right IR dist.
Move forward
8 / OST
8 / OST
Peak in right IR dist.
Turn right and move
forward
7 / OST
5
7 / SYD
Peak in right IR dist.
Look for candle
6 / SYD
6
6 / NORD
Ultrasonic < 150.
Turn left and move
7 / WEST
2
3
4
18
Nr.
Old status
Transition condition
Action
New status
forward
7
8
9
10
11
12
13
14
15
16
17
7 / WEST
Peak in right IR dist.
Turn right and move
forward
9 / NORD
9 / NORD
Peak in right IR dist.
Move forward
14 / NORD
14 / NORD
Peak in right IR dist.
Turn right and move
forward
16 / OST
16 / NORD
Peak in right IR dist.
Look for candle
13 / OST
13 / WEST
Ultrasonic < 150.
Turn left and move
forward
16 / SYD
16 / SYD
Peak in right IR dist.
Turn right and move
forward
14 / WEST
14 / WEST
Peak in left IR dist.
Look for candle
15 / WEST
15 / OST
Peak in right IR dist.
Turn right and move
forward
14 / SYD
14 / SYD
Ultrasonic < 3730
(130 cm)
Turn right and move
forward
9 / WEST
9 / WEST
Peak in right IR dist.
Turn left and move
forward
10 / SYD
10 / SYD
Peak in left IR dist.
Look for candle
17 / SYD
19
1.
5.
4.
3.
6.
2.
7.
8.
17.
15.
16.
14.
12.
10.
13.
9.
11.
Figure 2 Maze with divided areas and cardinal points
When it happens that touch sensors will indicate contact, robot moves some cm-s back
and turns 15 degrees away from the wall.
12.2.2
Finding the candle
In rooms, where the candle might me found (rooms 6,13,15,17) the robot moves along
right wall. When reaching a corner the robot takes 180 degree turn left and 90 degree turn
right to have a look over the room from current corner.
Candle detected signal is generated when big difference is found between two IR photo
diodes. AS IR photo diodes are always registering the picture of the room the signal of
candle detection is generated right away. After the signal has been generated, the double
check for the candle is executed by repeating the turns. When candle is detected second
time the robot stops turning immediately after the event and starts moving forward. To
prevent contact with the candle the signal from photo diode is measured. When it gets
bigger than 300 (last 8 measurement are bigger than 300) the candle is considered to
close enough to blow it away.
20
12.2.3
Blowing the candle
Ventilator is connected to the back of the candle. There for after reaching to the candle
the robot must turn 180 degrees to blow the candle off. Ventilator is activated, before the
turning has been started. When the ventilator has been directed to the candle, then the
robot turns slowly 45 degrees to left and then 45 degrees to right from the center. Slow
turns are made to make it sure that candle is blown off.
After the candle has been blown off, robot returns to the corner it came from and
continues searching for the candle.
12.3 Code
Programming code is divided into different modules given in the table below:
Table 3 programming code modules
ADC.c
Device driver for ATMEGA 128 port F for measuring ADC inputs
interrupts.c
Module for handling interrupts from touch sensors.
lcd.c
Device driver for LCD, which has been used for debugging.
LED.c
Module for handling LED blinking on the AVR kit.
motors.c
Device driver for motors, which mainly change PWM signal duty cycle.
move.c
Module that makes possible to move the robot with high level
commands.
Move along right wall, etc.
sam.c
Main module that controls the robot. Includes while(1) main loop.
timers.c
Module for counting time. It is used for making 90 degree turns and etc.
It can also call functions, generate events after expected timeout. For
example, measuring of analog inputs is always started by the timer.
UH.c
Module that handles Ultrasonic distance sensor measurements.
UH_TWI.c
Device driver for ultrasonic distance sensor, connected to TWI interface.
Note that this sensor is not used in final SW.
USART.c
Device driver for RS-232 interface. This has been used for debugging
the SW.
ADC.h
Header file for ADC.c
defines.h
Includes different defines.
interrupts.h
Header file for interrupts.c.
lcd.h
Header file for lcd.c.
21
LED.h
Header file for LED.c.
macros.h
Includes different macros.
motors.h
Header file for motors.c.
move.h
Header file for move.c.
timers.h
Header file for timers.c.
UH.h
Header file for UH.c.
UH_TWI.h
Header file for UH_TWI.c.
USART.h
Header file for USART.c..
ver.h
Header file for version management.
Code for TWI, USART and LCD modules has not been written by creator of the robots.
It has been found from internet.
LCD.c
http://jump.to/fleury
USART.c
http://www.raphnet.net/divers/anemometre/windmon_avr/usart.c
UH_TWI.c
http://wcvs.uniandes.edu.co/lib/Wire/utility/twi.c?rev=1.3&sortby=log&
content-type=text/vnd.viewcvs-markup
For source code see “Appendix 1: Source code”.
13 Conclusions
13.1.1
Conclusion of Märt
In the beginning, creating the firefighter seemed to be very easy task. Soon it became a
difficult one as robot design is not that easy task. Limited sensors and computing power
is not enough to compete the human. We could also beat robots on teamwork but as we
can see from the result that was not enough.
13.1.2
Conclusion of Margus
This project was really time-consuming, I think none of us imagined that we spend about
2-3 evenings in the lab every week. At the beginning the task seemed almost easy, as
time passed, all those little details, things that never should have occurred, problems
caused by surrounding environment just kept coming. Almost every decision we made
seemed also have a side effect. But I still feel that this project was most educational with
all those little and not so little drawbacks. Perfectly managed and planned project with
perfect implementation would have been pleasing too, but then we would have never
22
actually known how much trouble we can get into when not doing so. I'm sure my next
such projects are build with much more confidence and with much more planning.
13.1.3
Conclusion of Sulev
The development of a robot was a challenging project, at the beginning we knew that in
December we will have a working robot, but then we didn't yet know how we will make
it. It was very interesting to be involved in this project from first steps till the end. The
main problems which interrupted our effective work were some minor details which had
to be always fixed, like somewhere was a connection wire opened, or some mechanical
part was little bit unstable or the robot act strange in the maze because of a bug in the
program code. All these minor problems took too much time, so the final output was not
so satisfying. We had all these ideas which would made the robot perfect, but we didn't
have the time to use them because we had to deal with the details. I think the dealing with
details most of the time will go away with experiences, and then we can concentrate on
things that are really important.
14 References



Atmega 128 datasheet - http://www.atmel.com/atmel/acrobat/doc2467.pdf
“What is Servo?” - http://www.seattlerobotics.org/guide/servos.html
“Hacking a Servo” - http://www.seattlerobotics.org/guide/servohack.html
23
Appendix 1: Source code
ADC.c
#include
#include
#include
#include
#include
<inttypes.h>
<stdio.h>
<string.h>
<avr/io.h>
<util/delay.h>
#include "inc/ADC.h"
#define ADC_BUFFER_LEN
8 // should be even number due to ADC_get_peak()
#define CNT_OF_ADC_BUFFERS 4
// used for recording measurements
uint16_t channel_buffer[CNT_OF_ADC_BUFFERS][ADC_BUFFER_LEN];
// used for recording average measurements
uint16_t avg_channel_buffer[CNT_OF_ADC_BUFFERS][ADC_BUFFER_LEN];
void init_ADC(void)
{
// must clear all buffer at startup
memset(channel_buffer[0],0x00,ADC_BUFFER_LEN);
memset(channel_buffer[1],0x00,ADC_BUFFER_LEN); // Right dist sensor buffer
memset(channel_buffer[2],0x00,ADC_BUFFER_LEN);
memset(channel_buffer[3],0x00,ADC_BUFFER_LEN);
memset(avg_channel_buffer[0],0x00,ADC_BUFFER_LEN);
memset(avg_channel_buffer[1],0x00,ADC_BUFFER_LEN); // Right dist sensor buffer
memset(avg_channel_buffer[2],0x00,ADC_BUFFER_LEN);
memset(avg_channel_buffer[3],0x00,ADC_BUFFER_LEN);
}
// This function returns 0x02 when dirft was to right and 0x01 when drift was to left.
char get_right_sensor_drift()
{
char return_val;
//Dift is to left
if(channel_buffer[RIGHT_DIST_SENSOR][ADC_BUFFER_LEN-1] <
channel_buffer[RIGHT_DIST_SENSOR][0])
return_val = 0x01;
else
return_val = 0x02;
return return_val;
}
uint16_t get_last_measure_value(ADC_CHANNEL_NAME channel_name)
{
return channel_buffer[channel_name][ADC_BUFFER_LEN-1];
}
void push_ADC_value(ADC_CHANNEL_LIST channel, uint16_t value)
{
int i;
int average=0;
for(i=0;i<(ADC_BUFFER_LEN-1);i++) // -1 as last value is taken from input
{
channel_buffer[channel][i]=channel_buffer[channel][i+1];
avg_channel_buffer[channel][i]=avg_channel_buffer[channel][i+1];
average = (average*i + channel_buffer[channel][i])/(i+1);
}
24
channel_buffer[channel][ADC_BUFFER_LEN-1] = value;
average = (average*i + channel_buffer[channel][ADC_BUFFER_LEN-1])/(ADC_BUFFER_LEN);
avg_channel_buffer[channel][ADC_BUFFER_LEN-1] = average;
}
void measure_ADC_values(void)
{
int i;
for(i=0;i<CNT_OF_ADC_BUFFERS;i++)
{
push_ADC_value(i,ReadChannel(i));
}
}
uint16_t ReadLastMeasurement(uint8_t mux)
{
return channel_buffer[mux][ADC_BUFFER_LEN-1];
}
uint16_t ReadLastAvgMeasurement(uint8_t mux)
{
return avg_channel_buffer[mux][ADC_BUFFER_LEN-1];
}
//If last 4 measure avg is much more different last 5-8 then there is peak
char ADC_get_peak(ADC_CHANNEL_NAME selected_channel)
{
int i = 0;
int avg_frst_four = 0;
int avg_other_four = 0;
for(i = 0; i < ADC_BUFFER_LEN; i++)
{
if (channel_buffer[selected_channel][i] == 0x00)
return 0;
}
// calculate these two averageses
for(i = 0; i < ADC_BUFFER_LEN; i++)
{
if(i<ADC_BUFFER_LEN/2)
avg_frst_four = (avg_frst_four*i + channel_buffer[selected_channel][i])/(i+1);
else
avg_other_four = (avg_other_four*(i-ADC_BUFFER_LEN/2) +
channel_buffer[selected_channel][i])/((i-ADC_BUFFER_LEN/2)+1);
}
if(abs(avg_frst_four - avg_other_four) > 100)
return 1;
else
return 0;
}
uint16_t ReadChannel(uint8_t mux)
{
uint8_t i;
uint16_t result = 0;
ADCSRA = (1 << ADEN) | (1<<ADPS1) | (1<<ADPS0); // Prescaler 8(1) ja ADC aktiveerimine
ADMUX = mux; // kanali valik
ADMUX |= (1<<REFS1) | (1<<REFS0); //Valitakse sisemine referentspinge 2,56V
ADCSRA |= (1<<ADSC);
//AD koncerteerimine
while( ADCSRA & (1<<ADSC)){;}
//oodatakse kuni konverteerimine lõppenud
for(i=0;i<4;i++) // tegelik mõõtmine - teostatakse neli järjestikust mõõtmist
{
ADCSRA |= (1<<ADSC);
while( ADCSRA & (1<<ADSC)){;}
25
result += ADCW;
}
ADCSRA &= ~(1<<ADSC);
//ADC deaktiveerimine
return result /=4;
}
ADC.h
#ifndef _ADC_H_
#define _ADC_H_
typedef enum
{
CHANNEL_0,
CHANNEL_1,
CHANNEL_2,
CHANNEL_3
}ADC_CHANNEL_LIST;
typedef enum
{
LEFT_DIST_SENSOR,
RIGHT_DIST_SENSOR,
LEFT_CNDL_SENSOR,
RIGHT_CNDL_SENSOR
}ADC_CHANNEL_NAME;
uint16_t ReadLastMeasurement(uint8_t mux);
uint16_t ReadLastAvgMeasurement(uint8_t mux);
char ADC_get_peak(ADC_CHANNEL_NAME selected_channel);
uint16_t ReadChannel(uint8_t mux);
void init_ADC(void);
void measure_ADC_values(void);
uint16_t get_last_measure_value(ADC_CHANNEL_NAME channel_name);
#endif// _ADC_H_
26
interrupts.c
#include <avr/io.h>
#include <avr/interrupt.h>
#include "inc/macros.h"
#include "inc/interrupts.h"
ISR(INT7_vect)
{
static double cnt = 0;
cnt++;
PORTE INV_B(6);
}
void en_external_int(void)
{
DDRE SET_B(7); // Set PE7 as output
PORTE SET_B(7); // Activate pull up. (Output goes high)
EIMSK SET_B(7); // Enable INT7 in EIMSK register
EICRB CLR_B(7); // Set that interrupt will happen
EICRB CLR_B(6); // when signal gets low
}
interrupts.h
#ifndef _INT_H_
#define _INT_H_
void en_external_int(void);
#endif// _INT_H_
27
defines.h
/* DEBUG OFF/ON */
#define DEBUG_ON
/* CPU frequency */
// #define F_CPU 14745600L With AVR studio this define is made from project settings
/* UART baud rate */
#define UART_BAUD 9600
/* HD44780 LCD port connections */
#define HD44780_PORT A
#define HD44780_RS PORT6
#define HD44780_RW PORT4
#define HD44780_E PORT5
#define HD44780_D4 PORT0
#define HD44780_D5 PORT1
#define HD44780_D6 PORT2
#define HD44780_D7 PORT3
//#define LCD_IS_USED // LCD OFF/ON
#define LCD_LINE_LEN 20
28
LCD.c
/****************************************************************************
Title :
HD44780U LCD library
Author:
Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury
File:
$Id: lcd.c,v 1.14.2.1 2006/01/29 12:16:41 peter Exp $
Software: AVR-GCC 3.3
Target:
any AVR device, memory mapped mode only for AT90S4414/8515/Mega
DESCRIPTION
Basic routines for interfacing a HD44780U-based text lcd display
Originally based on Volker Oth's lcd library,
changed lcd_init(), added additional constants for lcd_command(),
added 4-bit I/O mode, improved and optimized code.
Library can be operated in memory mapped mode (LCD_IO_MODE=0) or in
4-bit IO port mode (LCD_IO_MODE=1). 8-bit IO port mode not supported.
Memory mapped mode compatible with Kanda STK200, but supports also
generation of R/W signal through A8 address line.
USAGE
See the C include lcd.h file for a description of each function
*****************************************************************************/
#include <inttypes.h>
#include <avr/io.h>
#include <avr/pgmspace.h>
#include "inc/lcd.h"
/*
** constants/macros
*/
#define DDR(x) (*(&x - 1))
/* address of data direction register of port x */
#if defined(__AVR_ATmega64__) || defined(__AVR_ATmega128__)
/* on ATmega64/128 PINF is on port 0x00 and not 0x60 */
#define PIN(x) ( &PORTF==&(x) ? _SFR_IO8(0x00) : (*(&x - 2)) )
#else
#define PIN(x) (*(&x - 2))
/* address of input register of port x
#endif
#if LCD_IO_MODE
#define lcd_e_delay()
#define lcd_e_high()
#define lcd_e_low()
#define lcd_e_toggle()
#define lcd_rw_high()
#define lcd_rw_low()
#define lcd_rs_high()
#define lcd_rs_low()
#endif
*/
__asm__ __volatile__( "rjmp 1f\n 1:" );
LCD_E_PORT |= _BV(LCD_E_PIN);
LCD_E_PORT &= ~_BV(LCD_E_PIN);
toggle_e()
LCD_RW_PORT |= _BV(LCD_RW_PIN)
LCD_RW_PORT &= ~_BV(LCD_RW_PIN)
LCD_RS_PORT |= _BV(LCD_RS_PIN)
LCD_RS_PORT &= ~_BV(LCD_RS_PIN)
#if LCD_IO_MODE
#if LCD_LINES==1
#define LCD_FUNCTION_DEFAULT
#else
#define LCD_FUNCTION_DEFAULT
#endif
#else
#if LCD_LINES==1
#define LCD_FUNCTION_DEFAULT
#else
#define LCD_FUNCTION_DEFAULT
#endif
#endif
LCD_FUNCTION_4BIT_1LINE
LCD_FUNCTION_4BIT_2LINES
LCD_FUNCTION_8BIT_1LINE
LCD_FUNCTION_8BIT_2LINES
29
#if LCD_CONTROLLER_KS0073
#if LCD_LINES==4
#define KS0073_EXTENDED_FUNCTION_REGISTER_ON 0x24
bit RE = 1 */
#define KS0073_EXTENDED_FUNCTION_REGISTER_OFF 0x20
#define KS0073_4LINES_MODE
0x09
extension-bit RE = 0 */
/* |0|010|0100 4-bit mode extension/* |0|000|1001 4 lines mode */
/* |0|001|0000 4-bit mode,
#endif
#endif
/*
** function prototypes
*/
#if LCD_IO_MODE
static void toggle_e(void);
#endif
/*
** local functions
*/
/*************************************************************************
delay loop for small accurate delays: 16-bit counter, 4 cycles/loop
*************************************************************************/
static inline void _delayFourCycles(unsigned int __count)
{
if ( __count == 0 )
__asm__ __volatile__( "rjmp 1f\n 1:" );
// 2 cycles
else
__asm__ __volatile__ (
"1: sbiw %0,1" "\n\t"
"brne 1b"
// 4 cycles/loop
: "=w" (__count)
: "0" (__count)
);
}
/*************************************************************************
delay for a minimum of <us> microseconds
the number of loops is calculated at compile-time from MCU clock frequency
*************************************************************************/
#define delay(us) _delayFourCycles( ( ( 1*(XTAL/4000) )*us)/1000 )
#if LCD_IO_MODE
/* toggle Enable Pin to initiate write */
static void toggle_e(void)
{
lcd_e_high();
lcd_e_delay();
lcd_e_low();
}
#endif
/*************************************************************************
Low-level function to write byte to LCD controller
Input:
data
byte to write to LCD
rs
1: write data
0: write instruction
Returns: none
*************************************************************************/
#if LCD_IO_MODE
static void lcd_write(uint8_t data,uint8_t rs)
{
30
unsigned char dataBits ;
if (rs) {
/* write data
(RS=1, RW=0) */
lcd_rs_high();
} else {
/* write instruction (RS=0, RW=0) */
lcd_rs_low();
}
lcd_rw_low();
if ( ( &LCD_DATA0_PORT == &LCD_DATA1_PORT) && ( &LCD_DATA1_PORT == &LCD_DATA2_PORT )
&& ( &LCD_DATA2_PORT == &LCD_DATA3_PORT )
&& (LCD_DATA0_PIN == 0) && (LCD_DATA1_PIN == 1) && (LCD_DATA2_PIN == 2) &&
(LCD_DATA3_PIN == 3) )
{
/* configure data pins as output */
DDR(LCD_DATA0_PORT) |= 0x0F;
/* output high nibble first */
dataBits = LCD_DATA0_PORT & 0xF0;
LCD_DATA0_PORT = dataBits |((data>>4)&0x0F);
lcd_e_toggle();
/* output low nibble */
LCD_DATA0_PORT = dataBits | (data&0x0F);
lcd_e_toggle();
/* all data pins high (inactive) */
LCD_DATA0_PORT = dataBits | 0x0F;
}
else
{
/* configure data pins
DDR(LCD_DATA0_PORT) |=
DDR(LCD_DATA1_PORT) |=
DDR(LCD_DATA2_PORT) |=
DDR(LCD_DATA3_PORT) |=
as output */
_BV(LCD_DATA0_PIN);
_BV(LCD_DATA1_PIN);
_BV(LCD_DATA2_PIN);
_BV(LCD_DATA3_PIN);
/* output high nibble first */
LCD_DATA3_PORT &= ~_BV(LCD_DATA3_PIN);
LCD_DATA2_PORT &= ~_BV(LCD_DATA2_PIN);
LCD_DATA1_PORT &= ~_BV(LCD_DATA1_PIN);
LCD_DATA0_PORT &= ~_BV(LCD_DATA0_PIN);
if(data & 0x80) LCD_DATA3_PORT |= _BV(LCD_DATA3_PIN);
if(data & 0x40) LCD_DATA2_PORT |= _BV(LCD_DATA2_PIN);
if(data & 0x20) LCD_DATA1_PORT |= _BV(LCD_DATA1_PIN);
if(data & 0x10) LCD_DATA0_PORT |= _BV(LCD_DATA0_PIN);
lcd_e_toggle();
/* output low nibble */
LCD_DATA3_PORT &= ~_BV(LCD_DATA3_PIN);
LCD_DATA2_PORT &= ~_BV(LCD_DATA2_PIN);
LCD_DATA1_PORT &= ~_BV(LCD_DATA1_PIN);
LCD_DATA0_PORT &= ~_BV(LCD_DATA0_PIN);
if(data & 0x08) LCD_DATA3_PORT |= _BV(LCD_DATA3_PIN);
if(data & 0x04) LCD_DATA2_PORT |= _BV(LCD_DATA2_PIN);
if(data & 0x02) LCD_DATA1_PORT |= _BV(LCD_DATA1_PIN);
if(data & 0x01) LCD_DATA0_PORT |= _BV(LCD_DATA0_PIN);
lcd_e_toggle();
/* all data pins high (inactive) */
LCD_DATA0_PORT |= _BV(LCD_DATA0_PIN);
LCD_DATA1_PORT |= _BV(LCD_DATA1_PIN);
LCD_DATA2_PORT |= _BV(LCD_DATA2_PIN);
LCD_DATA3_PORT |= _BV(LCD_DATA3_PIN);
}
}
#else
#define lcd_write(d,rs) if (rs) *(volatile uint8_t*)(LCD_IO_DATA) = d; else *(volatile
uint8_t*)(LCD_IO_FUNCTION) = d;
/* rs==0 -> write instruction to LCD_IO_FUNCTION */
31
/* rs==1 -> write data to LCD_IO_DATA */
#endif
/*************************************************************************
Low-level function to read byte from LCD controller
Input:
rs
1: read data
0: read busy flag / address counter
Returns: byte read from LCD controller
*************************************************************************/
#if LCD_IO_MODE
static uint8_t lcd_read(uint8_t rs)
{
uint8_t data;
if (rs)
lcd_rs_high();
else
lcd_rs_low();
lcd_rw_high();
/* RS=1: read data
*/
/* RS=0: read busy flag */
/* RW=1 read mode
*/
if ( ( &LCD_DATA0_PORT == &LCD_DATA1_PORT) && ( &LCD_DATA1_PORT == &LCD_DATA2_PORT )
&& ( &LCD_DATA2_PORT == &LCD_DATA3_PORT )
&& ( LCD_DATA0_PIN == 0 )&& (LCD_DATA1_PIN == 1) && (LCD_DATA2_PIN == 2) &&
(LCD_DATA3_PIN == 3) )
{
DDR(LCD_DATA0_PORT) &= 0xF0;
/* configure data pins as input */
lcd_e_high();
lcd_e_delay();
data = PIN(LCD_DATA0_PORT) << 4;
lcd_e_low();
lcd_e_delay();
lcd_e_high();
lcd_e_delay();
data |= PIN(LCD_DATA0_PORT)&0x0F;
lcd_e_low();
}
else
{
/* configure data pins
DDR(LCD_DATA0_PORT) &=
DDR(LCD_DATA1_PORT) &=
DDR(LCD_DATA2_PORT) &=
DDR(LCD_DATA3_PORT) &=
/* read high nibble first */
/* Enable 500ns low
*/
/* read low nibble
*/
as input */
~_BV(LCD_DATA0_PIN);
~_BV(LCD_DATA1_PIN);
~_BV(LCD_DATA2_PIN);
~_BV(LCD_DATA3_PIN);
/* read high nibble first */
lcd_e_high();
lcd_e_delay();
data = 0;
if ( PIN(LCD_DATA0_PORT) & _BV(LCD_DATA0_PIN)
if ( PIN(LCD_DATA1_PORT) & _BV(LCD_DATA1_PIN)
if ( PIN(LCD_DATA2_PORT) & _BV(LCD_DATA2_PIN)
if ( PIN(LCD_DATA3_PORT) & _BV(LCD_DATA3_PIN)
lcd_e_low();
lcd_e_delay();
/* read low nibble */
lcd_e_high();
lcd_e_delay();
if ( PIN(LCD_DATA0_PORT)
if ( PIN(LCD_DATA1_PORT)
if ( PIN(LCD_DATA2_PORT)
if ( PIN(LCD_DATA3_PORT)
lcd_e_low();
)
)
)
)
data
data
data
data
|=
|=
|=
|=
0x10;
0x20;
0x40;
0x80;
/* Enable 500ns low
&
&
&
&
_BV(LCD_DATA0_PIN)
_BV(LCD_DATA1_PIN)
_BV(LCD_DATA2_PIN)
_BV(LCD_DATA3_PIN)
)
)
)
)
data
data
data
data
|=
|=
|=
|=
*/
0x01;
0x02;
0x04;
0x08;
}
return data;
32
}
#else
#define lcd_read(rs) (rs) ? *(volatile uint8_t*)(LCD_IO_DATA+LCD_IO_READ) : *(volatile
uint8_t*)(LCD_IO_FUNCTION+LCD_IO_READ)
/* rs==0 -> read instruction from LCD_IO_FUNCTION */
/* rs==1 -> read data from LCD_IO_DATA */
#endif
/*************************************************************************
loops while lcd is busy, returns address counter
*************************************************************************/
static uint8_t lcd_waitbusy(void)
{
register uint8_t c;
/* wait until busy flag is cleared */
while ( (c=lcd_read(0)) & (1<<LCD_BUSY)) {}
/* the address counter is updated 4us after the busy flag is cleared */
delay(2);
/* now read the address counter */
return (lcd_read(0)); // return address counter
}/* lcd_waitbusy */
/*************************************************************************
Move cursor to the start of next line or to the first line if the cursor
is already on the last line.
*************************************************************************/
static inline void lcd_newline(uint8_t pos)
{
register uint8_t addressCounter;
#if LCD_LINES==1
addressCounter = 0;
#endif
#if LCD_LINES==2
if ( pos < (LCD_START_LINE2) )
addressCounter = LCD_START_LINE2;
else
addressCounter = LCD_START_LINE1;
#endif
#if LCD_LINES==4
#if KS0073_4LINES_MODE
if ( pos < LCD_START_LINE2 )
addressCounter = LCD_START_LINE2;
else if ( (pos >= LCD_START_LINE2) && (pos <
addressCounter = LCD_START_LINE3;
else if ( (pos >= LCD_START_LINE3) && (pos <
addressCounter = LCD_START_LINE4;
else
addressCounter = LCD_START_LINE1;
#else
if ( pos < LCD_START_LINE3 )
addressCounter = LCD_START_LINE2;
else if ( (pos >= LCD_START_LINE2) && (pos <
addressCounter = LCD_START_LINE3;
else if ( (pos >= LCD_START_LINE3) && (pos <
addressCounter = LCD_START_LINE4;
else
addressCounter = LCD_START_LINE1;
#endif
#endif
lcd_command((1<<LCD_DDRAM)+addressCounter);
LCD_START_LINE3) )
LCD_START_LINE4) )
LCD_START_LINE4) )
LCD_START_LINE2) )
}/* lcd_newline */
33
/*
** PUBLIC FUNCTIONS
*/
/*************************************************************************
Send LCD controller instruction command
Input:
instruction to send to LCD controller, see HD44780 data sheet
Returns: none
*************************************************************************/
void lcd_command(uint8_t cmd)
{
lcd_waitbusy();
lcd_write(cmd,0);
}
/*************************************************************************
Send data byte to LCD controller
Input:
data to send to LCD controller, see HD44780 data sheet
Returns: none
*************************************************************************/
void lcd_data(uint8_t data)
{
lcd_waitbusy();
lcd_write(data,1);
}
/*************************************************************************
Set cursor to specified position
Input:
x horizontal position (0: left most position)
y vertical position
(0: first line)
Returns: none
*************************************************************************/
void lcd_gotoxy(uint8_t x, uint8_t y)
{
#if LCD_LINES==1
lcd_command((1<<LCD_DDRAM)+LCD_START_LINE1+x);
#endif
#if LCD_LINES==2
if ( y==0 )
lcd_command((1<<LCD_DDRAM)+LCD_START_LINE1+x);
else
lcd_command((1<<LCD_DDRAM)+LCD_START_LINE2+x);
#endif
#if LCD_LINES==4
if ( y==0 )
lcd_command((1<<LCD_DDRAM)+LCD_START_LINE1+x);
else if ( y==1)
lcd_command((1<<LCD_DDRAM)+LCD_START_LINE2+x);
else if ( y==2)
lcd_command((1<<LCD_DDRAM)+LCD_START_LINE3+x);
else /* y==3 */
lcd_command((1<<LCD_DDRAM)+LCD_START_LINE4+x);
#endif
}/* lcd_gotoxy */
/*************************************************************************
*************************************************************************/
int lcd_getxy(void)
{
return lcd_waitbusy();
}
/*************************************************************************
34
Clear display and set cursor to home position
*************************************************************************/
void lcd_clrscr(void)
{
lcd_command(1<<LCD_CLR);
}
/*************************************************************************
Set cursor to home position
*************************************************************************/
void lcd_home(void)
{
lcd_command(1<<LCD_HOME);
}
/*************************************************************************
Display character at current cursor position
Input:
character to be displayed
Returns: none
*************************************************************************/
void lcd_putc(char c)
{
uint8_t pos;
pos = lcd_waitbusy();
// read busy-flag and address
if (c=='\n')
{
lcd_newline(pos);
}
else
{
#if LCD_WRAP_LINES==1
#if LCD_LINES==1
if ( pos == LCD_START_LINE1+LCD_DISP_LENGTH ) {
lcd_write((1<<LCD_DDRAM)+LCD_START_LINE1,0);
}
#elif LCD_LINES==2
if ( pos == LCD_START_LINE1+LCD_DISP_LENGTH ) {
lcd_write((1<<LCD_DDRAM)+LCD_START_LINE2,0);
}else if ( pos == LCD_START_LINE2+LCD_DISP_LENGTH
lcd_write((1<<LCD_DDRAM)+LCD_START_LINE1,0);
}
#elif LCD_LINES==4
if ( pos == LCD_START_LINE1+LCD_DISP_LENGTH ) {
lcd_write((1<<LCD_DDRAM)+LCD_START_LINE2,0);
}else if ( pos == LCD_START_LINE2+LCD_DISP_LENGTH
lcd_write((1<<LCD_DDRAM)+LCD_START_LINE3,0);
}else if ( pos == LCD_START_LINE3+LCD_DISP_LENGTH
lcd_write((1<<LCD_DDRAM)+LCD_START_LINE4,0);
}else if ( pos == LCD_START_LINE4+LCD_DISP_LENGTH
lcd_write((1<<LCD_DDRAM)+LCD_START_LINE1,0);
}
#endif
lcd_waitbusy();
#endif
lcd_write(c, 1);
}
counter
){
) {
) {
) {
}/* lcd_putc */
/*************************************************************************
Display string without auto linefeed
Input:
string to be displayed
Returns: none
*************************************************************************/
void lcd_puts(const char *s)
/* print string on lcd (no auto linefeed) */
35
{
register char c;
while ( (c = *s++) ) {
lcd_putc(c);
}
}/* lcd_puts */
/*************************************************************************
Display string from program memory without auto linefeed
Input:
string from program memory be be displayed
Returns:
none
*************************************************************************/
void lcd_puts_p(const char *progmem_s)
/* print string from program memory on lcd (no auto linefeed) */
{
register char c;
while ( (c = pgm_read_byte(progmem_s++)) ) {
lcd_putc(c);
}
}/* lcd_puts_p */
/*************************************************************************
Initialize display and select type of cursor
Input:
dispAttr LCD_DISP_OFF
display off
LCD_DISP_ON
display on, cursor off
LCD_DISP_ON_CURSOR
display on, cursor on
LCD_DISP_CURSOR_BLINK
display on, cursor on flashing
Returns: none
*************************************************************************/
void lcd_init(uint8_t dispAttr)
{
#if LCD_IO_MODE
/*
* Initialize LCD to 4 bit I/O mode
*/
if ( ( &LCD_DATA0_PORT == &LCD_DATA1_PORT) && ( &LCD_DATA1_PORT == &LCD_DATA2_PORT )
&& ( &LCD_DATA2_PORT == &LCD_DATA3_PORT )
&& ( &LCD_RS_PORT == &LCD_DATA0_PORT) && ( &LCD_RW_PORT == &LCD_DATA0_PORT) &&
(&LCD_E_PORT == &LCD_DATA0_PORT)
&& (LCD_DATA0_PIN == 0 ) && (LCD_DATA1_PIN == 1) && (LCD_DATA2_PIN == 2) &&
(LCD_DATA3_PIN == 3)
&& (LCD_RS_PIN == 4 ) && (LCD_RW_PIN == 5) && (LCD_E_PIN == 6 ) )
{
/* configure all port bits as output (all LCD lines on same port) */
DDR(LCD_DATA0_PORT) |= 0x7F;
}
else if ( ( &LCD_DATA0_PORT == &LCD_DATA1_PORT) && ( &LCD_DATA1_PORT ==
&LCD_DATA2_PORT ) && ( &LCD_DATA2_PORT == &LCD_DATA3_PORT )
&& (LCD_DATA0_PIN == 0 ) && (LCD_DATA1_PIN == 1) && (LCD_DATA2_PIN == 2) &&
(LCD_DATA3_PIN == 3) )
{
/* configure all port bits as output (all LCD data lines on same port, but
control lines on different ports) */
DDR(LCD_DATA0_PORT) |= 0x0F;
DDR(LCD_RS_PORT)
|= _BV(LCD_RS_PIN);
DDR(LCD_RW_PORT)
|= _BV(LCD_RW_PIN);
DDR(LCD_E_PORT)
|= _BV(LCD_E_PIN);
}
else
{
/* configure all port bits as output (LCD data and control lines on different
ports */
DDR(LCD_RS_PORT)
|= _BV(LCD_RS_PIN);
DDR(LCD_RW_PORT)
|= _BV(LCD_RW_PIN);
36
DDR(LCD_E_PORT)
DDR(LCD_DATA0_PORT)
DDR(LCD_DATA1_PORT)
DDR(LCD_DATA2_PORT)
DDR(LCD_DATA3_PORT)
}
delay(16000);
|=
|=
|=
|=
|=
_BV(LCD_E_PIN);
_BV(LCD_DATA0_PIN);
_BV(LCD_DATA1_PIN);
_BV(LCD_DATA2_PIN);
_BV(LCD_DATA3_PIN);
/* wait 16ms or more after power-on
*/
/* initial write to lcd is 8bit */
LCD_DATA1_PORT |= _BV(LCD_DATA1_PIN); // _BV(LCD_FUNCTION)>>4;
LCD_DATA0_PORT |= _BV(LCD_DATA0_PIN); // _BV(LCD_FUNCTION_8BIT)>>4;
lcd_e_toggle();
delay(4992);
/* delay, busy flag can't be checked here */
/* repeat last command */
lcd_e_toggle();
delay(64);
/* delay, busy flag can't be checked here */
/* repeat last command a third time */
lcd_e_toggle();
delay(64);
/* delay, busy flag can't be checked here */
/* now configure for 4bit mode */
LCD_DATA0_PORT &= ~_BV(LCD_DATA0_PIN);
// LCD_FUNCTION_4BIT_1LINE>>4
lcd_e_toggle();
delay(64);
/* some displays need this additional delay */
/* from now the LCD only accepts 4 bit I/O, we can use lcd_command() */
#else
/*
* Initialize LCD to 8 bit memory mapped mode
*/
/* enable external SRAM (memory mapped lcd) and one wait state */
MCUCR = _BV(SRE) | _BV(SRW);
/* reset LCD */
delay(16000);
lcd_write(LCD_FUNCTION_8BIT_1LINE,0);
delay(4992);
lcd_write(LCD_FUNCTION_8BIT_1LINE,0);
delay(64);
lcd_write(LCD_FUNCTION_8BIT_1LINE,0);
delay(64);
#endif
/*
/*
/*
/*
/*
/*
/*
wait 16ms after power-on
function set: 8bit interface
wait 5ms
function set: 8bit interface
wait 64us
function set: 8bit interface
wait 64us
*/
*/
*/
*/
*/
*/
*/
#if KS0073_4LINES_MODE
/* Display with KS0073 controller requires special commands for enabling 4 line mode
*/
lcd_command(KS0073_EXTENDED_FUNCTION_REGISTER_ON);
lcd_command(KS0073_4LINES_MODE);
lcd_command(KS0073_EXTENDED_FUNCTION_REGISTER_OFF);
#else
lcd_command(LCD_FUNCTION_DEFAULT);
/* function set: display lines */
#endif
lcd_command(LCD_DISP_OFF);
/* display off
*/
lcd_clrscr();
/* display clear
*/
lcd_command(LCD_MODE_DEFAULT);
/* set entry mode
*/
lcd_command(dispAttr);
/* display/cursor control
*/
}/* lcd_init */
37
LCD.h
#ifndef _LCD_H_
#define _LCD_H_
/*************************************************************************
Title :
C include file for the HD44780U LCD library (lcd.c)
Author:
Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury
File:
$Id: lcd.h,v 1.13.2.2 2006/01/30 19:51:33 peter Exp $
Software: AVR-GCC 3.3
Hardware: any AVR device, memory mapped mode only for AT90S4414/8515/Mega
***************************************************************************/
/**
@defgroup pfleury_lcd LCD library
@code #include <lcd.h> @endcode
@brief Basic routines for interfacing a HD44780U-based text LCD display
Originally based on Volker Oth's LCD library,
changed lcd_init(), added additional constants for lcd_command(),
added 4-bit I/O mode, improved and optimized code.
Library can be operated in memory mapped mode (LCD_IO_MODE=0) or in
4-bit IO port mode (LCD_IO_MODE=1). 8-bit IO port mode not supported.
Memory mapped mode compatible with Kanda STK200, but supports also
generation of R/W signal through A8 address line.
@author Peter Fleury pfleury@gmx.ch http://jump.to/fleury
@see The chapter <a href="http://homepage.sunrise.ch/mysunrise/peterfleury/avrlcd44780.html" target="_blank">Interfacing a HD44780 Based LCD to an AVR</a>
on my home page.
*/
/*@{*/
#if (__GNUC__ * 100 + __GNUC_MINOR__) < 303
#error "This library requires AVR-GCC 3.3 or later, update to newer AVR-GCC compiler !"
#endif
#include <inttypes.h>
#include <avr/pgmspace.h>
/**
* @name Definitions for MCU Clock Frequency
* Adapt the MCU clock frequency in Hz to your target.
*/
#define XTAL 4000000
/**< clock frequency in Hz, used to calculate delay
timer */
/**
* @name Definition for LCD controller type
* Use 0 for HD44780 controller, change to 1 for displays with KS0073 controller.
*/
#define LCD_CONTROLLER_KS0073 0 /**< Use 0 for HD44780 controller, 1 for KS0073
controller */
/**
* @name Definitions for Display Size
* Change these definitions to adapt setting to your display
*/
#define LCD_LINES
2
/**< number of visible lines of the display */
#define LCD_DISP_LENGTH
16
/**< visibles characters per line of the display */
#define LCD_LINE_LENGTH 0x40
/**< internal line length of the display
*/
#define LCD_START_LINE1 0x00
/**< DDRAM address of first char of line 1 */
#define LCD_START_LINE2 0x40
/**< DDRAM address of first char of line 2 */
#define LCD_START_LINE3 0x14
/**< DDRAM address of first char of line 3 */
#define LCD_START_LINE4 0x54
/**< DDRAM address of first char of line 4 */
38
#define LCD_WRAP_LINES
0
/**< 0: no wrap, 1: wrap at end of visibile line */
#define LCD_IO_MODE
1
/**< 0: memory mapped mode, 1: IO port mode */
#if LCD_IO_MODE
/**
* @name Definitions for 4-bit IO mode
* Change LCD_PORT if you want to use a different port for the LCD pins.
*
* The four LCD data lines and the three control lines RS, RW, E can be on the
* same port or on different ports.
* Change LCD_RS_PORT, LCD_RW_PORT, LCD_E_PORT if you want the control lines on
* different ports.
*
* Normally the four data lines should be mapped to bit 0..3 on one port, but it
* is possible to connect these data lines in different order or even on different
* ports by adapting the LCD_DATAx_PORT and LCD_DATAx_PIN definitions.
*
*/
// BLOCK MODIFIED BY M.H.
#define LCD_PORT
PORTA
#define LCD_DATA0_PORT
LCD_PORT
#define LCD_DATA1_PORT
LCD_PORT
#define LCD_DATA2_PORT
LCD_PORT
#define LCD_DATA3_PORT
LCD_PORT
#define LCD_DATA0_PIN
4
#define LCD_DATA1_PIN
5
#define LCD_DATA2_PIN
6
#define LCD_DATA3_PIN
7
#define LCD_RS_PORT
LCD_PORT
#define LCD_RS_PIN
2
#define LCD_RW_PORT
LCD_PORT
#define LCD_RW_PIN
3
#define LCD_E_PORT
LCD_PORT
#define LCD_E_PIN
0
/**<
/**<
/**<
/**<
/**<
/**<
/**<
/**<
/**<
/**<
/**<
/**<
/**<
/**<
/**<
port for the LCD lines
*/
port for 4bit data bit 0 */
port for 4bit data bit 1 */
port for 4bit data bit 2 */
port for 4bit data bit 3 */
was:0 pin for 4bit data bit 0 */
was:...pin for 4bit data bit 1 */
pin for 4bit data bit 2 */
pin for 4bit data bit 3 */
port for RS line
*/
pin for RS line
*/
port for RW line
*/
pin for RW line
*/
port for Enable line
*/
pin for Enable line
*/
#elif defined(__AVR_AT90S4414__) || defined(__AVR_AT90S8515__) ||
defined(__AVR_ATmega64__) || \
defined(__AVR_ATmega8515__)|| defined(__AVR_ATmega103__) ||
defined(__AVR_ATmega128__) || \
defined(__AVR_ATmega161__) || defined(__AVR_ATmega162__)
/*
* memory mapped mode is only supported when the device has an external data memory
interface
*/
#define LCD_IO_DATA
0xC000
/* A15=E=1, A14=RS=1
*/
#define LCD_IO_FUNCTION 0x8000
/* A15=E=1, A14=RS=0
*/
#define LCD_IO_READ
0x0100
/* A8 =R/W=1 (R/W: 1=Read, 0=Write
*/
#else
#error "external data memory interface not available for this device, use 4-bit IO port
mode"
#endif
/**
* @name Definitions for LCD command instructions
* The constants define the various LCD controller instructions which can be passed to
the
* function lcd_command(), see HD44780 data sheet for a complete description.
*/
/* instruction register bit positions, see HD44780U data sheet */
#define LCD_CLR
0
/* DB0: clear display
#define LCD_HOME
1
/* DB1: return to home position
#define LCD_ENTRY_MODE
2
/* DB2: set entry mode
#define LCD_ENTRY_INC
1
/*
DB1: 1=increment, 0=decrement
#define LCD_ENTRY_SHIFT
0
/*
DB2: 1=display shift on
#define LCD_ON
3
/* DB3: turn lcd/cursor on
*/
*/
*/
*/
*/
*/
39
#define
#define
#define
#define
#define
#define
#define
#define
#define
#define
#define
#define
#define
LCD_ON_DISPLAY
LCD_ON_CURSOR
LCD_ON_BLINK
LCD_MOVE
LCD_MOVE_DISP
LCD_MOVE_RIGHT
LCD_FUNCTION
LCD_FUNCTION_8BIT
LCD_FUNCTION_2LINES
LCD_FUNCTION_10DOTS
LCD_CGRAM
LCD_DDRAM
LCD_BUSY
2
1
0
4
3
2
5
4
3
2
6
7
7
/* set entry mode: display shift
#define LCD_ENTRY_DEC
#define LCD_ENTRY_DEC_SHIFT
#define LCD_ENTRY_INC_
#define LCD_ENTRY_INC_SHIFT
/*
/*
/*
/*
/*
/*
/*
/*
/*
/*
/*
/*
/*
DB2: turn display on
DB1: turn cursor on
DB0: blinking cursor ?
DB4: move cursor/display
DB3: move display (0-> cursor) ?
DB2: move right (0-> left) ?
DB5: function set
DB4: set 8BIT mode (0->4BIT mode)
DB3: two lines (0->one line)
DB2: 5x10 font (0->5x7 font)
DB6: set CG RAM address
DB7: set DD RAM address
DB7: LCD is busy
on/off, dec/inc cursor move direction */
0x04
/* display shift off, dec cursor move
0x05
/* display shift on, dec cursor move
0x06
/* display shift off, inc cursor move
0x07
/* display shift on, inc cursor move
/* display on/off, cursor on/off, blinking
#define LCD_DISP_OFF
0x08
/*
#define LCD_DISP_ON
0x0C
/*
#define LCD_DISP_ON_BLINK
0x0D
/*
#define LCD_DISP_ON_CURSOR
0x0E
/*
#define LCD_DISP_ON_CURSOR_BLINK 0x0F
/*
char at
display
display
display
display
display
/* move
#define
#define
#define
#define
move cursor left (decrement)
move cursor right (increment)
shift display left
shift display right
cursor/shift display */
LCD_MOVE_CURSOR_LEFT
LCD_MOVE_CURSOR_RIGHT
LCD_MOVE_DISP_LEFT
LCD_MOVE_DISP_RIGHT
0x10
0x14
0x18
0x1C
/*
/*
/*
/*
/* function set: set interface data length
#define LCD_FUNCTION_4BIT_1LINE 0x20
/*
#define LCD_FUNCTION_4BIT_2LINES 0x28
/*
#define LCD_FUNCTION_8BIT_1LINE 0x30
/*
#define LCD_FUNCTION_8BIT_2LINES 0x38
/*
#define LCD_MODE_DEFAULT
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
dir
dir
dir
dir
cursor position */
off
on, cursor off
on, cursor off, blink char
on, cursor on
on, cursor on, blink char
and number of display lines */
4-bit interface, single line, 5x7
4-bit interface, dual line,
5x7
8-bit interface, single line, 5x7
8-bit interface, dual line,
5x7
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
*/
dots
dots
dots
dots
*/
*/
*/
*/
((1<<LCD_ENTRY_MODE) | (1<<LCD_ENTRY_INC) )
/**
* @name Functions
*/
/**
@brief
@param
Initialize display and select type of cursor
dispAttr \b LCD_DISP_OFF display off\n
\b LCD_DISP_ON display on, cursor off\n
\b LCD_DISP_ON_CURSOR display on, cursor on\n
\b LCD_DISP_ON_CURSOR_BLINK display on, cursor on flashing
none
@return
*/
extern void lcd_init(uint8_t dispAttr);
/**
@brief
Clear display and set cursor to home position
@param
void
@return
none
*/
extern void lcd_clrscr(void);
/**
@brief
@param
Set cursor to home position
void
40
@return
none
*/
extern void lcd_home(void);
/**
@brief
Set cursor to specified position
@param
x horizontal position\n (0: left most position)
@param
y vertical position\n
(0: first line)
@return
none
*/
extern void lcd_gotoxy(uint8_t x, uint8_t y);
/**
@brief
Display character at current cursor position
@param
c character to be displayed
@return
none
*/
extern void lcd_putc(char c);
/**
@brief
Display string without auto linefeed
@param
s string to be displayed
@return
none
*/
extern void lcd_puts(const char *s);
/**
@brief
Display string from program memory without auto linefeed
@param
s string from program memory be be displayed
@return
none
@see
lcd_puts_P
*/
extern void lcd_puts_p(const char *progmem_s);
/**
@brief
Send LCD controller instruction command
@param
cmd instruction to send to LCD controller, see HD44780 data sheet
@return
none
*/
extern void lcd_command(uint8_t cmd);
/**
@brief
Send data byte to LCD controller
Similar to lcd_putc(), but without interpreting LF
@param
data byte to send to LCD controller, see HD44780 data sheet
@return
none
*/
extern void lcd_data(uint8_t data);
/**
@brief macros for automatically storing string constant in program memory
*/
#define lcd_puts_P(__s)
lcd_puts_p(PSTR(__s))
/*@}*/
#endif //_LCD_H_
41
LED.c
#include
#include
#include
#include
#include
<stdio.h>
<string.h>
<avr/io.h>
<avr/interrupt.h>
<avr/signal.h>
#include "inc/LED.h"
void ledOn(void)
{
PORTB |= 128;
}
void ledOff(void)
{
PORTB &= ~128;
}
void ledToggle(void)
{
if (PORTB & 128) {
ledOff();
} else {
ledOn();
}
}
LED.h
#ifndef _LED_H_
#define _LED_H_
void ledOn(void);
void ledOff(void);
void ledToggle(void);
#endif// _LED_H_
42
motors.c
/*AVR and C included*/
#include <inttypes.h>
#include <string.h>
#include <avr/io.h>
#include <avr/delay.h>
#include "inc/motors.h"
#include "inc/ADC.h"
#include "inc/timers.h"
#include "inc/LED.h"
/*Notes*/
//OCR1A - is motor on the right side of our robot
//OCR1B - is motor on the left side of our robot
/*Local declarations*/
typedef enum // Enum for describing moving status of the robot.
{
STOPED = 0x01,
MOVING_BACKWARD,
MOVING_FORWARD,
TURNING_LEFT,
TURNING_RIGHT,
} ROBOT_MOVING_STATE;
ROBOT_MOVING_COMMAND current_moving_action = STOP;
timer_executing = 0x00; // As long as timer executes ignore other commands
/*Global declarations*/
void m_init_pwm(void)
/***********************************************/
/* This function initializes timer for PWM
*/
/* used with servo motors.
*/
/***********************************************/
{
TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11);
// COM1A1 = 1 ja COM1A0 = 0 st. et Clear OCnA/OCnB/OCnC on compare match (set
output to low level).
// WGM11 WGM12 WGM13 määrab type of wafeform to be used. p.135
// The three clock select bits select the clock source to be used by the
Timer/Counter, see
// Figure 55 and Figure 56.
p. 136
TCCR1B = _BV(CS11) | _BV(WGM13) | _BV(WGM12);
//DDB6 and DDB5 set to 1 to use these outputs as PWM
DDRB = 0x77;
//PORTB 0b1110111
PORTB = 0xFF;
ICR1 = 36000;
m_drive_stop();
//XTAL/8/256*50
14745600
36000
}
void m_move_robot(ROBOT_MOVING_COMMAND input_command)
/***********************************************/
/* This function controls motors
*/
/***********************************************/
{
// as timer is executing command other commands are ingnored
static ROBOT_MOVING_COMMAND prev_command = 0x00;
unsigned char output_buffer[300];
if(prev_command != input_command)
{
43
sprintf(output_buffer, "motors command = 0x%x\n\r",input_command);
usart_write(output_buffer, strlen(output_buffer));
}
prev_command = input_command;
if(timer_executing)
return;
ledToggle();
switch(input_command)
{
case STOP:
m_drive_stop();
break;
case BACKWARD:
m_drive_backward(10);
break;
case FORWARD:
m_drive_forward(10);
break;
case LEFT:
m_turn_left(10);
break;
case RIGHT:
m_turn_right(10);
break;
case RIGHT_90_DEG:
m_turn_right_90_deg();
break;
case LEFT_90_DEG:
m_turn_left_90_deg();
break;
case RIGHT_SLOWLY:
m_turn_right_slowly();
break;
case LEFT_SLOWLY:
m_turn_left_slowly();
break;
case SMOOTH_FW_RIGHT:
m_smooth_drive_fw_right();
break;
case SMOOTH_FW_LEFT:
m_smooth_drive_fw_left();
break;
// this command turns around corner
case TURN_CORNER_RIGHT:
m_turn_around_right_corner();
break;
// this command turns around corner
case TURN_CORNER_LEFT:
m_turn_around_left_corner();
break;
default:
break;
}
current_moving_action = input_command;
}
void m_drive_stop(void)
/***********************************************/
/* This function stops motors
*/
/***********************************************/
44
{
OCR1A = 2484;
OCR1B = 2510;
}
void m_drive_backward(int drive_time)
/***********************************************/
/* This function drives servos x ms backward
*/
/***********************************************/
{
OCR1A = 2650; /*3600 should be measured out*/
OCR1B = 2280;
}
void m_drive_forward(int drive_time)
/***********************************************/
/* This function drives servos x ms forward
*/
/***********************************************/
{
OCR1A = 2334; /* Calculated (2484 - 150) - (STOP_PWM - 150) */
OCR1B = 2710; /* Calculated (2510 + 200) - (STOP_PWM + 200) */
}
void m_turn_right(int degree)
/***********************************************/
/* This function turns servos x degrees right */
/***********************************************/
{
OCR1A = 3600; /*3600 should be measured out*/
OCR1B = OCR1A;
}
void m_turn_left(int degree)
/***********************************************/
/* This function turns servos x degrees left
*/
/***********************************************/
{
OCR1A = 1800; /*1800 should be measured out*/
OCR1B = OCR1A;
}
void m_smooth_drive_fw_right(void)
/***********************************************/
/* This function turns servos smoothly right
*/
/***********************************************/
{
OCR1A = 2339; // Stops at 2484
OCR1B = 2710;
}
void m_smooth_drive_fw_left(void)
/***********************************************/
/* This function turns servos smoothly left
*/
/***********************************************/
{
OCR1A = 2334;
OCR1B = 2650; // stops at 2510
}
void m_turn_right_90_deg(void)
/***********************************************/
/* This function turns servos 90 deg right
*/
/***********************************************/
{
m_turn_right(10);
ask_timer_command(150,current_moving_action); // 140 is time it takes to turn 90 deg,
after that STOP
}
void m_turn_left_90_deg(void)
45
/***********************************************/
/* This function turns servos 90 deg left
*/
/***********************************************/
{
m_turn_left(10);
ask_timer_command(150,current_moving_action); // 140 is time it takes to turn 90 deg,
after that STOP
}
void m_turn_left_slowly(void)
/***********************************************/
/* This function turns left slowly
*/
/***********************************************/
{
OCR1A = 2424;
OCR1B = 2450;
}
void m_turn_right_slowly(void)
/***********************************************/
/* This function turns right slowly
*/
/***********************************************/
{
OCR1A = 2544;
OCR1B = 2570;
}
void m_turn_around_left_corner(void)
/***********************************************/
/* This function turns aroudn corner
*/
/***********************************************/
{
//2484
//2510
OCR1A = 2334; /* Calculated (2484 - 150) - (STOP_PWM - 150) */
OCR1B = 2610; /* Calculated (2510 + 200) - (STOP_PWM + 200) */
ask_timer_command(150,current_moving_action); // 140 is time it takes to turn 90 deg,
after that STOP
}
void m_turn_around_right_corner(void)
/***********************************************/
/* This function turns aroudn corner
*/
/***********************************************/
{
OCR1A = 2434; /* Calculated (2484 - 150) - (STOP_PWM - 150) */
OCR1B = 2710; /* Calculated (2510 + 200) - (STOP_PWM + 200) */
ask_timer_command(350,FORWARD); // 140 is time it takes to turn 90 deg, after that STOP
}
void set_timer_executing(void)
/***********************************************/
/* Set the flag that indicates timer execution */
/***********************************************/
{
timer_executing = 0x01;
}
void clear_timer_executing(void)
/*************************************************/
/* Clear the flag that indicates timer execution */
/*************************************************/
{
timer_executing = 0x00;
}
46
motors.h
#ifndef _MOTORS_H_
#define _MOTORS_H_
typedef enum // Enum for describing moving status of the robot.
{
STOP = 0x78,
// 'x'
BACKWARD = 0x73,
// 's'
FORWARD = 0x77,
// 'w'
LEFT = 0x61,
// 'a'
RIGHT = 0x64,
// 'd'
RIGHT_90_DEG = 0x65, // 'e'
LEFT_90_DEG = 0x71, // 'q'
RIGHT_SLOWLY = 0x70, // 'p'
LEFT_SLOWLY = 0x6F, // 'o'
SMOOTH_FW_LEFT = 0x10,
SMOOTH_FW_RIGHT = 0x11,
TURN_CORNER_RIGHT = 0x69, //'i'
TURN_CORNER_LEFT = 0x75
//'u'
}ROBOT_MOVING_COMMAND;
void m_move_robot(ROBOT_MOVING_COMMAND input_command);
/***********************************************/
/* This function controls motors
*/
/***********************************************/
void m_init_pwm(void);
/***********************************************/
/* This function initializes timer for PWM
*/
/* used with servo motors.
*/
/***********************************************/
void m_drive_stop(void);
/***********************************************/
/* This function stops motors
*/
/***********************************************/
void m_drive_forward(int drive_time);
/***********************************************/
/* This function drives servos x ms forward
*/
/***********************************************/
void m_drive_backward(int drive_time);
/***********************************************/
/* This function drives servos x ms backward
*/
/***********************************************/
void m_turn_right(int degree);
/***********************************************/
/* This function turns servos x degrees right */
/***********************************************/
void m_turn_left(int degree);
/***********************************************/
/* This function turns servos x degrees left
*/
/***********************************************/
void
void
void
void
m_smooth_drive_fw_right(void);
m_smooth_drive_fw_left(void);
m_turn_right_90_deg(void);
m_turn_left_90_deg(void);
void m_turn_left_slowly(void);
/***********************************************/
47
/* This function turns left slowly
*/
/***********************************************/
void m_turn_right_slowly(void);
/***********************************************/
/* This function turns right slowly
*/
/***********************************************/
void set_timer_executing(void);
/***********************************************/
/* Set the flag that indicates timer execution */
/***********************************************/
void clear_timer_executing(void);
/*************************************************/
/* Clear the flag that indicates timer execution */
/*************************************************/
void m_turn_around_left_corner(void);
/***********************************************/
/* This function turns aroudn corner
*/
/***********************************************/
void m_turn_around_right_corner(void);
/***********************************************/
/* This function turns aroudn corner
*/
/***********************************************/
#endif// _MOTORS_H_
48
macros.h
/*This file has all available macros*/
/* 1 - port manipulation */
#define SET_B(x) |=(1<<x) // defineeritakse kõrge bit pordis x
#define CLR_B(x) &=~(1<<x) // defineeritakse madal bit pordis x
#define INV_B(x) ^=(1<<x) // defineeritakse biti inverteerimine pordis x
49
move.c
/*System
#include
#include
#include
#include
#include
icludes*/
"inc/motors.h"
"inc/timers.h"
"inc/move.h"
"inc/lcd.h" // if you delete this then it will not compile
"inc/ADC.h"
static ROBOT_POSITION robot_position = ROOM_01; // This is start position
/*static ROBOT_DIRRECTION robot_direction = NORD; // North is the initial direction
//Gives overview how moves can be made
//START_POS,FIN_POS,HEAD_DIRECTION to have the move possible
static char possible_moves[21][3]=
{{ROOM_01,ROOM_02,NORD},
{ROOM_02,ROOM_01,SYD},
{ROOM_02,ROOM_03,NORD},
{ROOM_03,ROOM_02,SYD},
{ROOM_03,ROOM_04,OST},
{ROOM_03,ROOM_07,WEST},
{ROOM_03,ROOM_09,NORD},
{ROOM_04,ROOM_03,WEST},
{ROOM_04,ROOM_05,OST},
{ROOM_05,ROOM_04,WEST},
{ROOM_05,ROOM_06,SYD},
{ROOM_06,ROOM_05,NORD},
{ROOM_07,ROOM_03,OST},
{ROOM_08,ROOM_07,NORD},
{ROOM_08,ROOM_14,WEST},
{ROOM_09,ROOM_10,NORD},
{ROOM_09,ROOM_12,WEST},
{ROOM_10,ROOM_11,OST},
{ROOM_11,ROOM_10,WEST},
{ROOM_12,ROOM_13,NORD},
{ROOM_13,ROOM_12,SYD}
};
*/
ROBOT_POSITION get_current_position(void)
{
return robot_position;
}
void m_move_robot_along_coridor(void)
/********************************************************************/
/* This function controls motors to move along coridor
*/
/********************************************************************/
{
if (get_last_measure_value(RIGHT_DIST_SENSOR)<get_last_measure_value(LEFT_DIST_SENSOR))
m_smooth_drive_fw_right();
else
m_smooth_drive_fw_left();
}
void m_move_robot_along_right_wall(void)
/********************************************************************/
/* This function controls motors to move along wall on the right
*/
/********************************************************************/
{
int right_dist_value = get_last_measure_value(RIGHT_DIST_SENSOR);
int time1,time2;
double time_value = time1*1000+time2;
static double prev_time_turn_exectued = 0;
get_timer_value(&time1,&time2);
50
// check if right wall has ended
/* if(ADC_get_peak(RIGHT_DIST_SENSOR) && right_dist_value < 250)
{
init_ADC();
m_move_robot(TURN_CORNER_RIGHT);
}*/
// should consider
if(ADC_get_peak(RIGHT_DIST_SENSOR) && (time_value - prev_time_turn_exectued)>500/* &&
right_dist_value < 100*/)
{
init_ADC();
m_move_robot(TURN_CORNER_RIGHT);
prev_time_turn_exectued = time1*1000+time2;
}
if(right_dist_value < 700 || ((get_right_sensor_drift() == 0x01) && right_dist_value
<850))
m_move_robot(SMOOTH_FW_RIGHT);
else if(right_dist_value > 850 || (get_right_sensor_drift() == 0x02)) //0x02 - drift
is to right
m_move_robot(SMOOTH_FW_LEFT);
}
move.h
#ifndef _MOVE_H_
#define _MOVE_H_
typedef enum
{
ROOM_01,
ROOM_02,
ROOM_03,
ROOM_04,
ROOM_05,
ROOM_06,
ROOM_07,
ROOM_08,
ROOM_09,
ROOM_10,
ROOM_11,
ROOM_12,
ROOM_13,
ROOM_14
}ROBOT_POSITION;
typedef enum
{
NORD,
SYD,
OST,
WEST,
}ROBOT_DIRRECTION;
void m_move_robot_along_right_wall(void);
/********************************************************************/
/* This function controls motors to move along wall on the right
*/
/********************************************************************/
void m_move_robot_along_coridor(void);
/********************************************************************/
/* This function controls motors to move along coridor
*/
/********************************************************************/
#endif// _ADC_H_
51
sam.c
/*AVR and C included*/
#include <inttypes.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <avr/io.h>
#include <avr/delay.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
/*System
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
icludes*/
"inc/ver.h"
"inc/macros.h"
"inc/motors.h"
"inc/move.h"
"inc/lcd.h"
"inc/ADC.h"
"inc/USART.h"
"inc/LED.h"
"inc/timers.h"
"inc/defines.h"
"inc/interrupts.h"
"inc/UH.h"
/*Global declarations*/
/*Local declarations*/
typedef enum
{
MOVING_TO_ROOM_06,
SEARCHING_CNDL_ROOM_06,
MOVING_TO_ROOM_11,
SEARCHING_CNDL_ROOM_11,
MOVING_TO_ROOM_13,
SEARCHING_CNDL_ROOM_13,
MOVING_TO_ROOM_14,
SEARCHING_CNDL_ROOM_14
}GLOBAL_POSITION;
static volatile unsigned int counter_ms=0;
static volatile unsigned int counter_sec=0;
SIGNAL(SIG_OUTPUT_COMPARE0)
{
counter_ms++;
if (counter_ms>1000) {
counter_ms = 0;
counter_sec++;
}
}
static unsigned char output_buffer[300];
void init_system(void)
{
int a, b, c;
DDRE SET_B(6); // Set port PE6 as output. It is used for switching ventilator.
PORTE CLR_B(6); // Clear PE6 Used for ventilator
usart_init(9.6); // Note that this baud rate is not used. It is hardcoded
cli();
DDRB |= 128;
ledOff();
// input
DDRE &= ~(1<<4); // PE4 input
52
// timer
TCCR0 = 0;
TCNT0 = 0;
OCR0 = 125; // divide by 125, output_compare0 occurs at 1 khz
TIMSK |= (1<<OCIE0);
TCCR0 |= 8; // CTC mode
TCCR0 |= 3; // /32 prescaler ( 125khz)
for (c=0; c<4; c++) {
for (a=0; a<500; a++) {
for (b=0; b<500; b++) {
asm volatile("nop");
}
}
ledToggle();
}
counter_ms = counter_sec = 0;
}
int main(void){
int timer_x = 0;
int timer_y = 0;
GLOBAL_POSITION global_position = MOVING_TO_ROOM_06; // To have overview of current
action
// values used for IR detector readings
uint16_t y0,
// L_IR_DIST
y1,
// R_IR_DIST
y2,
// L_IR_CNDL
y3 = 0, // R_IR_CNDL
y4 = 0, // FRONT_US_DIST
y5 = 0;
char usart_input_cmd;
init_system();
init_ADC();
// Init Ultra Sonic measurement module
init_US();
sei();
sprintf(output_buffer, "status: ready\n\r");
usart_write(output_buffer, strlen(output_buffer));
m_init_pwm();
#ifdef LCD_IS_USED
char string[LCD_LINE_LEN]; // string should not excede LCD_LINE_LEN
// initialize display, cursor off
lcd_init(LCD_DISP_ON);
lcd_clrscr();
lcd_puts("LCD Test Line 1\n");
lcd_gotoxy(0,1);
sprintf(string,"SW ver. %d.%d%s",VER_HIGH,VER_LOW,VER_LTR);
lcd_puts(string);
#endif // LCD_IS_USED
// DDRD = 0x00; //PORTD sisend
// PORTD = 0x07; //Esimesele kolmele bitile pull-up sisse lülitatud
// SREG |= 0x80; // The Global Interrupt Enable bit must be set for the interrupts to be
enabled.
// char data[30];
//
memset(data,0x00,30);
53
twi_writeTo(0xF0,data,2,0); // F0 is the address of our IIC SFR08
_delay_ms(250);_delay_ms(250);
_delay_ms(250);_delay_ms(250);
_delay_ms(250);_delay_ms(250);
_delay_ms(250);_delay_ms(250);
data[0] = 0x02;
twi_writeTo(0xF0,data,2,0);
twi_readFrom(0xF0,data,2); // When reading 0 bytes it seems that still one byte is
sent from slave.
*/
//
sprintf(output_buffer, "0x%X, 0x%X\n\r",data[0],data[1]);
//
usart_write(output_buffer, strlen(output_buffer));
// m_drive_forward(10);
activate_timer();
en_external_int();
while(1) //SYSTEM LOOP
{
switch(global_position)
{
case MOVING_TO_ROOM_06 :
m_move_robot_along_right_wall();
break;
default:
break;
}
usart_input_cmd = usart_buffer_poll();
if(usart_buffer_poll() != 0x00)
{
get_timer_value(&timer_x,&timer_y);
sprintf(output_buffer,"TIMER = %6d, %6d\n\r",timer_x,timer_y);
usart_write(output_buffer, strlen(output_buffer));
usart_read(&usart_input_cmd,1);
m_move_robot(usart_input_cmd);
PORTE INV_B(6);
}
y0 = ReadLastMeasurement(0);
y1 = ReadLastMeasurement(1);
y2 = ReadLastMeasurement(2);
y3 = ReadLastMeasurement(3);
y4 = ReadLastUSMeasurement();
y5 = ReadLastAvgMeasurement(1);
//
//y2=y5;
sprintf(output_buffer, "IR_DIST_L = %4d, IR_DIST_R = %4d, IR_CNDL_L = %4d, IR_CNDL_R
= %4d, UH_DIST = %6d \n\r",y0,y1,y2,y3,y4);
usart_write(output_buffer, strlen(output_buffer));
sprintf(output_buffer, "IR_DIST_DIF = %4d, IR_CNDL_DIF = %4d\n\r",abs(y0-y1),abs(y2y3));
usart_write(output_buffer, strlen(output_buffer));
sprintf(output_buffer, "PINDE = %x \n\r",PINE);
usart_write(output_buffer, strlen(output_buffer));
static int prev = 0;
if(abs(y3-y2)>40)
{
m_move_robot(STOP);
}
prev = y3;
//
}
}
m_turn_left(10);
54
timers.c
/*This module handles timers*/
/*This module handles timers*/
#include <inttypes.h>
#include <math.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "inc/motors.h"
#include "inc/ADC.h"
#include "inc/UH.h"
static int i;
int SAGEDUS = 1000;
int x=0;
int execute_time = 0;
ROBOT_MOVING_COMMAND command_to_be_executed = STOP;
char motors_req_command = 0x00;
ISR(TIMER3_OVF_vect)
{
static double time_value = 0;
static int ir_dist_big_jump = 0;
static double previous_ADC_measurement_time = 0;
static double previous_US_measurement_time = 0;
time_value = 1000*x + i;
//Here is the time wich says how often the measure is executed
if((abs(time_value - previous_ADC_measurement_time))>10)
{
previous_ADC_measurement_time = time_value;
measure_ADC_values();
}
//Here is the time wich says how often the measure is executed
if((abs(time_value - previous_US_measurement_time))>100)
{
previous_US_measurement_time = time_value;
measure_US_values();
}
/*
// If right channel has big jump in output.
if(abs(ReadLastMeasurement(1) - ReadLastAvgMeasurement(1))>500)
{
m_move_robot(STOP);
}
*/
i++;
if(i>SAGEDUS)
{
i=0;
x++;
}
if(motors_req_command && (time_value > execute_time))
{
// this is used to avoid executing other command while using timer to turn 90deg
clear_timer_executing();
m_move_robot(command_to_be_executed);
motors_req_command = 0x00;
}
/* if(i==2)
m_move_robot(FORWARD);
if(i==600)
m_move_robot(BACKWARD);*/
}
55
void activate_timer(void)
{
TCCR3A = 0x00;
// Use timer in normal mode
TCCR3B = _BV(CS30); // Do not use prescaler. Freq depends directly from clock
ETIMSK = _BV(TOIE3); // Enable overload interrupt
}
void get_timer_value(int *val_1,int *val_2)
{
*val_1 = x;
*val_2 = i;
}
void ask_timer_command(int time,ROBOT_MOVING_COMMAND command_to_start)
{
execute_time = x*1000 + i + time;
command_to_be_executed = command_to_start;
motors_req_command = 0x01;
set_timer_executing();
}
timers.h
void activate_timer(void);
void get_timer_value(int *val_1,int *val_2);
void ask_timer_command(int time,ROBOT_MOVING_COMMAND command_to_start);
56
UH.c
#include
#include
#include
#include
#include
<inttypes.h>
<stdio.h>
<string.h>
<avr/io.h>
<util/delay.h>
#include "inc/UH.h"
#include "inc/macros.h"
#define UH_BUFFER_LEN
8
uint16_t uh_channel_buffer[UH_BUFFER_LEN];
void ISR(INT1_vect) // this interrupt is not used right now in version 0.01i
{
static double cnt = 0;
cnt++;
}
void init_US(void) // Initialize ultrasonic sensor
{
DDRD |= (1<<PD0); // This is initialize pin ->direction of sonar port pins
DDRD &= ~(1<<PD1); // This is echo pin -> set PD1 input
//
//
//
DDRE SET_B(7); // Set PE7 as output
PORTE SET_B(7); // Activate pull up. (Output goes high)
EIMSK SET_B(1);
// Enable INT1 in EIMSK register
//
//
EICRA CLR_B(2);
EICRA SET_B(3);
// Set that interrupt will happen
// when signal gets falling edge
}
void push_US_value(uint16_t value)
{
int i;
for(i=0;i<(UH_BUFFER_LEN-1);i++) // -1 as last value is taken from input
{
uh_channel_buffer[i]=uh_channel_buffer[i+1];
}
uh_channel_buffer[UH_BUFFER_LEN-1] = value;
}
void measure_US_values(void)
{
unsigned int l,son1,son2 = 0;
static unsigned int cnt_of_illegal_values = 0;
PORTD |= (1 << PD0); //start sonar input pulse
for(l=0;l<10000;l++){} // >= 10us
PORTD &= ~(1 << PD0);
for(son1 = 0; son1 < 65535; son1++)
{
if(PIND & 0x02) //rising edge
break;
}
for(son2 = 0; son2 < 65535; son2++)
{
if(!(PIND & 0x02)) //falling edge
break;
//
for (m=0;m<16;m++);
}
if(abs(son2-son1) < US_250_CM) // 6200 - 250_CM
{
57
push_US_value(son2-son1);
}
// this is check for not inserting noise to buffer
// if there is more than 7 illegal values in row
// then it was propably correct measurement and may insert
if ((abs(abs(son2-son1)-ReadLastUSMeasurement()) < 2000) || (cnt_of_illegal_values >
7))
{
// if measurement doesn't excede maximal possible value in the court
if(abs(son2-son1) < US_250_CM) // 6200 - 250_CM
{
push_US_value(son2-son1);
cnt_of_illegal_values = 0;
}
}
else
cnt_of_illegal_values++;
}
uint16_t ReadLastUSMeasurement(void)
{
return uh_channel_buffer[UH_BUFFER_LEN-1];
}
UH.h
#ifndef _UH_H_
#define _UH_H_
void init_US(void);
uint16_t ReadLastUSMeasurement(void);
void measure_US_values(void);
#endif// _UH_H_
typedef enum
{
US_250_CM = 6200, //MAX size of the court
US_220_CM = 5820,
US_190_CM = 5520,
US_160_CM = 4620,
US_130_CM = 3720,
US_100_CM = 2850,
US_70_CM = 1950,
US_40_CM = 1150,
US_20_CM = 540,
US_10_CM = 230,
}US_MEASURE;
58
UH_TWI.c
static unsigned char output_buffer[300];
/*
twi.c - TWI/I2C library for Wiring & Arduino
Copyright (c) 2006 Nicholas Zambetti. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
*/
#include
#include
#include
#include
#include
#include
#include
#include
#include
USA
<math.h>
<stdlib.h>
<string.h>
<stdio.h>
<inttypes.h>
<avr/io.h>
<avr/interrupt.h>
<compat/twi.h>
<util/delay.h>
/*#include "WConstants.h"*/
#include "inc/USART.h"
#include "inc/UH_TWI.h"
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
static volatile uint8_t twi_state;
static uint8_t twi_slarw;
static void (*twi_onSlaveTransmit)(void);
static void (*twi_onSlaveReceive)(uint8_t*, int);
static uint8_t* twi_masterBuffer;
static volatile uint8_t twi_masterBufferIndex;
static uint8_t twi_masterBufferLength;
static uint8_t* twi_txBuffer;
static volatile uint8_t twi_txBufferIndex;
static volatile uint8_t twi_txBufferLength;
static uint8_t* twi_rxBuffer;
static volatile uint8_t twi_rxBufferIndex;
/*
* Function twi_init
* Desc
readys twi pins and sets twi bitrate
* Input
none
* Output
none
*/
void twi_init(void)
{
59
usart_write("twi_init(void)", strlen("twi_init(void)"));
// initialize state
twi_state = TWI_READY;
// activate internal pull-ups for twi
// as per note from atmega128 manual pg204
sbi(PORTD, 0);
sbi(PORTD, 1);
// initialize twi prescaler and bit rate
cbi(TWSR, TWPS0);
cbi(TWSR, TWPS1);
TWBR = ((CPU_FREQ / TWI_FREQ) - 16)/ 2;// M2rt
/* twi bit rate formula from atmega128 manual pg 204
SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
note: TWBR should be 10 or higher for master mode
It is 72 for a 16mhz Wiring board with 100kHz TWI */
// enable twi module, acks, and twi interrupt
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
// allocate buffers
twi_masterBuffer = (uint8_t*) calloc(TWI_BUFFER_LENGTH, sizeof(uint8_t));
twi_txBuffer = (uint8_t*) calloc(TWI_BUFFER_LENGTH, sizeof(uint8_t));
twi_rxBuffer = (uint8_t*) calloc(TWI_BUFFER_LENGTH, sizeof(uint8_t));
}
/*p-209 The TWAR should be loaded with the 7-bit slave address (in the seven most
significant
bits of TWAR) to which the TWI will respond when programmed as a slave transmitter or
receiver, and not needed in the master modes. In multimaster systems, TWAR must be
set in masters which can be addressed as slaves by other masters.*/
/*
* Function twi_slaveInit
* Desc
sets slave address and enables interrupt
* Input
none
* Output
none
*/
void twi_setAddress(uint8_t address)
{
// set twi slave address (skip over TWGCE bit)
TWAR = address << 1;
}
/*
* Function twi_readFrom
* Desc
attempts to become twi bus master and read a
*
series of bytes from a device on the bus
* Input
address: 7bit i2c device address
*
data: pointer to byte array
*
length: number of bytes to read into array
* Output
byte: 0 ok, 1 length too long for buffer
*/
uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length)
{
usart_write("twi_readFrom()", strlen("twi_readFrom()"));
uint8_t i;
// ensure data will fit into buffer
if(TWI_BUFFER_LENGTH < length){
return 1;
}
// wait until twi is ready, become master receiver
while(TWI_READY != twi_state){
continue;
}
twi_state = TWI_MRX;
60
// initialize buffer iteration vars
twi_masterBufferIndex = 0;
twi_masterBufferLength = length;
// build sla+w, slave device address + w bit
twi_slarw = TW_READ;
twi_slarw |= address << 1;
// send start condition
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
// wait for read operation to complete
while(TWI_MRX == twi_state){
continue;
}
// copy twi buffer to data
for(i = 0; i < length; ++i){
data[i] = twi_masterBuffer[i];
}
return 0;
}
/*
* Function twi_writeTo
* Desc
attempts to become twi bus master and write a
*
series of bytes to a device on the bus
* Input
address: 7bit i2c device address
*
data: pointer to byte array
*
length: number of bytes in array
*
wait: boolean indicating to wait for write or not
* Output
byte: 0 ok, 1 length too long for buffer
*/
uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait)
{
usart_write("twi_writeTo()", strlen("twi_writeTo()"));
uint8_t i;
// ensure data will fit into buffer
if(TWI_BUFFER_LENGTH < length){
return 1;
}
// wait until twi is ready, become master transmitter
while(TWI_READY != twi_state){
continue;
}
twi_state = TWI_MTX;
// initialize buffer iteration vars
twi_masterBufferIndex = 0;
twi_masterBufferLength = length;
// copy data to twi buffer
for(i = 0; i < length; ++i){
twi_masterBuffer[i] = data[i];
}
// build sla+w, slave device address + w bit
twi_slarw = TW_WRITE;
twi_slarw |= address << 1;
// send start condition
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
// wait for write operation to complete
while(wait && (TWI_MTX == twi_state)){
continue;
}
61
return 0;
}
/*
* Function twi_transmit
* Desc
fills slave tx buffer with data
*
must be called in slave tx event callback
* Input
data: pointer to byte array
*
length: number of bytes in array
* Output
1 length too long for buffer
*
2 not slave transmitter
*
0 ok
*/
uint8_t twi_transmit(uint8_t* data, uint8_t length)
{
usart_write("twi_transmit()", strlen("twi_transmit()"));
uint8_t i;
// ensure data will fit into buffer
if(TWI_BUFFER_LENGTH < length){
return 1;
}
// ensure we are currently a slave transmitter
if(TWI_STX != twi_state){
return 2;
}
// set length and copy data into tx buffer
twi_txBufferLength = length;
for(i = 0; i < length; ++i){
twi_txBuffer[i] = data[i];
}
return 0;
}
/*
* Function twi_attachSlaveRxEvent
* Desc
sets function called before a slave read operation
* Input
function: callback function to use
* Output
none
*/
void twi_attachSlaveRxEvent( void (*function)(uint8_t*, int) )
{
usart_write("twi_attachSlaveRxEvent()", strlen("twi_attachSlaveRxEvent()"));
twi_onSlaveReceive = function;
}
/*
* Function twi_attachSlaveTxEvent
* Desc
sets function called before a slave write operation
* Input
function: callback function to use
* Output
none
*/
void twi_attachSlaveTxEvent( void (*function)(void) )
{
usart_write("twi_attachSlaveTxEvent()", strlen("twi_attachSlaveTxEvent()"));
twi_onSlaveTransmit = function;
}
/*
* Function twi_reply
* Desc
sends byte or readys receive line
* Input
ack: byte indicating to ack or to nack
* Output
none
*/
void twi_reply(uint8_t ack)
{
// transmit master read ready signal, with or without ack
if(ack){
62
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);
}else{
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
}
}
/*
* Function twi_stop
* Desc
relinquishes bus master status
* Input
none
* Output
none
*/
void twi_stop(void)
{
usart_write("twi_stop()", strlen("twi_stop()"));
// send stop condition
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO);
// wait for stop condition to be exectued on bus
// TWINT is not set after a stop condition!
while(TWCR & _BV(TWSTO)){
continue;
}
// update twi state
twi_state = TWI_READY;
}
/*
* Function twi_releaseBus
* Desc
releases bus control
* Input
none
* Output
none
*/
void twi_releaseBus(void)
{
usart_write("twi_releaseBus()", strlen("twi_releaseBus()"));
// release bus
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT);
// update twi state
twi_state = TWI_READY;
}
//! I2C (TWI) interrupt service routine
SIGNAL(SIG_2WIRE_SERIAL)
{
sprintf(output_buffer,"STATUS = 0x%X\n\r",TW_STATUS);
usart_write(output_buffer, strlen(output_buffer));
switch(TW_STATUS){
// All Master
case TW_START:
// sent start condition
case TW_REP_START: // sent repeated start condition
// copy device address and r/w bit to output register and ack
TWDR = twi_slarw;
twi_reply(1);
break;
// Master Transmitter
case TW_MT_SLA_ACK: // slave receiver acked address
case TW_MT_DATA_ACK: // slave receiver acked data
// if there is data to send, send it, otherwise stop
if(twi_masterBufferIndex < twi_masterBufferLength){
// copy data to output register and ack
TWDR = twi_masterBuffer[twi_masterBufferIndex++];
twi_reply(1);
}else{
twi_stop();
}
break;
63
case TW_MT_SLA_NACK: // address sent, nack received
case TW_MT_DATA_NACK: // data sent, nack received
twi_stop();
break;
case TW_MT_ARB_LOST: // lost bus arbitration
twi_releaseBus();
break;
// Master Receiver
case TW_MR_DATA_ACK: // data received, ack sent
// put byte into buffer
twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
case TW_MR_SLA_ACK: // address sent, ack received
// ack if more bytes are expected, otherwise nack
if(twi_masterBufferIndex < twi_masterBufferLength){
twi_reply(1);
}else{
twi_reply(0);
}
break;
case TW_MR_DATA_NACK: // data received, nack sent
// put final byte into buffer
twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
case TW_MR_SLA_NACK: // address sent, nack received
twi_stop();
break;
// TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case
// Slave Receiver
case TW_SR_SLA_ACK:
// addressed, returned ack
case TW_SR_GCALL_ACK: // addressed generally, returned ack
case TW_SR_ARB_LOST_SLA_ACK:
// lost arbitration, returned ack
case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack
// enter slave receiver mode
twi_state = TWI_SRX;
// indicate that rx buffer can be overwritten and ack
twi_rxBufferIndex = 0;
twi_reply(1);
break;
case TW_SR_DATA_ACK:
// data received, returned ack
case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack
// if there is still room in the rx buffer
if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){
// put byte in buffer and ack
twi_rxBuffer[twi_rxBufferIndex++] = TWDR;
twi_reply(1);
}else{
// otherwise nack
twi_reply(0);
}
break;
case TW_SR_STOP: // stop or repeated start condition received
// put a null char after data if there's room
if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){
twi_rxBuffer[twi_rxBufferIndex] = '\0';
}
// callback to user defined callback
twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex);
// ack future responses
twi_reply(1);
// leave slave receiver state
twi_state = TWI_READY;
break;
case TW_SR_DATA_NACK:
// data received, returned nack
case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack
// nack back at master
twi_reply(0);
break;
// Slave Transmitter
case TW_ST_SLA_ACK:
// addressed, returned ack
64
case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack
// enter slave transmitter mode
twi_state = TWI_STX;
// ready the tx buffer index for iteration
twi_txBufferIndex = 0;
// set tx buffer length to be zero, to verify if user changes it
twi_txBufferLength = 0;
// request for txBuffer to be filled and length to be set
// note: user must call twi_transmit(bytes, length) to do this
twi_onSlaveTransmit();
// if they didn't change buffer & length, initialize it
if(0 == twi_txBufferLength){
twi_txBufferLength = 1;
twi_txBuffer[0] = 0x00;
}
// transmit first byte from buffer, fall
case TW_ST_DATA_ACK: // byte sent, ack returned
// copy data to output register
TWDR = twi_txBuffer[twi_txBufferIndex++];
// if there is more to send, ack, otherwise nack
if(twi_txBufferIndex < twi_txBufferLength){
twi_reply(1);
}else{
twi_reply(0);
}
break;
case TW_ST_DATA_NACK: // received nack, we are done
case TW_ST_LAST_DATA: // received ack, but we are done already!
// ack future responses
twi_reply(1);
// leave slave receiver state
twi_state = TWI_READY;
break;
// All
case TW_NO_INFO:
// no state information
break;
case TW_BUS_ERROR: // bus error, illegal stop/start
twi_stop();
break;
}
}
UH_TWI.h
/*
twi.h - TWI/I2C library for Wiring & Arduino
Copyright (c) 2006 Nicholas Zambetti. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
*/
USA
#ifndef twi_h
#define twi_h
#include <inttypes.h>
65
//#define ATMEGA8
#ifndef CPU_FREQ
#define CPU_FREQ 14745600L
#endif
#ifndef F_CPU
#define F_CPU CPU_FREQ
#endif
#ifndef TWI_FREQ
#define TWI_FREQ 100000L
#endif
#ifndef TWI_BUFFER_LENGTH
#define TWI_BUFFER_LENGTH 32
#endif
#define
#define
#define
#define
#define
TWI_READY
TWI_MRX
TWI_MTX
TWI_SRX
TWI_STX
0
1
2
3
4
void twi_init(void);
void twi_setAddress(uint8_t);
uint8_t twi_readFrom(uint8_t, uint8_t*, uint8_t);
uint8_t twi_writeTo(uint8_t, uint8_t*, uint8_t, uint8_t);
uint8_t twi_transmit(uint8_t*, uint8_t);
void twi_attachSlaveRxEvent( void (*)(uint8_t*, int) );
void twi_attachSlaveTxEvent( void (*)(void) );
void twi_reply(uint8_t);
void twi_stop(void);
void twi_releaseBus(void);
#endif
66
USART.c
/**
FROM : http://www.raphnet.net/divers/anemometre/windmon_avr/usart.c
* ATmega128, Usart 0 code.
*
* Quickly hacked from the code for ATmega8.
* 2005, Raphael Assenat
*/
#include
#include
#include
#include
<avr/io.h>
<avr/interrupt.h>
<avr/signal.h>
"inc/USART.h"
#define INTERRUPT_DRIVEN
#ifdef
static
static
static
INTERRUPT_DRIVEN
unsigned char usart_buffer[16];
unsigned char usart_buffer_head = 0;
unsigned char usart_buffer_tail = 0;
SIGNAL(SIG_USART1_RECV)
{
usart_buffer[usart_buffer_head]=UDR1;
usart_buffer_head++;
usart_buffer_head &= 0xf;
}
#endif
int usart_read(unsigned char *buf, int max)
{
#ifdef INTERRUPT_DRIVEN
int i=0;
cli();
while (usart_buffer_head != usart_buffer_tail && i<max)
{
buf[i] = usart_buffer[usart_buffer_tail];
usart_buffer_tail++;
usart_buffer_tail &= 0xf;
i++;
}
sei();
return i;
#else
while (!(UCSR1A & (1<<RXC1)));
*buf = UDR1;
return 1;
#endif
}
char usart_buffer_poll(void)
{
#ifdef INTERRUPT_DRIVEN
cli();
if (usart_buffer_head != usart_buffer_tail) {
sei();
return 1;
}
sei();
return 0;
#else
return UCSR1A & (1<<RXC1);
#endif
}
void usart_init(unsigned int baud)
{
cli();
UBRR1H = 0;// Dont know when this must be used - (unsigned char)(baud>>8);
67
UBRR1L = (unsigned char)baud;
UBRR1L = (F_CPU / (16 * 19200UL)) - 1; /* 9600 Bd */
UCSR1A &= ~(1<<U2X1); // Make sure that speed is not set to double
UCSR1B = (1<<RXEN1)|(1<<TXEN1)|(1<<RXCIE1);
UCSR1C = 0x06; // sane default
sei();
}
void usart_write(const unsigned char *data, int len)
{
int i;
for (i=0; i<len; i++)
{
// wait for buffer to be empty
while (!(UCSR1A & (1<<UDRE1)));
UDR1 = data[i];
//
data++;
}
// wait for complete transfer
while (!(UCSR1A & (1<<TXC1)));
}
unsigned char usart_waitByte(void)
{
unsigned char c;
while (!usart_buffer_poll());
usart_read(&c, 1);
return c;
}
USART.h
#ifndef _usart_h__
#define _usart_h__
int usart_read(unsigned char *buf, int max);
char usart_buffer_poll(void);
void usart_init(unsigned int baud);
void usart_write(const unsigned char *data, int len);
unsigned char usart_waitByte(void);
#endif // _USART_H__
68
ver.h
#define VER_HIGH 0
#define VER_LOW 1
#define VER_LTR "m"
// Version 0.1m
// a) Moving along right wall works paritially.
// Version 0.1l
// a) Don't remember what i did yesterday but i make the copy before last changes as
contest is tomorrow.
// Version 0.1k
// a) Have added avg buffer for ADC. Just making copy before completing the code.
// Version 0.1j
// a) Guess that there were no big changes but it is just a backup before one.
// Version 0.1i
// a) AS TWI Ultra Sonic sensor didn't start working then included the one with digital.
// Version 0.1h
// a) Added timer for interrupts. Now we can drive for certain timeout FW, BW and etc.
// b) The version also prints measurements in ADC to USART.
// Version 0.1g
// a) Added interrupt handling to PE7. USe it for example. If PE7/INT then PE6=1.
// Version 0.1f
// a) Updated ADC with measurement buffer (FIFO buffer).
// b) Included timer module. Now timer module takes measurement after each 1,12 sec.
// c) Still control work though USART2. When senging 's', 'a' etc. commands, user can
dirve the robot.
//
//
//
//
//
//
Version 0.1e TWI has been added but it is not working
It is also possible to move robot by commands. Using 's', 'a', 'd', 'w' and 'x'
command through USART robot will move as command require.
s - BACKWARD, a - TURN_LEFT, d-TURN_RIGHT etc.
Version 0.1d IR distance sensors and candle detectors are working
Version 0.1c is version were motors should be improved
69