Goals.h
#ifndef _GOALS_H_
#define _GOALS_H_
void Goal1(void);
/*Drives forward slowly and drops balls in goal 1*/
void Goal3(void);
/*Drives forward slowly and drops balls in goal 3*/
#endif
Goals.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"
#include "MotorControl.h"
#include "BallDrop.h"
#include "LineFollow.h"
#include "Goals.h"
//~~~~~~~~~~~~~~~~~~~~ Goal3 ~~~~~~~~~~~~~~~~~~~~~~~
/*Drives robot slowly forward until contact switch is pressed
then releases balls*/
void Goal3(void)
{
int i;
int j;
char Switch;
RightWheelForward(30);
LeftWheelForward(30);
Switch = (PTU & BIT5HI);
for(j=0; j<100;j++)
{
while ((Switch==0))
{
for (i=0;i<100;i++);
Switch = (PTU & BIT5HI);
}
}
StopRight();
StopLeft();
BallDrop();
BackUp(50,500);
return;
}
//~~~~~~~~~~~~~~~~~~~~~ Goal1 ~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Drives robot forward until off the end of the tape, drops balls then
drives backwards until back on tape. This ensures that the balls roll out of goal 1*/
void Goal1(void)
{
int i;
int j;
char Right;
char Left;
RightWheelForward(30);
LeftWheelForward(30);
Right = (PTT & BIT1HI);
Left = (PTT & BIT0HI);
for(j=0; j<100;j++)
{
while (Right || Left)
{
for (i=0;i<100;i++);
Right = (PTT & BIT1HI);
Left = (PTT & BIT0HI);
}
}
StopRight();
StopLeft();
BallDrop();
RightWheelBackward(30);
LeftWheelBackward(30);
Right = (PTT & BIT1HI);
Left = (PTT & BIT0HI);
for(j=0; j<100;j++)
{
while (Right==0 || Left==0)
{
for (i=0;i<100;i++);
Right = (PTT & BIT1HI);
Left = (PTT & BIT0HI);
}
}
StopRight();
StopLeft();
BackUp(30,500);
return;
}