Drama - University of Florida



Drama

Final Report

Created By: Michael Guidi

EEL5666 Summer 2007

Drs. A. A. Arroyo and E. M. Schwartz

TAs: Adam Barnett and Kevin Claycomb

Table of Contents

Abstract…………………………………………………………………...…3

Executive Summary………………………………………………………....3

Introduction………..……………………………………………………...…3

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

Mobile Platform……………………………………………………….…….6

Actuation….…………………………………………………………………7

Sensors………………………………………………………………………8

Behaviors……………………………………………………………...……13

Experimental Layout and Results……………………………………….…14

Conclusion……...……………………………………………………….…16

Documentation…………………………………………………………..…17

Appendix...…………………………………………………………….…...18

Abstract

Drama’s purpose, if you will, is to act out scenes from children’s stories. Drama currently has two stories; Little Red Riding Hood (LRRH) in which Drama will put up a wolf’s head and chase the color red, and The Wizard of Oz (or simply Oz) in which he follows the yellow brick road until he encounters the Wicked Witch.

Executive Summary

Drama, in his current form, has a wooden circular chassis for simplified balance and easy maneuvering. The top portion of Drama is very clean: the CMUcam is attached to the front pointing forwards, battery packs are in the middle, wheels are on in the middle on each side and a caster wheel in the front, LCD is in the back, and status LEDs are next to the batteries. Underneath are the guts of Drama. The Mavric-II is mounted via screws and connected to all components underneath the chassis, except the CMUcam that has wires running through a hole to the top. The servos and SRFs are mounted on wooden inserts that can be easily removed. The line tracking sensor is mounted in an unusual fashion, using tiers of padding so that it can hang adjacent to the ground and have some flexibility.

Drama currently follows a light line on a dark background, including any twists and turns present. The differential steering of a two wheel setup allows for easy movement for turning in place and continuing on a line.

The sonars are place in the front angled slightly outward for a good range of coverage. They are currently set to a threshold, but relay data as analogue values that can be used in different contexts.

Introduction

The driving inspiration for Drama was actually from a video game. There is an encounter that changes each time you do it, and the event is based off of children’s stories. I liked the idea and saw ways that I could incorporate that into some of the fundamentals of robotics. I wanted something that I would have fun with, and Drama has the potential to be exactly that. This paper starts with a brief overview of the concept of Drama’s system and gradually gets more detailed into his exact components.

Integrated System

Figure-1 shows a high level flowchart of Drama’s thinking process. The initial decision of which scene will be acted out is based on color; red initiates LRRH mode and green initiates Oz mode.

[pic]

Figure-1: High Level Flowchart

Components

The main electronic components of Drama are listed here. More detailed information can be found in later sections of the report.

• Mavric-II development board, centered around an Atmega128.

• CMUcam for color recognition and tracking.

• Three-pair IR line tracking module to follow the yellow road.

• Sonar range finders for obstacle avoidance.

• Servo motors for robot movement and camera action (tilt/pan).

• Bump sensors for collision detection.

• LCD and LEDs for debugging purposes.

A block diagram of Drama’s major systems and communication paths is given below in Figure-2. Black lines represent processor inputs, red lines represent outputs, and green lines represent communication in both directions. The tilt/pan module was removed, and the camera is mounted stationary. Having a tilting camera was unnecessary and added another level of complexity (i.e. calculating the current angle of the camera to determine the movement direction).

[pic]

Figure-2: Drama’s major systems and communication directions.

Power

There are three separate power supplies for Drama. One is an unregulated 6V, which is used to power the microprocessor. The servo power supply is approximately 5.5V, and the CMUcam supply is slightly over 6V. Each component’s power supply is isolated from the other in case a component begins to draw excess current. For example if the servos became stalled, the current drawn would significantly increase, and possibly reset the microprocessor or camera. All supplies use NiCad rechargeable batteries.

Mobile Platform

In order to keep Drama balanced, the heavier parts are split up. The CMUcam and tilt/pan module need to be mounted in the front, so to balance, the wheels are placed mid-back and the LCD is mounted on the rear. The board and most of the wiring is underneath, providing a clean top. The battery packs are placed in the middle on the top, which gives Drama a solid center of mass. Some pictures of the platform are shown below in Figure-3.

[pic][pic]

Figure-3: Pictures of Drama

This was the first time I had ever designed anything using AutoCAD, so I think it turned out well for the most part. The end product was fairly easy to take apart and put back together, which was nice.

I designed the board based off of specifications given on datasheets and websites, unfortunately those weren’t 100% correct. The servos weren’t quite as long as they were listed, which caused the tires to end up slightly lopsided. The servos are attached to pieces of wood that are slid through the top of the platform, and the servos are then in turn slid through that piece of wood. While this made it easy to take apart and put back together, it wasn’t the sturdiest of all designs. Since the servos aren’t completely snug against the underside of the board, they have a tendency to slide up a little, causing the wheels to sort of ‘bow out’. A little electrical tape fixes the problem for the most part, but they are still slightly wobbly.

The IR detector module has a very short range, especially on off-white colors that I was using at first. The holders I originally designed weren’t long enough and I was forced to use a different method to allow the sensor to hang just above the ground.

Actuation

Movement

Drama’s movement is controlled by two continuous rotation servo motors. Servo motors were chosen because they are easy to use and sufficient for the desired application since speed is not a big consideration. The servos are controlled entirely with the hardware of the Mavric-II board. The system’s built in timers allow for simple PWM so very few additional calculations are needed. Servo code, along with all other code can be found in the appendix.

Since Drama operates indoors, the servos only needed a moderate amount of torque and rotation speed. The specs for the Futaba servos I used are given below:

Weight: 45.0 g

Output Torque: 3.4 kg-cm

Operating Speed at 4.8v 0.23 sec/60 degrees

Power Consumption: 6.0v/12mA at idle

Since the motors are operating at about 5.5V, they move at a higher speed than listed here.

One problem I came across, that I originally tried to stop motion by setting the output compare value to 0, thinking that since this stopped the pulsing, it would keep the servo from moving. Instead, the 0 value caused the servo to try to rotate to its ‘center’ position.

Since this was a hacked servo, it would inevitably overshoot that position and continue rotating to try and stop there again. Thus, using an output compare of 0 couldn’t be used to stop the servos. Instead, timer control registers had to be altered to disable the PWM signal from being sent at all. This worked for stopping the servos.

The tires are nothing special since Drama was made to run indoors. The tires are thin and light to keep Drama’s weight down, but they also have a decent size diameter to help with balance. Their exact measurements are 2.63” x 0.35”. The tires are shown below in Figure-4.

[pic]

Figure-4: Drama’s Tires

Sensors

CMUcam

CMUcam Basics

The main function that I will be using for the CMUcam is color tracking. Before we delve into that, however, let’s first look at some of the more basic CMUcam functions involved in setup.

-Packets

Type C packet – returned from Color Tracking command with Middle Mass off.

Type F packet – returned from the Dump Frame command.

Type M packet – returned from Color Tracking command with Middle Mass on.

Type N packet – same as type M with added info on servo position.

Type S packet – statistical information on the camera’s view.

The only packets we will be concerned with are Type M and Type S packets.

-Commands

➢ Carriage Return (/r) – Puts the camera in an idle state

➢ Reset (rs) – resets the camera to default options.

➢ Set camera internal registers (CR)

o Registers include: Contrast, Brightness, Color Mode, Clock Speed, and Auto Exposure.

➢ Set window (SW) – Sets the camera window size (80x143 max).

o Format - sw x1 y1 x2 y2

➢ Get mean (GM) – Returns the mean window color value (Type S packet).

o Returned Packet: S Rmean Gmean Bmean Rdev Gdev Bdev.

➢ Middle Mass (MM) – Turns Middle Mass mode on/off.

o MM 0 – Disengaged.

o MM 1 – Engaged (Default).

o MM 2 – Engaged and uses servo to track color (centers on camera view).

➢ Track Color (TC) – Tracks a given color, returns Type M or Type C packets.

o Format – TC Rmin Rmax Gmin Gmax Bmin Bmax.

o Return Packet: M mx my x1 y1 x2 y2 pixels confidence.

o Confidence > 50 indicates a good color lock.

-White Balance Feature

The white balance feature of the CMUcam adjusts color brightness so that the sum of its world appears grey. This is a common way to auto-adjust the camera when it changes lighting environments. One drawback to this feature is that if the camera’s view is dominated by a single color, it will adjust itself to make that color appear grey. The way that we solve this is to turn white balance on during start-up and allow the camera to auto-adjust for a period of about 5 seconds, after which we turn white balance off.

Communication

The CMUcam comes standard with level shifter serial communication ports, which is what is used to communicate with the microprocessor. Some information about the serial communication:

➢ The baud rate is set using jumpers on the camera, I set up the camera for a baud rate of 38,400 since this has a low error rate for a 16MHz clock.

➢ The data is sent and received as 8 Data bits and 1 stop bit.

➢ All data is sent using ASCII characters, spaces are used to separate parameters.

➢ All commands are followed by the carriage return character. After receiving a command, the camera responds with either an ACK or a NCK and the carriage return.

Difficulties

To this point, the CMUcam is not working as intended. The main problem I am getting is in communication. My test CMU code is in the appendix with the rest, but I haven’t been able to verify command recognition at this point. I have checked and double checked baud rates, command sequences, and connections to no avail. I have discovered some errors along the way, but none of these has let to a complete fix. One thing I noticed is that I originally had my baud multiplier off, so that was corrected. Also, when I initially hooked up the camera to the Mavric-II, I didn’t realize there were ‘activation switches’ to turn on the built in level shifter. While this did not in itself fix the problem, it might point me in a possible direction, albeit a bad one. If the board wasn’t set up to receive +- 12V and the camera tried to send a signal of that magnitude, I may have damaged the MAX232, which could be the problem. As I move forward with the project that is the next thing I am going to check, and will keep moving on from there.

Sonar Range Finders

The SRF05 is the main obstacle avoidance module of Drama. The SRF05 operates at 5V DC and is based on trigger-echo mechanics. Drama has two range finders mounted at his front in a crossing pattern to ensure full coverage. The ‘max’ range of the SRF05 is listed at 4m.

[pic]

Figure-5: Sonar Range Finder

Trigger-Echo Mechanics

The SRF05 receives a trigger pulse of approximately 10ms width as its start signal. The SRF05 then sends out pulses from its emitter and waits for a response at the detector. After the pulses are sent out from the SRF05, the echo line is raised high. When it detects something at the receiver, it the echo line is lowered. Thus you can tell how far away something is by the length of the pulse from the echo line. A typical trigger, pulse, echo sequence is shown below in Figure-6.

[pic]

The 10ms pulse is sent to the SRF05 using the Atmega128’s internal counters, very similar to the servos. The trigger for each of the SRF05s can be hooked up to the same output compare line on the Mavric-II so we can control both with the same algorithm.

In continuing with interrupt controlled I/O, the echo pin is hooked up to two external interrupt pins on the Mavric-II. My goal here is to measure the pulse width, so for that I need to interrupt on both the rising and falling edge of the echo line. The external interrupt pins on the Atmega128 can only be set to interrupt on one edge or the other, so to measure both I have to connect them to one that is set to rising edge trigger, and one that is set to falling edge trigger. The difference in the timer values when the interrupts occur gives us the pulse width, which in turn tells us the distance to the object (if there is one). Some measurements are given later in the results section.

One thing that hung me up for awhile was the way that variables work between interrupts and main using the AVR GCC. When a variable is created, many times it is stored in a register for easy access. When an interrupt occurs, all registers are pushed onto the stack to save the computer’s state. In my application, I have global variables for the start time, the end time, and the difference for the echo pulse. These variable were getting pushed onto the stack for interrupts, so when these values were modified during the interrupt, those modifications were discarded when the state was restored. In order to fix this, all global variable shared between interrupt service routines (ISRs) and Main, must be preempted by the keyword volatile. This prevents the processor from storing the variables in registers.

IR Line Tracker

For line following, a Lynxmotion TRA-01 is used. The tracker has three IR emitters and detectors with status indicator LEDs. When the detector senses the reflected IR wave, it raises it’s line high. If no reflection is detected, the line stays low.

[pic]

Figure-7: IR Line Tracking Module

The Atmega takes the signals from the IR detector and adjusts Drama’s movement accordingly. The uP continuously keeps track of the previous IR values so that when it gets to a turn it knows which way to go. For example, on a right turn, the left IR should be dark and the right IR should be light, so when Drama completely loses the line, it checks these past values and turns right.

Since the premise of Drama is to follow a yellow road, I built a path with a light line on a dark background. One problem I had with the sensor is the detection range. The line sensor has to be no more than a few centimeters off the ground or else it won’t be able to detect the line. My original mounting scheme did not have it this close, so I was forced to change the way it was mounted to allow it to be closer to the ground.

I planned to use a yellow line, but the IR detector is very sensitive and became inconsistent when using the color yellow. White works much better, so for demonstration purposes, a white line on a black background is used. Some light yellow coloring would probably be fine as well.

Another problem was dealing with faulty values, such as when Drama thinks he loses the line for a few clock cycles. This would cause Drama to turn when he shouldn’t. This was solved by adding a variable that counted when all signals registered dark to make sure that there was a ‘significant’ time without detecting a line.

Behaviors

These are the planned behaviors for Drama, although currently the ones that involve the CMU are not working.

Go Home

Go Home is Drama’s start up behavior. The basis of this behavior is for Drama to be able to return himself to a starting location, the beginning of the yellow brick road. When he reaches home, he enters Sentry.

Sentry

Sentry is the basic scanning mode where drama looks for color for confirmation of which scene to act out. The CMUcam will pan back and forth looking for color confirmation to begin.

LRRH

The LRRH behavior follows the color red, provided the camera can find that color. If no red is detected, the Drama will scan his surroundings looking for red.

OZ

The OZ behavior is the yellow line (road) following behavior.

Obstacle Avoidance

Random wandering avoiding obstacles along the way.

Experimental Layout and Results

CMUcam

While the CMUcam is currently not working with the processor, I was able to connect it to the computer and get some test values for the color tracking. These results are shown in Table-1.

|Track Color Settings |Confidence |

|Red |Green |Blue |Red |White |Black |

|Min |Max |Min |

| |2ft |6ft |2ft |6ft |

|Rm |129 |106 |145 |140 |

|Gm |29 |50 |30 |55 |

|Bm |33 |62 |31 |60 |

|Rdev |5.5 |19 |7 |19 |

|Gdev |4 |33 |4 |41 |

|Bdev |4 |26 |5 |40 |

Table-2: White Balance Test Results

SRF05

Table-3 shows the test data for the pulse length recorded with objects at a given distance away. This data can be used to either set a threshold for turning, a digital approach, or to slowly turn if it sees something in the distance, a more analog approach.

|Distance |1ft |2ft |4ft |8ft |

| | | | | |

|Trial | | | | |

|1 |320 |651 |1185 |2175 |

|2 |316 |677 |1182 |2234 |

|3 |333 |650 |1180 |2190 |

|4 |321 |644 |1195 |2202 |

|5 |326 |649 |1189 |2154 |

Table-3: SRF05 Test Values

Conclusion

When I first started working on Drama, I had visions or all the different behaviors I could incorporate into him. I still love the idea and want to continue adding on to him. As he stands now, I think the hardest part, at least for me, of Drama is finished. Mechanical engineering is not my cup of tea, so things such as designing a quality board, working with locomotion, and balancing the robot were the things that took the most to get myself to work on. Those are pretty much done and gone, although I would like to make another revision to the chassis. The majority of enhancements left to be done to Drama are EE focused, and are the type of thing I would enjoy working on in my free time. While I may not have been enthused with some aspects of the project, they did teach me more, and makes it that much easier to do them again when the time comes.

Finishing the camera and color tracking are the first things that I plan to do as I continue with Drama. Debugging something of that nature is difficult in that there is very little information to go by. From there I will be adding some props to Drama. I purchased a fake wolf head that will be used for the LRRH behavior and am looking for some shoes for OZ. One more behavior that I would like to try is Cinderella, performing a waltz before running away. This is the type of project where it is fairly easy to add on more and more.

The parts that are finished turned out very well. As I said before, one of my goals in this project was to focus more on interrupt drive I/O to free up processor time. I was able to control locomotion and obstacle detection entirely with interrupts, and the CMUcam code that I have written is entirely USART interrupt driven as well. In the past I have relied a little too much on polling and I think this is a step in the right direction of quality.

If I were starting this project anew, the biggest thing I would do differently is to make better use or the TAs. I’m kind of hard headed when it comes to a lot of things. I enjoy teaching myself and figuring things out for myself. While this may be good for remembering what you learned and not making the same mistake twice, it does make the process take longer that it otherwise would. During a summer semester, time is of the essence.

Documentation

CMUcam User’s Manual v2.00

SRF05 Technical Documentation



Parralax Continuous Rotation Servo Data



Mavric-IIb User’s Manual



4-Bit-LCD Notes



Lynxmotion Line Tracking Sensor User’s Manual v5.0



Atmega128 Datasheet



Miscellaneous AVR info



Appendix - Code

Obstacle Avoidance

#include

#include

/**********************************************************************

Obstacle Avoidance Code:

Servos Controlled Via Timer1

Sonar Range Finders Controlled Via Timer3

***********************************************************************/

//Function Declaration

void InitializeLT();

void InitializeSRF();

void InitializeServo();

void Servo(uint8_t Side,uint8_t Direction);

void OZ();

//Defines

#define LEFT 0

#define RIGHT 1

#define BACK 0

#define FORWARD 1

#define STOP 2

#define IRdir DDRA

#define IRin PINA

#define IRout PORTA

#define LEFTEYE (PINA & 0x01)

#define MIDEYE (PINA & 0x02)

#define RIGHTEYE (PINA & 0x04)

#define LEFTDARK (LEFTEYE == 0x00)

#define LEFTLIGHT (LEFTEYE == 0x01)

#define MIDDARK (MIDEYE == 0x00)

#define MIDLIGHT (MIDEYE == 0x02)

#define RIGHTDARK (RIGHTEYE == 0x00)

#define RIGHTLIGHT (RIGHTEYE == 0x04)

//Global Variables

volatile unsigned int SRFstartTimeL,SRFendTimeL,SRFstartTimeR,SRFendTimeR;

volatile int SRFresponseTimeL,SRFresponseTimeR;

int main()

{

InitializeServo();

InitializeSRF();

while(1)

{

if(1)//LEFTDARK & MIDDARK & RIGHTDARK)

{

if((SRFresponseTimeL < 600) & (SRFresponseTimeR > 600))

{

Servo(LEFT,STOP);

Servo(RIGHT,FORWARD);

}

else if((SRFresponseTimeL > 600) & (SRFresponseTimeR < 600))

{

Servo(RIGHT,STOP);

Servo(LEFT,FORWARD);

}

else if((SRFresponseTimeL < 600) & (SRFresponseTimeR < 600))

{

Servo(LEFT,BACK);

Servo(RIGHT,FORWARD);

}

else

{

Servo(LEFT,FORWARD);

Servo(RIGHT,FORWARD);

}

}

else

{

OZ();

}

}

return 0;

}

void InitializeLT()

{

IRdir &= 0xF8; //PA2:0 Inputs

}

void OZ()

{

while(1)

{

if(MIDLIGHT & LEFTDARK)

{

Servo(LEFT,FORWARD);

Servo(RIGHT,FORWARD);

}

else if(LEFTLIGHT & MIDDARK)

{

Servo(RIGHT,FORWARD);

Servo(LEFT,STOP);

}

else if(LEFTLIGHT & MIDLIGHT)

{

Servo(LEFT,FORWARD);

Servo(RIGHT,BACK);

}

else if(MIDLIGHT & RIGHTDARK)

{

Servo(LEFT,FORWARD);

Servo(RIGHT,FORWARD);

}

else if(MIDDARK & RIGHTLIGHT)

{

Servo(LEFT,FORWARD);

Servo(RIGHT,STOP);

}

else if(RIGHTLIGHT & MIDLIGHT)

{

Servo(RIGHT,FORWARD);

Servo(LEFT,BACK);

}

else

{

Servo(LEFT,FORWARD);

Servo(RIGHT,BACK);

}

}

}

void InitializeServo()

{

cli();

DDRB |= 0xC0; //Set PB7 and PB6 for outputs

TCCR1A = 0x2B; //COM1B 1:0 = 10

//COM1C 1:0 = 10

//WGM1 1:0 = 11

TCCR1B = 0x1A; //WGM1 3:2 = 11

//CS1 2:0 = 010

OCR1A = 40000; //Set OCR1A to 40000 for 20ms overflow

TIMSK |= 0x08; //Enable OCIE1B and OCIE1C

ETIMSK |= 0x01;

sei();

}

/**************************************************************

Initializes SRF

WGM = Fast PWM with TOP stored in OCR3A

COM = Clear on OC, Set on BOTTOM

Sets input capture to rising edge

Clock = F_CPU/64 = 250kHz

For second SRF External Interrupt 0 and 1 are used.

************************************************************/

void InitializeSRF()

{

cli();

OCR3A = 13000; //Slightly over 50ms Period

OCR3C = 10; //20us Ping Pulse Width

//PE5 is output

DDRE |= 0x20;

//PD0:3 are INT0:3 -> Set to Input

DDRD &= 0xF0;

TCCR3A = 0x0B;

TCCR3B = 0x1B;

EICRA |= 0xBB; //ISC01:0 = 11 = interrupt on rising edge.

//ISC11:0 = 10 = interrupt on falling edge.

//ICS21:0 = 11 = int on rising edge

//ICS31:0 = 10 = int on falling edge

ETIMSK |= 0x02; //Enable Interrupts

EIMSK |= 0x0F;

//TIMSK |= 0x20;

sei();

}

void Servo(uint8_t Side,uint8_t Direction)

{

cli();

if((Side == LEFT) & (Direction == FORWARD))

{

TCCR1A |= 0x08;

OCR1C = 2000;

}

else if((Side == LEFT) & (Direction == BACK))

{

TCCR1A |= 0x08;

OCR1C = 4000;

}

else if((Side == LEFT) & (Direction == STOP))

TCCR1A &= 0xF3;

//OCR1C=0;

else if((Side == RIGHT) & (Direction == FORWARD))

{

TCCR1A |= 0x20;

OCR1B = 4000;

}

else if((Side == RIGHT) & (Direction == BACK))

{

TCCR1A |= 0x20;

OCR1B = 2000;

}

else if((Side == RIGHT) & (Direction == STOP))

TCCR1A &= 0xCF;

//OCR1B=0;

sei();

//return;

}

ISR(INT0_vect)

{

SRFstartTimeL = TCNT3;

EIFR |= 0x01; //Clear Flag

}

ISR(INT1_vect)

{

SRFendTimeL = TCNT3;

SRFresponseTimeL = SRFendTimeL - SRFstartTimeL;

if(SRFresponseTimeL < 0)

SRFresponseTimeR += 13000;

EIFR |= 0x02; //clear flag

}

ISR(INT2_vect)

{

SRFstartTimeR = TCNT3;

EIFR |= 0x01; //Clear Flag

}

ISR(INT3_vect)

{

SRFendTimeR = TCNT3;

SRFresponseTimeR = SRFendTimeR - SRFstartTimeR;

if(SRFresponseTimeR < 0)

SRFresponseTimeR += 13000;

EIFR |= 0x02; //clear flag

}

ISR(TIMER1_COMPB_vect){}

ISR(TIMER1_COMPC_vect){}

ISR(TIMER3_COMPC_vect){}

Line Following

#include

#include

/**********************************************************************

Line Following Code

Servos Controlled Via Timer1

***********************************************************************/

//Function Declaration

void InitializeLT();

void InitializeSRF();

void InitializeServo();

void Servo(uint8_t Side,uint8_t Direction);

void OZ();

//Defines

#define LEFT 0

#define RIGHT 1

#define BACK 0

#define FORWARD 1

#define STOP 2

#define IRdir DDRA

#define IRin PINA

#define IRout PORTA

#define LEFTEYE (PINA & 0x01)

#define MIDEYE (PINA & 0x02)

#define RIGHTEYE (PINA & 0x04)

#define LEFTDARK (LEFTEYE == 0x00)

#define LEFTLIGHT (LEFTEYE == 0x01)

#define MIDDARK (MIDEYE == 0x00)

#define MIDLIGHT (MIDEYE == 0x02)

#define RIGHTDARK (RIGHTEYE == 0x00)

#define RIGHTLIGHT (RIGHTEYE == 0x04)

#define PREVLEFTLIGHT (PREVLEFT == 0x01)

#define PREVLEFTDARK (PREVLEFT == 0X00)

#define PREVMIDLIGHT (PREVLEFT == 0x02)

#define PREVMIDDARK (PREVLEFT == 0X00)

#define PREVRIGHTLIGHT (PREVLEFT == 0x04)

#define PREVRIGHTDARK (PREVLEFT == 0X00)

//Global Variables

volatile unsigned int SRFstartTimeL,SRFendTimeL,SRFstartTimeR,SRFendTimeR;

volatile int SRFresponseTimeL,SRFresponseTimeR;

volatile uint8_t PREVLEFT,PREVRIGHT,PREVMID;

volatile double Lost=0;

int main()

{

InitializeServo();

InitializeSRF();

OZ();

return 0;

}

void InitializeLT()

{

IRdir &= 0xF8; //PA2:0 Inputs

}

void OZ()

{

while(1)

{

if(MIDLIGHT)

{

Servo(LEFT,FORWARD);

Servo(RIGHT,FORWARD);

Lost = 0;

}

else if(RIGHTLIGHT)

{

Servo(LEFT,FORWARD);

Servo(RIGHT,STOP);

Lost = 0;

}

else if(LEFTLIGHT)

{

Servo(RIGHT,FORWARD);

Servo(LEFT,STOP);

Lost = 0;

}

else if(LEFTDARK & MIDDARK & RIGHTDARK)

{

Lost += 1;

if(Lost == 3000)

{

if(PREVLEFTLIGHT)

{

Servo(RIGHT,FORWARD);

Servo(LEFT,BACK);

Lost = 0;

}

else

{

Servo(LEFT,FORWARD);

Servo(RIGHT,BACK);

Lost = 0;

}

}

}

else

{

Servo(LEFT,FORWARD);

Servo(RIGHT,FORWARD);

}

if(LEFTLIGHT | RIGHTLIGHT)

{

PREVLEFT = LEFTEYE;

PREVMID = MIDEYE;

PREVRIGHT = RIGHTEYE;

}

}

}

void InitializeServo()

{

cli();

DDRB |= 0xC0; //Set PB7 and PB6 for outputs

TCCR1A = 0x2B; //COM1B 1:0 = 10

//COM1C 1:0 = 10

//WGM1 1:0 = 11

TCCR1B = 0x1A; //WGM1 3:2 = 11

//CS1 2:0 = 010

OCR1A = 40000; //Set OCR1A to 40000 for 20ms overflow

TIMSK |= 0x08; //Enable OCIE1B and OCIE1C

ETIMSK |= 0x01;

sei();

}

/**************************************************************

Initializes SRF

WGM = Fast PWM with TOP stored in OCR3A

COM = Clear on OC, Set on BOTTOM

Sets input capture to rising edge

Clock = F_CPU/64 = 250kHz

For second SRF External Interrupt 0 and 1 are used.

************************************************************/

void InitializeSRF()

{

cli();

OCR3A = 13000; //Slightly over 50ms Period

OCR3C = 10; //20us Ping Pulse Width

//PE5 is output

DDRE |= 0x20;

//PD0:3 are INT0:3 -> Set to Input

DDRD &= 0xF0;

TCCR3A = 0x0B;

TCCR3B = 0x1B;

EICRA |= 0xBB; //ISC01:0 = 11 = interrupt on rising edge.

//ISC11:0 = 10 = interrupt on falling edge.

//ICS21:0 = 11 = int on rising edge

//ICS31:0 = 10 = int on falling edge

ETIMSK |= 0x02; //Enable Interrupts

EIMSK |= 0x0F;

//TIMSK |= 0x20;

sei();

}

void Servo(uint8_t Side,uint8_t Direction)

{

cli();

if((Side == LEFT) & (Direction == FORWARD))

{

TCCR1A |= 0x08;

OCR1C = 2000;

}

else if((Side == LEFT) & (Direction == BACK))

{

TCCR1A |= 0x08;

OCR1C = 4000;

}

else if((Side == LEFT) & (Direction == STOP))

TCCR1A &= 0xF3;

//OCR1C=0;

else if((Side == RIGHT) & (Direction == FORWARD))

{

TCCR1A |= 0x20;

OCR1B = 4000;

}

else if((Side == RIGHT) & (Direction == BACK))

{

TCCR1A |= 0x20;

OCR1B = 2000;

}

else if((Side == RIGHT) & (Direction == STOP))

TCCR1A &= 0xCF;

//OCR1B=0;

sei();

//return;

}

ISR(INT0_vect)

{

SRFstartTimeL = TCNT3;

EIFR |= 0x01; //Clear Flag

}

ISR(INT1_vect)

{

SRFendTimeL = TCNT3;

SRFresponseTimeL = SRFendTimeL - SRFstartTimeL;

if(SRFresponseTimeL < 0)

SRFresponseTimeR += 13000;

EIFR |= 0x02; //clear flag

}

ISR(INT2_vect)

{

SRFstartTimeR = TCNT3;

EIFR |= 0x01; //Clear Flag

}

ISR(INT3_vect)

{

SRFendTimeR = TCNT3;

SRFresponseTimeR = SRFendTimeR - SRFstartTimeR;

if(SRFresponseTimeR < 0)

SRFresponseTimeR += 13000;

EIFR |= 0x02; //clear flag

}

ISR(TIMER1_COMPB_vect){}

ISR(TIMER1_COMPC_vect){}

ISR(TIMER3_COMPC_vect){}

LCD Code

#include

#include

//For delay.h, F_CPU is defined in the header file as 16MHz for use with

//delay functions.

//4-bit LCD code

/* LCD Pinout

1: GND

2: VCC

3: Contrast Vo

4: RS

5: R/W

6: E

7: DB4

8: DB5

9: DB6

10: DB7

uP Port Map

0: DB4

1: DB5

2: DB6

3: DB7

4: RS

5: R/W

6: E

7: NC

*/

//LCD Header is PortC of Mavric-II

#define LCDout PORTC

#define LCDdir DDRC

#define Enable 0x40

//Function Declarations

void InitializeLCD();

void NibbleToLCD(uint8_t LCDregister, uint8_t LCDdata);

void ByteToLCD(uint8_t LCDregister, uint8_t LCDdata);

void StringToLCD(uint8_t *pString);

void ClearLCD();

//Global Variables

uint8_t Command = 0,Data = 1;

uint8_t LCDposition = 0;

int main()

{

InitializeLCD();

ClearLCD();

while(1)

{

StringToLCD("Testing");

}

return 0;

}

void InitializeLCD()

{

LCDdir = 0x7F; //Set all pins for LCD to outputs

_delay_ms(15); //Power Up Delay

//4-Bit Mode Enable

NibbleToLCD(Command,0x03);

_delay_ms(5);

NibbleToLCD(Command,0x03);

_delay_us(150);

NibbleToLCD(Command,0x03);

_delay_ms(5);

NibbleToLCD(Command,0x02);

_delay_us(75);

//2-Line Mode

NibbleToLCD(Command,0x02);

_delay_ms(2);

NibbleToLCD(Command,0x08);

_delay_us(75);

//Display,Cursor,Blink

NibbleToLCD(Command,0x00);

_delay_ms(2);

NibbleToLCD(Command,0x0F);

_delay_us(75);

//Clear Home

NibbleToLCD(Command,0x00);

_delay_ms(2);

NibbleToLCD(Command,0x01);

_delay_ms(2);

return;

}

void NibbleToLCD(uint8_t LCDregister,uint8_t LCDdata)

{

//Pull enable high

LCDout |= Enable;

//Set upper 4 bits of the data to zero since they aren't used

LCDdata &= 0x0F;

//Check if it is a command or data and set RS appropriately

if(LCDregister == Command)

LCDout |= (0x00 | LCDdata);

else

LCDout |= (0x10 | LCDdata);

//Clear enable to latch

LCDout &= (!Enable);

return;

}

void ByteToLCD(uint8_t LCDregister,uint8_t LCDdata)

{

uint8_t LCDdataLow = (LCDdata & 0x0F);

uint8_t LCDdataHigh = (LCDdata >> 4);

//Check if LCD is at the end of first line, if so go to next line

if(LCDposition == 16)

ByteToLCD(Command,0xC0);

else if(LCDposition == 32)

ClearLCD();

//Write Nibbles to LCD, first high then low

NibbleToLCD(LCDregister,LCDdataHigh);

_delay_ms(2);

NibbleToLCD(LCDregister,LCDdataLow);

_delay_us(75);

//Increment LCDposition on Byte Write

if(LCDregister == Data)

LCDposition++;

return;

}

void StringToLCD(uint8_t *pString)

{

while(*pString)

ByteToLCD(Data,*pString++);

return;

}

void ClearLCD()

{

ByteToLCD(Command,0x01);

LCDposition = 0;

Return;

CMUcam Testing Code

//CMUcam Test Program - Red Following

#include

#include

#include

void InitializeUART();

void InitializeCMU();

char WaitForAck();

#define CarRet 13

#define NUL 0

#define ACK 6

#define Setup 0

volatile char WhiteBalOn[] = "TW";//"CR 18 44";

volatile char WhiteBalOff[] = "CR 18 50";

volatile char MidMassOn[] = "MM 1";

volatile char Response;

volatile char *CurrentTransmit;

volatile int SetupFlag = 0;

volatile uint8_t Stage;

int main()

{

_delay_ms(5); //RS232 Startup delay

InitializeUART();

UDR0 = CarRet;

while(1);

InitializeCMU();

return 0;

}

/******************************************************************

Initialize the UART to a baud rate of 38,400, with 8 data bits,

1 stop bit and parity disabled.

*****************************************************************/

void InitializeUART()

{

cli(); //Clear Global interrupts while setting up

UBRR0L = 25; //Set Baud Rate to 38,400

UCSR0C = 0x06; //Set data to 8 bits, 1 stop bit, parity disabled

UCSR0B = 0xD8;//D8 //Enable Xmit, Rec, Xmit comp Interrupt,RXcomp Int.

sei(); //Set Global Interrupts back on

}

void InitializeCMU()

{

Stage = Setup;

while(1)

{

SetupFlag = 1;

//UCSR0B = 0x48; //Enable transmit complete interrupt, turn on transmitter.

CurrentTransmit = &WhiteBalOn[0]; //Initialize the CMU start up pointer

UDR0 = *CurrentTransmit; //Send out the first character

while(SetupFlag != 1); //Loop until First xmit Complete

UCSR0B &= 0xBF; //Disable xmit complete interrupt

while(SetupFlag == 1);

if(Response == ACK)

break;

}

_delay_ms(5000); //5 Second Delay for White Blance

while(1)

{

SetupFlag = 0;

//UCSR0B = 0x48; //Enable transmit complete interrupt, turn on transmitter.

CurrentTransmit = &WhiteBalOff[0]; //Initialize the CMU start up pointer

UDR0 = *CurrentTransmit; //Send out the first character

while(SetupFlag != 1); //Loop until xmit Complete

if(Response == ACK)

break;

}

while(1)

{

SetupFlag = 0;

//UCSR0B = 0x48; //Enable transmit complete interrupt, turn on transmitter.

CurrentTransmit = &MidMassOn[0]; //Initialize the CMU start up pointer

UDR0 = *CurrentTransmit; //Send out the first character

while(SetupFlag != 1); //Loop until First xmit Complete

char Response = WaitForAck(); //Wait for Ack Response

if(Response == ACK)

break;

}

return;

}

char WaitForAck()

{

//UCSR0B &= 0xF7; //Turn off transmitter

//UCSR0B |= 0x10; //Turn on receiver

while((UCSR1A & 0x80) == 0x00); //Wait for Receive complete flag

return UDR0;

}

ISR(USART0_TX_vect)

{

CurrentTransmit++; //Increment Pointer

if(*CurrentTransmit == NUL) //If at the end of a string, send the /r character

{

UDR0 = 0x00;

SetupFlag = 1;

}

else

UDR0 = *CurrentTransmit; //otherwise send the character at the pointer

}

ISR(USART0_RX_vect)

{

if(Stage == Setup)

{

Response = UDR0;

SetupFlag = 0;

}

}

................
................

In order to avoid copyright disputes, this page is only a partial summary.

Google Online Preview   Download