GoingPlaces.h
#ifndef _GoingPlaces_H_
#define _GoingPlaces_H_
void FirstDispenser(char Side);
/*Run this after determining what side the robot is on. Drives robot to
ball dispenser*/
void DispTo3(char Side, char backup);
/*Drives robot from dispenser to goal 3.
Set back up equal to one to make robot back up slightly to drop balls
in a different part of the goal.*/
void G3ToDisp(char Side);
/*Drives robot from goal 3 to dispenser*/
void DispTo1(char Side);
/*Drives robot from dispenser to goal 1*/
void G1ToDisp(char Side);
/*Drives robot from goal 1 to dispenser*/
#endif
GoingPlaces.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 "BallDrop.h"
#include "IRbeacon.h"
#include "MotorControl.h"
#include "LineFollow.h"
#include "Goals.h"
//~~~~~~~~~~~~~~~~~~ FirstDispenser ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Drives motor up to the dispenser from starting area and requests 5 balls
Call this after side is determined by AorB()*/
void FirstDispenser(char Side)
{
//drive up to line leading away from goal 1
GetCenteronLine(65);
if(Side=='A')
{
//drive past line
//arc left to not hit dispenser
RightWheelForward(50);
LeftWheelForward(70);
delay(600);
//drive up to line heading towards dispenser and turn left on it
GetonLine_Left(50);
}
else if(Side=='B')
{
//drive past line
//arc right to not hit dispenser
LeftWheelForward(50);
RightWheelForward(70);
delay(600);
//drive up to line heading towards dispenser and turn right on it
GetonLine_Right(50);
}
//line follow up to dispenser and get 5 balls
LineFollow(40);
CollectBall();
return;
}
//~~~~~~~~~~~~~~~~ Dispto3 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Call when at dispenser, drives robot to goal 3 and drops balls in goal*/
void DispTo3(char Side, char backup)
{
BackUp(50,300);
if(Side=='A')
{
FindBeacon(85,LEFT);
//Run forward enough to get off of black line
//slight arc right to reduce line follow at goal 3
RightWheelForward(50);
LeftWheelForward(55);
delay(500);
//get on green line and line follow
GetCenteronLine(50);
TurnRight(30);
LineFollow(40);
if(backup)
{
BackUp(50,300);
}
//turn to face goal
FindBeacon(90, LEFT);
}
else if(Side=='B')
{
FindBeacon(85,RIGHT);
//Run forward enough to get off of black line
//slight arc right to ensure robot hits green line at goal 3
RightWheelForward(50);
LeftWheelForward(55);
delay(500);
//get on green line and line follow
GetCenteronLine(50);
TurnLeft(30);
LineFollow(40);
if(backup)
{
BackUp(50,300);
}
//turn to face goal
FindBeacon(90,RIGHT);
}
//dump balls
Goal3();
return;
}
//~~~~~~~~~~~~~~~~~~~~~ G3ToDisp ~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Call at goal 3, drives robot to dispenser and requests 5 balls*/
void G3ToDisp(char Side)
{
if(Side=='A')
{
FindBeacon(30,RIGHT);
//Arc left to not hit dispenser
RightWheelForward(50);
LeftWheelForward(50);
delay(500);
//drive up to line
GetonLine_Right(50);
}
else if(Side=='B')
{
FindBeacon(30,LEFT);
//Arc right to not hit dispenser
RightWheelForward(50);
LeftWheelForward(50);
delay(500);
//drive up to line
GetonLine_Left(50);
}
//line follow up to dispenser and collect 5 balls
LineFollow(40);
CollectBall();
return;
}
//~~~~~~~~~~~~~~~~~~~~~~~ DispTo1 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Call at ball dispenser, drives robot to goal 1 and drops balls in goal*/
void DispTo1(char Side)
{
BackUp(50,300);
if(Side=='A')
{
FindBeacon(30, RIGHT);
//drive to goal 1
//arc left to hit line leading towards goal
RightWheelForward(70);
LeftWheelForward(50);
delay(800);
//line follow to goal 1
GetonLine_Right(50);
LineFollow(40);
}
else if(Side=='B')
{
FindBeacon(30, LEFT);
//drive to goal 1
//arc right to hit line leading towards goal
LeftWheelForward(70);
RightWheelForward(50);
delay(800);
//line follow to goal 1
GetonLine_Left(50);
LineFollow(40);
}
//dump balls in goal 1
Goal1();
return;
}
//~~~~~~~~~~~~~~~~~~~~~~ G1ToDisp ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Call at goal 1, drives robot to ball dispenser and requests 5 balls*/
void G1ToDisp(char Side)
{
if(Side=='A')
{
FindBeacon(50,LEFT);
//arc right to not hit dispenser
RightWheelForward(50);
LeftWheelForward(60);
delay(1000);
//drive to line leading up to dispenser and line follow
GetonLine_Left(50);
}
else if(Side=='B')
{
FindBeacon(50,LEFT);
//arc left to not hit dispenser
RightWheelForward(60);
LeftWheelForward(50);
delay(1000);
//drive to line leading up to dispenser and line follow
GetonLine_Right(50);
}
//Line follow to dispenser and collect 5 balls
LineFollow(40);
CollectBall();
return;
}