LineFollow.h
#ifndef _LineFollow_H_
#define _LineFollow_H_
char AorB(unsigned char DC);
/*Moves forward until one of the front sensors is tirggered
returns what side of the playfield robot is on 'A' or 'B'*/
void LineFollow(unsigned char DC);
/*Start on line, follows line to end assuming no 90 degree corner*/
void GetonLine_Left(unsigned char DC);
/*Start off line, moves until robot is on line and turns left onto it*/
void GetonLine_Right(unsigned char DC);
/*Start off line, moves until robot is on line and turns right onto it*/
void BackUp(unsigned char DC, unsigned int t);
/*Drive backwards at given duty cycle for given number of miliseconds*/
void GetCenteronLine(unsigned char DC);
/*Drives robot forward until back sensor (center) sees a line*/
void TurnRight (unsigned char DC);
/*Call after center sensor is on line, turns robot in place until its on the line
facing right*/
void TurnLeft(unsigned char DC);
/*Call after center sensor is on line, turns robot in place until its on the line
facing left.*/
#endif
LineFollow.c
#include "MotorControl.h"
#include "LineFollow.h"
#include <hidef.h> /* common defines and macros */
#include <mc9s12e128.h> /* derivative information */
#include <S12E128bits.h> /*E128 bit definitions */
#include <bitdefs.h> /* BIT0HI, BIT0LO definitions */
#include <stdio.h>
#include <TIMERS12.h>
#include "Startup.h"
//~~~~~~~~~~~~~~~~~~ GetonLine_Right ~~~~~~~~~~~~~~~~~~~~~~~
/*Drives robot forward until line is detected on center sensor the turns
right onto the line*/
void GetonLine_Right(unsigned char DC)
{
GetCenteronLine(DC);
TurnRight(DC);
return;
}
//~~~~~~~~~~~~~~~~~~ GetonLine_Left ~~~~~~~~~~~~~~~~~~~~~~~
/*Drives robot forward until line is detected on center sensor the turns
left onto the line*/
void GetonLine_Left(unsigned char DC)
{
GetCenteronLine(DC);
TurnLeft(DC);
return;
}
//~~~~~~~~~~~~~~~~~ LineFollow ~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Follows a line until both front sensors hit a line (end of line)*/
void LineFollow(unsigned char DC)
{
char Right;
char Center;
char Left;
int i;
int j;
Right = (PTT & BIT1HI);
Center = (PTT & BIT4HI);
Left = (PTT & BIT0HI);
for(j=0; j<100;j++)
{
while ((Right==0) || (Left==0))
{
for (i=0;i<100;i++);
Right = (PTT & BIT1HI);
Center = (PTT & BIT4HI);
Left = (PTT & BIT0HI);
if ((Right==0) && (Left==0)) //straight
{
RightWheelForward(DC);
LeftWheelForward(DC);
}
else if((Left && (Right==0)) && (Center==0))
{
RightWheelForward(DC);
StopLeft();
}
else if ((Right && (Left==0)) && (Center==0))
{
LeftWheelForward(DC);
StopRight();
}
else if ((Left && (Right==0)) && Center) // left sensor on line
{
RightWheelForward(25);
LeftWheelBackward(25);
}
else if ((Right && (Left==0)) && Center) // if drifts left
{
LeftWheelForward(25);
RightWheelBackward(25);
}
else if (((Right && Left)))
{
StopRight();
StopLeft();
}
}
}
StopRight();
StopLeft();
return;
}
//~~~~~~~~~~~~~~~~~~~~~ AorB ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Drives forward robot out of start box then drives until one of the
front tape sensors sees the line from goal 1 (stops robot). Returns what side of the
playing field robot is on depending on which tape sensor triggers first.*/
char AorB(unsigned char DC)
{
char Right;
char Left;
char i;
char j=0;
char NumMax=10;
Right = (PTT & BIT1HI);
Left = (PTT & BIT0HI);
//drive forward out of box
RightWheelForward(DC);
LeftWheelForward(DC);
delay(1000);
//start looking for tape on front sensors
while (j<NumMax)
{
while ((Right==0) && (Left==0))
{
Right = (PTT & BIT1HI);
Left = (PTT & BIT0HI);
for (i=0;i<100;i++);
}
j++;
}
StopRight();
StopLeft();
//return side robot is on
if (Right)
{
return 'B';
}
else if (Left)
{
return 'A';
}
}
//~~~~~~~~~~~~~~~~~~~~~~~ GetCenteronLine ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Drives robot forward until center sensor sees a line*/
void GetCenteronLine(unsigned char DC)
{
char Center;
int i;
int j;
Center = (PTT & BIT4HI);
RightWheelForward(DC);
LeftWheelForward(DC);
for(j=0;j<10;j++)
{
while((Center==0))//going to line
{
for(i=0;i<100;i++);
Center = (PTT & BIT4HI);
}
}
StopRight();
StopLeft();
return;
}
//~~~~~~~~~~~~~~~~~~~~ TurnLeft ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Call after GetCenteronLine, turns robot left until tape sensors
straddle line*/
void TurnLeft (unsigned char DC)
{
char Left;
int i;
int j;
Left = (PTT & BIT0HI);
RightWheelForward(DC);
LeftWheelBackward(DC);
for(j=0;j<100;j++)
{
while((Left==0)) //turn until bot sees line on left sensor
{
for (i=0;i<100;i++);
Left=(PTT & BIT0HI);
}
}
for(j=0;j<100;j++)
{
while(Left) //keep turning until left sensor leaves line
{
for (i=0;i<100;i++);
Left=(PTT & BIT0HI);
}
}
StopRight();
StopLeft();
return;
}
//~~~~~~~~~~~~~~~~~~~~ TurnRight ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Call after GetCenteronLine, turns robot right until tape sensors
straddle line*/
void TurnRight(unsigned char DC)
{
char Right;
int i;
int j;
Right= (PTT & BIT1HI);
LeftWheelForward(DC);
RightWheelBackward(DC);
for(j=0;j<100;j++)
{
while((Right==0)) //turn until bot sees line on right sensor
{
for (i=0;i<100;i++);
Right=(PTT & BIT1HI);
}
}
for(j=0;j<100;j++)
{
while(Right) //keep turning until right sensor leaves line
{
for (i=0;i<100;i++);
Right=(PTT & BIT1HI);
}
}
StopRight();
StopLeft();
return;
}
//~~~~~~~~~~~~~~~~~~~~~~ BackUp ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Drives robot backwards at given duty cycle for given number of
miliseconds*/
void BackUp(unsigned char DC, unsigned int t)
{
RightWheelBackward(DC);
LeftWheelBackward(DC);
delay(t);
StopRight();
StopLeft();
return;
}