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