/**************************************************************************
Main.c
ME 218B Project Hats Off
Team Firefox
March 7, 2010
***************************************************************************/
/*-------------------------- Include Files
-----------------------------*/
#include <stdio.h>
#include <termio.h>
#include <hidef.h>
#include <mc9s12e128.h>
#include <bitdefs.h>
#include <S12E128bits.h>
#include
"S12eVec.h"
#include
"ADS12.h"
#include
"ME218_E128.h"
#include
"PWM01.h"
#include
"Motors.h"
#include
"myTimer.h"
#include
"InputsModule.h"
#include
"TapeSensors.h"
#include
"Position.h"
#include
"Arm.h"
#include
"SPI.h"
//included with main.c for E128
/* common defines and macros */
/* derivative information */
#pragma LINK_INFO DERIVATIVE "SampleS12"
#define printf (void)printf
typedef enum{ TOTARGET,
HITTING,
CONFIRMING_HIT,
GETTING_NEW_TARGET,
TOHOME,
WAVING,
DONE,
ERROR_OUT } MainState_t;
/*-------------------------- Module Functions
-----------------------------*/
void readIR(void);
void setLED(char turnOn);
/*-------------------------- Module Variables
-----------------------------*/
static char IR_Aligned = FALSE;
static POS_StartColor_t startColor;
static SPI_Status_t SPIstatus;
static MainState_t state, lastState;
static POS_State_t pos_state;
static char ArmStopped;
static int
i=0, pos = 0, cmd
= 0, value=0;;
static char ch, resetPosFlag, blinkState
= 0;
static int
curTarget, lastTarget;
static unsigned int
lastPrint = 0, lastRead
= 0, swingTime
= 0, lastBumpTime;
char waitingOnBump, waveCount;
/*--------------------------
const static int
targetDestroyedTimeout
= 5000; //in [ms]
const static char numWave = 2;
void main(){
(void)printf("Initializing Timer . . .\r\n");
MyTimer_Init(); //Initialize
1 [ms] timer for general use
wait(500); //allow circuits to power up before running init code
(void)printf("\nProgram Started\r\n");
//Set output
pins
DDRU =
0x03; //00000011
set U pins. 1=output
DDRP =
0x0F; //00001111
DDRT =
0x8E; //10001110 set T pins
DDRS =
0x0C; //00001100
(void)ADS12_Init("AAAAAAAA");
(void)printf("Initializing Arm . . .\r\n");
Arm_Init(); //Initialize
Arm module
(void)printf("Initializing Motors . . .\r\n");
Motors_Init(); //Initialize DC
Motor module
(void)printf("Initializing SPI . . .\r\n");
SPI_Init(); //will wait
until we get command from overlord
//determine
which team robot is on based on first assigned beacon
curTarget = SPI_getCurTarget();
if
(curTarget
== 0) {
startColor =
POS_RED; //red team!
PTT
|= BIT1HI; //turn on red LED
PTT
&= BIT3LO; //turn off green LED
}
else if (curTarget == 8) {
startColor =
POS_GREEN; //green team!
PTT
|= BIT3HI; //turn on
green LED
PTT
&= BIT1LO; //turn off red LED
}
else
{
//not assigned target 0 or 8 first, ERROR!!
//turn on both red and green LEDs
PTT
|= BIT1HI; //turn on red LED
PTT
|= BIT3HI; //turn on green LED
(void)printf("ERROR!! Not assigned target 0 or 8 to start\r\n");
while(1) {}; //do nothing/soft crash
}
InitPositioning(startColor); //Initialize positioning
SPI_requestTargetStatus();
//check if current beacon was already knocked off
(void)printf("Playing the Game . . .\r\n");
lastPrint = MyTimer_GetTime();
//Move to
Start Position (target 0 or 8)
(void)Pos_GoToStart();
goToAttackPosition();
//move
arm to attack position
state =
TOTARGET;
lastState = ERROR;
resetPosFlag = FALSE;
waitingOnBump = FALSE;
//game on
while
(1) {
SPIstatus =
SPI_getStatus();
//always
communicate with Overlord
ArmStopped =
isArmStopped();
//always
check arm status
pos_state = Pos_getStatus(SPIstatus); //always check positioning state
if (state != lastState) {
(void)printf("
switch(state) {
case TOTARGET: (void)printf("Moving to Target #%d\r\n", curTarget); break;
case HITTING: (void)printf("Hitting Target\r\n"); break;
case CONFIRMING_HIT: (void)printf("Confirming Target Destroyed\r\n"); break;
case GETTING_NEW_TARGET: (void)printf("Waiting for New Target\r\n"); break;
case TOHOME: (void)printf("Heading Home\r\n"); break;
case DONE: (void)printf("All done, game over\r\n"); break;
case ERROR_OUT: (void)printf("\r\nERRORED OUT\r\n"); break;
}
lastState =
state;
}
switch (state) {
case TOTARGET: //currently
moving to target
if ((pos_state
== POS_STOPPED) && (SPIstatus != SPI_DELAY)) { //in
position and not delayed
if (SPIstatus
== SPI_GAME_OVER) { //check if game over, then abort hit
(void)Pos_GoToHome();
state
= TOHOME;
}
else {
swingArm(curTarget);
state
= HITTING;
}
}
break;
case HITTING: //currently
striking the target
if
(ArmStopped) {
state
= CONFIRMING_HIT;
SPI_requestTargetStatus();
}
break;
case CONFIRMING_HIT: //arms has
completed swing, checking if target was destroyed
if
(SPIstatus
== SPI_GAME_OVER) { //check if game over
(void)Pos_GoToHome();
state
= TOHOME;
break;
}
if
(SPIstatus
== SPI_TARGET_STATUS_READY) { //if delayed, this should never be true
if (SPI_isCurTargetHit()) {
state
= GETTING_NEW_TARGET; //target destroyed
lastTarget =
curTarget;
resetPosFlag =
FALSE;
}
else { //target wasn't hit
if
(resetPosFlag) { //already
reset position, skip current target
resetPosFlag =
FALSE;
SPI_skipCurTarget();
state =
GETTING_NEW_TARGET;
lastTarget =
curTarget;
}
else
{ //need to reset our position using tape
if
(Pos_ResetCurrentPos(curTarget) == FALSE) (void)printf("ERROR: Failed to reset position\r\n");
state =
TOTARGET;
resetPosFlag =
TRUE;
}
}
}
break;
case GETTING_NEW_TARGET: //waiting
to be assigned a new target
if (SPIstatus
== SPI_GAME_OVER) { //if game over, go
home
(void)printf("sent bot home from
GETTING_NEW_TARGET with this set of procs:\r\n");
//send bot home
if
(Pos_GoToHome() == FALSE) (void)printf("ERROR: Failed to go home\r\n");
debug_printCurExecPlan();
//print
planned path out to screeen
state
= TOHOME;
break;
}
curTarget =
SPI_getCurTarget(); //get new target
from SPI module
if (curTarget
!= lastTarget) {
printf("Getting New
Target, newTarget = %d\r\n", curTarget);
lastTarget =
curTarget;
}
if (curTarget
!= -1) {
(void)Pos_GoToTarget(curTarget); //move to new target
state =
TOTARGET;
}
break;
case TOHOME: //moving home
if (pos_state
== POS_STOPPED) { //in position
//start doing the wave
waveArm();
waveCount =
0;
state =
WAVING;
}
break;
case
WAVING: //at home,
currently doing victory wave
if (isWaveStopped()) {
waveCount++;
if (waveCount < numWave) {
waveArm(); //wave
arm again
}
else {
goToAttackPosition();
//return
arm to centered position
state =
DONE;
}
}
break;
case DONE: //all
done, just sit here
if
(isArmStopped()) {
goToHomePosition(); //make arm go back to rest position on top of robot
}
//do nothing
break;
case ERROR_OUT: //something
went wrong
default:
//do nothing for now
break;
}
}
}