MasterSM.c
/****************************************************************************
Module
MasterSM.c
Revision
2.0.1
Description
This is a template for the top level Hierarchical state machine
Notes
History
When Who What/Why
-------------- --- --------
02/08/12 01:39 jec converted from MW_MasterMachine.c
02/06/12 22:02 jec converted to Gen 2 Events and Services Framework
02/13/10 11:54 jec converted During functions to return Event_t
so that they match the template
02/21/07 17:04 jec converted to pass Event_t to Start...()
02/20/07 21:37 jec converted to use enumerated type for events
02/21/05 15:03 jec Began Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "MasterSM.h"
#include "ES_ServiceHeaders.h"
/*----------------------------- Module Defines ----------------------------*/
#define _MS_ *375
#define PERIOD10 (10_MS_);
#define PERIOD30 (30_MS_);
/*---------------------------- Module Functions ---------------------------*/
static void processKeyboard(uint16_t);
static ES_Event DuringMaster( ES_Event Event);
static void SPIInit(void );
static void PWMInit(void );
static void TimerInit(void );
static void ServoInitCh1(void );
static void RespondToOC1(void );
static void updateActive(void );
static ES_Event DuringShootingTargets( ES_Event Event);
static ES_Event DuringMovingToFire( ES_Event Event);
static ES_Event DuringMovingToTape( ES_Event Event);
static ES_Event DuringPreparingToLoad( ES_Event Event);
static ES_Event DuringLoading( ES_Event Event);
static ES_Event DuringEndingGame( ES_Event Event);
static ES_Event DuringMoveOutOfCamo( ES_Event Event);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, though if the top level state machine
// is just a single state container for orthogonal regions, you could get
// away without it
static MasterState_t CurrentState;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;
static boolean activeShips[40];
static int enemyShip;
static int enemyPowerY;
static int align = 0;
static int color = 0; //1 = red, 2 = blue
static int ammo;
static int emitterActive; //1 is on, 0 is off
static int twomin = -5000;
static int shipJustShot = 0;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitMasterSM
Parameters
uint8_t : the priorty of this service
Returns
boolean, False if error in initialization, True otherwise
Description
Saves away the priority, and starts
the top level state machine
Notes
Author
J. Edward Carryer, 02/06/12, 22:06
****************************************************************************/
boolean InitMasterSM ( uint8_t Priority )
{
ES_Event ThisEvent;
MyPriority = Priority; // save our priority
ThisEvent.EventType = ES_ENTRY;
// Start the Master State machine
SPIInit();
// Init SPI
TimerInit();
PWMInit();
ServoInitCh1();
//Port outputs/inputs
DDRS |= BIT7HI;
DDRT |= BIT1HI;
DDRU |= (BIT6HI | BIT7HI);
DDRP |= (BIT1HI);
//Analog Init
ADS12_Init("OAAAAAAI");
enemyShip = 0;
ammo = 5;
//Initial state actions
Servo_SetAngle(FiducialServo, 30);
resetLoad();
Servo_SetAngle(AimServo, 90);
Servo_SetAngle(BallSpinServo, 128);
setEmitter(0);
Stop();
SetDuty(0,1);
LEDoff();
StartMasterSM( ThisEvent );
return True;
}
/****************************************************************************
Function
PostMasterSM
Parameters
ES_Event ThisEvent , the event to post to the queue
Returns
boolean False if the post operation failed, True otherwise
Description
Posts an event to this state machine's queue
Notes
Author
J. Edward Carryer, 10/23/11, 19:25
****************************************************************************/
boolean PostMasterSM( ES_Event ThisEvent )
{
return ES_PostToService( MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunMasterSM
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
the run function for the top level state machine
Notes
uses nested switch/case to implement the machine.
Author
J. Edward Carryer, 02/06/12, 22:09
****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855
ES_Event RunMasterSM( ES_Event CurrentEvent )
{
ES_Event currEvent;
unsigned char MakeTransition = False;/* are we making a state transition? */
MasterState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };
ES_Event ReturnEvent = CurrentEvent;
static int i = 0;
CurrentEvent = DuringMaster(CurrentEvent);
//printf("%d\r\n", CurrentEvent.EventParam);
switch(CurrentState)
{
//Shoots until out of balls
case ShootingTargets :
CurrentEvent = DuringShootingTargets(CurrentEvent);
switch (CurrentEvent.EventType)
{
//Prepare for reloading if we run out of balls
case OutOfBalls :
NextState = MovingToTape;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
//Update and decide what to do after a ball is fired (Shooting SM finished)
case BallFired :
updateFAC();
if (ammo == -1) //Do not ever shoot power station in real game
{
printf("ammo is now %d\r\n", ammo);
if (color == 1)
{
RotateToExact(270);
} else
{
RotateToExact(90);
}
currEvent.EventType = ES_ENTRY;
StartShootPowerSM(currEvent);
} else if (ammo != 0)
{
currEvent.EventType = ES_ENTRY;
StartShootTargetSM (currEvent);
}
break;
}
break;
//Movement to firing spot
case MovingToFire :
CurrentEvent = DuringMovingToFire(CurrentEvent);
switch (CurrentEvent.EventType)
{
case MovementFinished :
NextState = ShootingTargets;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
//Gets out of camoflague near resupply
case MoveOutOfCamo :
CurrentEvent = DuringMoveOutOfCamo(CurrentEvent);
switch (CurrentEvent.EventType)
{
case botfound :
NextState = MovingToFire;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
//Moves to the tape in front of the resupply but outside of the camoflague
case MovingToTape :
CurrentEvent = DuringMovingToTape(CurrentEvent);
switch (CurrentEvent.EventType)
{
case MovementFinished :
NextState = PreparingToLoad;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
//Start preparing to load balls
case PreparingToLoad :
CurrentEvent = DuringPreparingToLoad(CurrentEvent);
switch (CurrentEvent.EventType)
{
//After rear bumper is hit, reverse for a little more to align
case BumpSwitchHit :
ES_Timer_SetTimer(4, 300);
ES_Timer_StartTimer(4);
break;
//After reversing a little more, stop and start loading
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 4)
{
NextState = Loading;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
break;
}
break;
//Starts loading balls
case Loading :
CurrentEvent = DuringLoading(CurrentEvent);
switch (CurrentEvent.EventType)
{
case LoadingFinished:
NextState = MoveOutOfCamo;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
//Never actually used
case ShootingPower :
case OutOfBalls :
NextState = MovingToTape;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
case BallFired :
updateFAC();
if (ammo != 0)
{
currEvent.EventParam = ES_ENTRY;
StartShootTargetSM (currEvent);
}
break;
break;
//Never actually used
case EndingGame :
CurrentEvent = DuringEndingGame(CurrentEvent);
switch (CurrentEvent.EventType)
{
}
break;
//Next two states are for initializing, need to see which side of the field we are on and waits for start from FAC
case WaitingForFAC :
if (CurrentEvent.EventType == UpdateFinish && color != 0) //After we get a color, wait for start command
{
NextState = WaitForStart;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
ES_Timer_SetTimer(7, 100);
ES_Timer_StartTimer(7);
} else if (CurrentEvent.EventType == UpdateFinish) //If we don't, update the FAC again
{
updateFAC();
}
break;
//Waits for start command from FAC
case WaitForStart :
if (CurrentEvent.EventType == GameStart)
{
//Start the timer, rev up the shooter motor, turn on LED indicator.
twomin = 0;
SetDuty(20,1);
LEDon();
NextState = ShootingTargets;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
case Finished :
break;
}
if ( CurrentEvent.EventType == EV_CLEAR)
{
// First pass exit messages to the lower level machines
CurrentEvent.EventType = ES_EXIT;
RunMasterSM(CurrentEvent);
// Now pass entry messages, since this is a self transition
CurrentEvent.EventType = ES_ENTRY;
RunMasterSM(CurrentEvent);
}
if (MakeTransition == True)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunMasterSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunMasterSM(EntryEventKind);
printf("Changed States\r\n");
}
// in the absence of an error the top level state machine should
// always return ES_NO_EVENT
CurrentEvent.EventType = ES_NO_EVENT;
return(CurrentEvent);
}
/****************************************************************************
Function
StartMasterSM
Parameters
ES_Event CurrentEvent
Returns
nothing
Description
Does any required initialization for this state machine
Notes
Author
J. Edward Carryer, 02/06/12, 22:15
****************************************************************************/
void StartMasterSM ( ES_Event CurrentEvent )
{
// local variable to get debugger to display the value of CurrentEvent
volatile ES_Event LocalEvent = CurrentEvent;
// if there is more than 1 state to the top level machine you will need
// to initialize the state variable
//ES_Timer_SetTimer(1, 2000);
//ES_Timer_StartTimer(1);
CurrentState = WaitingForFAC;
//CurrentState = ShootingTargets;
// now we need to let the Run function init the lower level state machines
// use LocalEvent to keep the compiler from complaining about unused var
RunMasterSM(LocalEvent);
return;
}
MasterState_t QueryMasterSM(void )
{
return CurrentState;
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringMaster( ES_Event Event )
{
coord botcoord;
ES_Event currEvent;
// process ES_ENTRY & ES_EXIT events
if ( Event.EventType == ES_ENTRY)
{
//Start with cloaking servo tucked into bot
updateFAC();
// CloakBot();
} else if ( Event.EventType == GameEnd)
{
//Does all game ending activities here, and passes down to lower level, too
CurrentState = Finished;
Stop();
SetDuty(0, 1);
LEDoff();
setEmitter(0);
RunShootTargetSM(Event);
RunMovementSM(Event);
RunBallSM(Event);
RunShootPowerSM(Event);
RunCommSM(Event);
Event.EventType = ES_NO_EVENT;
} else if (Event.EventType == ES_TIMEOUT && Event.EventParam == 4)
{
//Forward movement timer, stop after timeout
Stop();
RunMovementSM(Event);
} else if (Event.EventType == ES_TIMEOUT && Event.EventParam == 1)
{
//Same as timer 4, but for turns
Stop();
RunMovementSM(Event);
RunShootTargetSM(Event);
RunShootPowerSM(Event);
} else if (Event.EventType == ES_TIMEOUT && Event.EventParam == 7)
{
//Game status updater
currEvent.EventType = ES_ENTRY;
StartCommSM(currEvent, 0x3F);
if (twomin++ >= 240)
{
//two minute timer
currEvent.EventType = GameEnd;
PostMasterSM(currEvent);
}
if (twomin < -1) // For before the game, keeps updating the FAC
{
ES_Timer_SetTimer(7, 100);
ES_Timer_StartTimer(7);
} else //For after the game, 500 ms for each update
{
ES_Timer_SetTimer(7, 500);
ES_Timer_StartTimer(7);
}
} else if ( Event.EventType == UpdateFinish) //Every time the FAC is updated, run this and also pass to lower levels to tell them it finished
{
updateActive();
if (color == 0)
{
botcoord = queryShip(15);
if((botcoord.x > 127) && (botcoord.x != 255)) color = 2;
else if((botcoord.x <= 127) && (botcoord.x != 0)) color = 1;
// printf("color %d\r\n", color);
}
RunMovementSM(Event);
RunShootTargetSM(Event);
RunShootPowerSM(Event);
} else if ( Event.EventType == ES_NEW_KEY) //Keyboard debugging
{
processKeyboard(Event.EventParam);
} else if ( Event.EventType == OC1Exp) //Used for IR emitters
{
RespondToOC1();
}
else //Pass down to state machines below if it doesn't fit the other categories
{
RunShootTargetSM(Event);
RunMovementSM(Event);
RunBallSM(Event);
RunShootPowerSM(Event);
RunCommSM(Event);
// printf("%d\r\n", Event.EventParam);
}
return(Event);
}
static ES_Event DuringShootingTargets( ES_Event Event)
{
ES_Event ReturnEvent = Event;
ES_Event currEvent;
if (Event.EventType == ES_ENTRY) //Starts shoot target SM
{
currEvent.EventType = ES_ENTRY;
StartShootTargetSM(currEvent);
printf("Starting ShootTargetSM\r\n");
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringMovingToFire( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY) //Starts movement to firing position
{
printf("Going to MovingToFire\r\n");
SetDuty(15,1); //Revs up motor
if(color == 1)
{
StartMovementSM(Event, 80, 127, 270, 4, 0);
} else if (color == 2)
{
StartMovementSM(Event, 176, 127, 90, 4, 0);
}
}
else if ( Event.EventType == ES_EXIT )
{
resetCurrentSector();
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringMovingToTape( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY) //Starts movement to pre-loading position
{
SetDuty(0,1);
if(color == 1)
{
StartMovementSM(Event, 51, 127, 270, 2, 1);
} else if (color == 2)
{
StartMovementSM(Event, 205, 127, 90, 2, 1);
}
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
//Moves forward until FAC detects bot, used to get out of supply depot camoflague
static ES_Event DuringMoveOutOfCamo( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY)
{
//Requests event checker to start looking for event of FAC detection
requestFind();
DriveForward();
updateFAC();
ES_Timer_SetTimer(4, 500);
ES_Timer_StartTimer(4);
}
else if ( Event.EventType == ES_EXIT )
{
Stop();
} else if (Event.EventType == ES_TIMEOUT && Event.EventParam == 4)
{
updateFAC();
DriveForward();
ES_Timer_SetTimer(4, 500);
ES_Timer_StartTimer(4);
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringPreparingToLoad( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY)
{
//Requests event checkers for rear bump detection
requestBump();
DriveReverse();
}
else if ( Event.EventType == ES_EXIT )
{
//Stops on exit
Stop();
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringLoading( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY)
{
StartBallSM(Event);
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringEndingGame( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY)
{
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static void SPIInit(void )
{
//SPI init, to receive messages, manual set for line
SPIBR = (_S12_SPPR1 | _S12_SPR2 | _S12_SPR1 | _S12_SPR0);
SPICR1 |= _S12_MSTR;
SPICR1 |= (_S12_CPOL | _S12_CPHA);
SPICR1 &= ~_S12_SSOE;
//SPICR1 |= (_S12_SPIE);
SPICR2 &= ~_S12_MODFEN;
SPICR2 &= ~(_S12_SPC0);
DDRS |= _S12_SS;
SPICR1 |= _S12_SPE;
}
static void PWMInit(void )
{
/*******Set Clock A Pre-scaler/Scale******/
/******* Channels 0, 1, 4, 5 ******/
PWMPRCLK |= (_S12_PCKA2|_S12_PCKA1); //set pre-scaler to % 64
PWMSCLA = 0x02; //set scaler to %8
/*******Set Clock B Pre-scaler/Scale******/
/******* Channels 2, 3 ******/
PWMPRCLK |= (_S12_PCKB2|_S12_PCKB1); //set pre-scaler to % 64
PWMSCLB = 0x02; //set scaler to %8
/************PWM channel 0************/
PWME |= _S12_PWME0; //enable PWM channel 0
//MODRR |= _S12_MODRR0; //leave control for TIMER
PWMCLK |= _S12_PCLK0; //set use prescaled+scaled clock A
PWMPOL |= _S12_PPOL0; //Set PWM polarity to 1 (starts high)
PWMPER0 = 250; //set to drive period
PWMDTY0 = 0; //initialize w/ 0% Duty Cycle
/************PWM channel 1************/
PWME |= _S12_PWME1; //enable PWM channel 1
MODRR |= _S12_MODRR1; //set control to PWM
PWMCLK |= _S12_PCLK1; //set use prescaled+scaled clock A
PWMPOL |= _S12_PPOL1; //Set PWM polarity to 1 (starts high)
PWMPER1 = 250; //set to drive period
PWMDTY1 = 0; //initialize w/ 0% Duty Cycle
/************PWM channel 2************/
PWME |= _S12_PWME2; //enable PWM channel 2
MODRR |= _S12_MODRR2; //set control to PWM (controls RIGHT MOTOR)
PWMCLK |= _S12_PCLK2; //set use prescaled+scaled clock B
PWMPOL |= _S12_PPOL2; //Set PWM polarity to 1 (starts high)
PWMPER2 = 250; //set to drive period
PWMDTY2 = 0; //initialize w/ 0% Duty Cycle
/************PWM channel 3************/
PWME |= _S12_PWME3; //enable PWM channel 3
MODRR |= _S12_MODRR3; //set control to PWM (controls LEFT MOTOR)
PWMCLK |= _S12_PCLK3; //set use prescaled+scaled clock B
PWMPOL |= _S12_PPOL3; //Set PWM polarity to 1 (starts high)
PWMPER3 = 250; //set to drive period
PWMDTY3 = 0; //initialize w/ 0% Duty Cycle
/************PWM channel 4************/
PWME |= _S12_PWME4; //enable PWM channel 4
MODRR |= _S12_MODRR4; //set control to PWM
PWMCLK |= _S12_PCLK4; //set use prescaled+scaled clock A
PWMPOL |= _S12_PPOL4; //Set PWM polarity to 1 (starts high)
PWMPER4 = 250; //set to drive period
PWMDTY4 = 0; //initialize w/ 0% Duty Cycle
/************PWM channel 5************/
PWME |= _S12_PWME5; //enable PWM channel 5
MODRR |= _S12_MODRR5; //set control to PWM
PWMCLK |= _S12_PCLK5; //set use prescaled+scaled clock A
PWMPOL |= _S12_PPOL5; //Set PWM polarity to 1 (starts high)
PWMPER5 = 250; //set to drive period
PWMDTY5 = 0; //initialize w/ 0% Duty Cycle
}
static void TimerInit(void )
{
//Timer init
TIM0_TSCR1 = _S12_TEN;
TIM0_TSCR2 = (_S12_PR1 | _S12_PR2); //64
//Timer 4 channel 0 used for IR detection
TIM0_TCTL3 |= (_S12_EDG4A);
TIM0_TIE &= ~_S12_C4I;
TIM0_TFLG1 = _S12_C4F;
//Timer 5 channel 1 used for IR emitter
TIM0_TIOS |= (_S12_IOS5);
TIM0_TCTL1 &= ~(_S12_OL5 | _S12_OM5);
//10 ms period to start
TIM0_TC5 = TIM0_TCNT + PERIOD10;
TIM0_TFLG1 = _S12_C5F;
EnableInterrupts;
}
static void ServoInitCh1(void )
{
//Set TIM1 to Servo control
TIM1_TSCR1 |= _S12_TEN;
// enable timer system
TIM1_TSCR2 = _S12_PR1 | _S12_PR0; // 011 = 24MHz / 8 = 3MHz
//Select channels on TIM1 for output compare
TIM1_TIOS |= (_S12_IOS4 | _S12_IOS5 | _S12_IOS6 | _S12_IOS7);
//Program a rise on compare
TIM1_TCTL1 |= (_S12_OL4 | _S12_OM4);
TIM1_TCTL1 |= (_S12_OL5 | _S12_OM5);
TIM1_TCTL1 |= (_S12_OL6 | _S12_OM6);
TIM1_TCTL1 |= (_S12_OL7 | _S12_OM7);
//Set bit to toggle overflow
TIM1_TTOV |= (_S12_TOV4 | _S12_TOV5 | _S12_TOV6 | _S12_TOV7);
//Output pulse width for inital position
TIM1_TC4 = 0xFFFF - 4500;
TIM1_TC5 = 0xFFFF - 4500;
TIM1_TC6 = 0xFFFF - 4500;
TIM1_TC7 = 0xFFFF - 4500;
//For any pins to remove from timer
}
//Updates active ships
static void updateActive(void )
{
int i;
//Searches for an enemy ship until an active one is found, and sets it for the rest of the runtime
if (enemyShip == 0)
{
for (i = 3; i < 20; i++)
{
if ((queryActive(i)) && (i != 15))
{
enemyShip = i;
printf("enemy bot = %d\r\n", enemyShip);
}
}
}
//Updates active ships
for (i = 20; i < 40; i++)
{
if (queryActive(i))
{
activeShips[i] = TRUE;
} else
{
activeShips[i] = FALSE;
}
}
}
//Used for IR detection, can disable or enable when necessary
void interrupt _Vec_tim0ch4 GetPeriod(void)
{
//Keeps track of last edge time and second to last edge
static long lastEdge, secondLastEdge = 0;
ES_Event currEvent;
long period, period2, perioduS, perioduS2;
int Time;
//Clear flag, gets current timer time
TIM0_TFLG1 = _S12_C4F;
Time = TIM0_TC4;
//Calculates all periods, then converts into uS
period = Time - lastEdge;
period2 = Time - secondLastEdge;
perioduS = (period * 267)/100;
perioduS2 = (period2 * 267)/100;
//Waits for 2 legitimate periods in a row
//Had them seperate, but wasn't necessary due to mechanical design
if ((perioduS <= 14200 && perioduS >= 13800) || (perioduS2 <= 14200 && perioduS2 >= 13800) || (perioduS <= 20200 && perioduS >= 19800) || (perioduS2 <= 20200 && perioduS2 >= 19800))
{
// if (color == 1)
// {
if (align == 1)
{
//Post aligned event and turn off interrupt
TIM0_TIE &= ~_S12_C4I;
currEvent.EventType = Aligned;
PostMasterSM(currEvent);
align = 0;
} else if (align != 0 && align != 1)
{
align -= 1;
}
// }
} else
{
align = 2;
}/*else if ()
{
// if (color == 2)
// {
TIM0_TIE &= ~_S12_C4I;
currEvent.EventType = Aligned;
PostMasterSM(currEvent);
align = 0;
// }
} */
secondLastEdge = lastEdge;
lastEdge = Time;
}
/*******************************
Public Functions
*******************************/
void requestAlign(void )
{
align = 2;
TIM0_TIE |= _S12_C4I;
}
int getColor(void )
{
return color;
}
//Calculates next target to hit
int getNextTarget(void )
{
int i;
int NumberOurShips = 0;
int NumberEnemyShips = 0;
if (color == 2)
{
NumberOurShips = queryBlueShips();
NumberEnemyShips = queryRedShips();
} else if (color == 1)
{
NumberOurShips = queryRedShips();
NumberEnemyShips = queryBlueShips();
}
if(NumberOurShips >= NumberEnemyShips && ammo == 1 && enemyShip == 6) return enemyShip;
if((NumberEnemyShips+3 <= NumberOurShips) || NumberEnemyShips == 0) return enemyShip; // enemy Bot
if (color == 1)
{
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 1 && i != shipJustShot)
{
return i;
}
}
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 4 && i != shipJustShot)
{
return i;
}
}
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 2 && i != shipJustShot)
{
return i;
}
}
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 5 && i != shipJustShot)
{
return i;
}
}
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 3 && i != shipJustShot)
{
return i;
}
}
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 6 && i != shipJustShot)
{
return i;
}
}
} else if (color == 2)
{
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 1 && i != shipJustShot)
{
return i;
}
}
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 4 && i != shipJustShot)
{
return i;
}
}
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 2 && i != shipJustShot)
{
return i;
}
}
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 5 && i != shipJustShot)
{
return i;
}
}
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 3 && i != shipJustShot)
{
return i;
}
}
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 6 && i != shipJustShot)
{
return i;
}
}
}
return enemyShip; // enemy Bot
//return enemyShip; // if no ships left, return enemy Bot
}
//Keeps track of last ship shot, so we don't have to wait for it to go down to calculate the next target
void setLastShip(int num)
{
shipJustShot = num;
if (color == 1)
{
if (queryBlueShips() <= 1)
{
shipJustShot = 0;
}
} else
{
if (queryRedShips() <= 1)
{
shipJustShot = 0;
}
}
}
void processKeyboard(uint16_t input)
{
ES_Event currEvent;
char key = (char)input;\
static int CurrentDuty = 50;
static int BatVoltage;
static int testangle = 360;
double Current_BatVoltage;
double ShooterMotorPWM;
static double ShootingDistance = 36*32/12;
//.
int i;
static int angle = 65;
static int angle2 = 150;
coord coordi;
currEvent.EventType = ES_ENTRY;
if (key == 'a')
{
updateFAC();
printf("Update FAC\r\n");
}
if (key == '2')
{
for (i = 3; i < 40; i++)
{
coordi = queryShip(i);
printf("%d, x = %d, y = %d, r = %d, sector = %d\r\n", i, coordi.x, coordi.y, coordi.r, coordi.sector);
}
}
if (key == 'm')
{
setEmitter(1);
printf("ActivateEmitter\r\n");
}
if (key == 'n')
{
setEmitter(0);
printf("DeactivateEmitter\r\n");
}
if (key == 'q')
{
angle2 += 5;
Servo_SetAngle(BallSpinServo, angle2);
printf("angle2 %d\r\n", angle2);
}
if (key == 'w')
{
angle2 -= 5;
Servo_SetAngle(BallSpinServo, angle2);
printf("angle2 %d\r\n", angle2);
}
if (key == 'e')
{
angle2 = 170;
Servo_SetAngle(BallSpinServo, angle2);
printf("angle2 %d\r\n", angle2);
}
if (key == '3')
{
ShootingDistance -=6*32/12;
Current_BatVoltage = (double)ADS12_ReadADPin(4);
ShooterMotorPWM = (CAL_BAT_VOLTAGE/Current_BatVoltage)*(ShootingDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
printf("PWM = %d, ShootingDist = %d\r\n", (int)ShooterMotorPWM, (int)(12*ShootingDistance/32));
SetDuty(ShooterMotorPWM,1);
printf("shooting dist %4.4f \r\n", (ShootingDistance*12/32));
}
if (key == '4')
{
ShootingDistance +=6*32/12;
Current_BatVoltage = (double)ADS12_ReadADPin(4);
ShooterMotorPWM = (CAL_BAT_VOLTAGE/Current_BatVoltage)*(ShootingDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
printf("PWM = %d, ShootingDist = %d\r\n", (int)ShooterMotorPWM, (int)(ShootingDistance*12/32));
SetDuty(ShooterMotorPWM,1);
}
if (key == '5')
{
ShootingDistance =36*32/12;
Current_BatVoltage = (double)ADS12_ReadADPin(4);
ShooterMotorPWM = (CAL_BAT_VOLTAGE/Current_BatVoltage)*(ShootingDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
printf("PWM = %d, ShootingDist = %d\r\n", (int)ShooterMotorPWM, (int)(ShootingDistance*12/32));
SetDuty(ShooterMotorPWM,1);
}
if (key == '6')
{
CurrentDuty =18;
Current_BatVoltage = (double)ADS12_ReadADPin(4);
SetDuty(CurrentDuty,1);
printf("currentduty %d \r\n", CurrentDuty);
}
if (key == 'y')
{
Stop();
}
if (key == 'u')
{
DriveForward();
}
if (key == 'i')
{
DriveReverse();
}
if (key == '7')
{
RotateCW(360, 1);
}
if (key == 'b')
{
shootBall();
printf("LoadServo 112\r\n");
}
if (key == 'v')
{
Servo_SetAngle(LoadServo, 80);
}
if (key == 'd')
{
resetLoad();
printf("LoadServo 55\r\n");
}
if (key == 'k')
{
RotateCW(360, 1);
}
if (key == 'l')
{
RotateCCW(360, 1);
}
if (key == '8')
{
loadShooter();
printf("LoadShooter\r\n");
}
if (key == '9')
{
currEvent.EventType = ES_ENTRY;
StartBallSM(currEvent);
printf("Start Loading\r\n");
}
if(key == 's')
{
printf("Event to ShootPower\r\n");
currEvent.EventType = ES_ENTRY;
StartShootPowerSM(currEvent);
}
if(key == 'p')
{
printf("Aligned\r\n");
currEvent.EventType = Aligned;
PostMasterSM(currEvent);
align = 0;
}
if(key == 'c')
{
BatVoltage = ADS12_ReadADPin(4);
printf("bat volt %d \r\n", BatVoltage);
}
if(key == 'g')
{
printf("Green LED on\r\n");
LEDon();
}
if(key == 'h')
{
printf("Red LED on\r\n");
LEDoff();
}
}
/************************************
CLOAK STUFF
************************************/
void CloakBot(void)
{
Servo_SetAngle(FiducialServo,0);
ES_Timer_SetTimer(3,1000);
ES_Timer_StartTimer(3);
}
void UncloakBot(void)
{
Servo_SetAngle(FiducialServo, 30);
}
/************************************
AMMO TRACKING STUFF
************************************/
int getAmmo(void )
{
return ammo;
}
void changeAmmo(int change)
{
ES_Event currEvent;
ammo += change;
printf("Ammo = %d\r\n", ammo);
if (ammo == 0)
{
printf("Out Of Balls\r\n");
currEvent.EventType = OutOfBalls;
PostMasterSM(currEvent);
}
}
/************************************
EMITTER STUFF
************************************/
//Pass 1 in for on, 0 in for off
void setEmitter(int onoff )
{
if(onoff == 1)
{
emitterActive = 1;
TIM0_TFLG1 = _S12_C5F;
TIM0_TC5 += PERIOD30;
}
else if (onoff == 0)
{
emitterActive = 0;
PTT &= BIT1LO;
}
}
static void RespondToOC1(void)
{
TIM0_TFLG1 = _S12_C5F;
if (emitterActive == 1)
{
if (PTT & BIT1HI) //If PTT is high set it low and add 30ms to period for low time
{
PTT &= BIT1LO;
TIM0_TC5 += PERIOD30;
}
else //If PTT is low set it high and add 10ms to period for high time
{
PTT |= BIT1HI;
TIM0_TC5 += PERIOD10;
}
}
}
/************************************
IN GAME LED
************************************/
void LEDon(void)
{
PTP |= BIT1HI;
}
void LEDoff(void)
{
PTP &= ~(BIT1HI);
}
Module
MasterSM.c
Revision
2.0.1
Description
This is a template for the top level Hierarchical state machine
Notes
History
When Who What/Why
-------------- --- --------
02/08/12 01:39 jec converted from MW_MasterMachine.c
02/06/12 22:02 jec converted to Gen 2 Events and Services Framework
02/13/10 11:54 jec converted During functions to return Event_t
so that they match the template
02/21/07 17:04 jec converted to pass Event_t to Start...()
02/20/07 21:37 jec converted to use enumerated type for events
02/21/05 15:03 jec Began Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "MasterSM.h"
#include "ES_ServiceHeaders.h"
/*----------------------------- Module Defines ----------------------------*/
#define _MS_ *375
#define PERIOD10 (10_MS_);
#define PERIOD30 (30_MS_);
/*---------------------------- Module Functions ---------------------------*/
static void processKeyboard(uint16_t);
static ES_Event DuringMaster( ES_Event Event);
static void SPIInit(void );
static void PWMInit(void );
static void TimerInit(void );
static void ServoInitCh1(void );
static void RespondToOC1(void );
static void updateActive(void );
static ES_Event DuringShootingTargets( ES_Event Event);
static ES_Event DuringMovingToFire( ES_Event Event);
static ES_Event DuringMovingToTape( ES_Event Event);
static ES_Event DuringPreparingToLoad( ES_Event Event);
static ES_Event DuringLoading( ES_Event Event);
static ES_Event DuringEndingGame( ES_Event Event);
static ES_Event DuringMoveOutOfCamo( ES_Event Event);
/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, though if the top level state machine
// is just a single state container for orthogonal regions, you could get
// away without it
static MasterState_t CurrentState;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;
static boolean activeShips[40];
static int enemyShip;
static int enemyPowerY;
static int align = 0;
static int color = 0; //1 = red, 2 = blue
static int ammo;
static int emitterActive; //1 is on, 0 is off
static int twomin = -5000;
static int shipJustShot = 0;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
InitMasterSM
Parameters
uint8_t : the priorty of this service
Returns
boolean, False if error in initialization, True otherwise
Description
Saves away the priority, and starts
the top level state machine
Notes
Author
J. Edward Carryer, 02/06/12, 22:06
****************************************************************************/
boolean InitMasterSM ( uint8_t Priority )
{
ES_Event ThisEvent;
MyPriority = Priority; // save our priority
ThisEvent.EventType = ES_ENTRY;
// Start the Master State machine
SPIInit();
// Init SPI
TimerInit();
PWMInit();
ServoInitCh1();
//Port outputs/inputs
DDRS |= BIT7HI;
DDRT |= BIT1HI;
DDRU |= (BIT6HI | BIT7HI);
DDRP |= (BIT1HI);
//Analog Init
ADS12_Init("OAAAAAAI");
enemyShip = 0;
ammo = 5;
//Initial state actions
Servo_SetAngle(FiducialServo, 30);
resetLoad();
Servo_SetAngle(AimServo, 90);
Servo_SetAngle(BallSpinServo, 128);
setEmitter(0);
Stop();
SetDuty(0,1);
LEDoff();
StartMasterSM( ThisEvent );
return True;
}
/****************************************************************************
Function
PostMasterSM
Parameters
ES_Event ThisEvent , the event to post to the queue
Returns
boolean False if the post operation failed, True otherwise
Description
Posts an event to this state machine's queue
Notes
Author
J. Edward Carryer, 10/23/11, 19:25
****************************************************************************/
boolean PostMasterSM( ES_Event ThisEvent )
{
return ES_PostToService( MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunMasterSM
Parameters
ES_Event: the event to process
Returns
ES_Event: an event to return
Description
the run function for the top level state machine
Notes
uses nested switch/case to implement the machine.
Author
J. Edward Carryer, 02/06/12, 22:09
****************************************************************************/
// make recursive call warning into info
#pragma MESSAGE INFORMATION C1855
ES_Event RunMasterSM( ES_Event CurrentEvent )
{
ES_Event currEvent;
unsigned char MakeTransition = False;/* are we making a state transition? */
MasterState_t NextState = CurrentState;
ES_Event EntryEventKind = { ES_ENTRY, 0 };
ES_Event ReturnEvent = CurrentEvent;
static int i = 0;
CurrentEvent = DuringMaster(CurrentEvent);
//printf("%d\r\n", CurrentEvent.EventParam);
switch(CurrentState)
{
//Shoots until out of balls
case ShootingTargets :
CurrentEvent = DuringShootingTargets(CurrentEvent);
switch (CurrentEvent.EventType)
{
//Prepare for reloading if we run out of balls
case OutOfBalls :
NextState = MovingToTape;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
//Update and decide what to do after a ball is fired (Shooting SM finished)
case BallFired :
updateFAC();
if (ammo == -1) //Do not ever shoot power station in real game
{
printf("ammo is now %d\r\n", ammo);
if (color == 1)
{
RotateToExact(270);
} else
{
RotateToExact(90);
}
currEvent.EventType = ES_ENTRY;
StartShootPowerSM(currEvent);
} else if (ammo != 0)
{
currEvent.EventType = ES_ENTRY;
StartShootTargetSM (currEvent);
}
break;
}
break;
//Movement to firing spot
case MovingToFire :
CurrentEvent = DuringMovingToFire(CurrentEvent);
switch (CurrentEvent.EventType)
{
case MovementFinished :
NextState = ShootingTargets;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
//Gets out of camoflague near resupply
case MoveOutOfCamo :
CurrentEvent = DuringMoveOutOfCamo(CurrentEvent);
switch (CurrentEvent.EventType)
{
case botfound :
NextState = MovingToFire;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
//Moves to the tape in front of the resupply but outside of the camoflague
case MovingToTape :
CurrentEvent = DuringMovingToTape(CurrentEvent);
switch (CurrentEvent.EventType)
{
case MovementFinished :
NextState = PreparingToLoad;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
//Start preparing to load balls
case PreparingToLoad :
CurrentEvent = DuringPreparingToLoad(CurrentEvent);
switch (CurrentEvent.EventType)
{
//After rear bumper is hit, reverse for a little more to align
case BumpSwitchHit :
ES_Timer_SetTimer(4, 300);
ES_Timer_StartTimer(4);
break;
//After reversing a little more, stop and start loading
case ES_TIMEOUT :
if (CurrentEvent.EventParam == 4)
{
NextState = Loading;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
break;
}
break;
//Starts loading balls
case Loading :
CurrentEvent = DuringLoading(CurrentEvent);
switch (CurrentEvent.EventType)
{
case LoadingFinished:
NextState = MoveOutOfCamo;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
}
break;
//Never actually used
case ShootingPower :
case OutOfBalls :
NextState = MovingToTape;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
break;
case BallFired :
updateFAC();
if (ammo != 0)
{
currEvent.EventParam = ES_ENTRY;
StartShootTargetSM (currEvent);
}
break;
break;
//Never actually used
case EndingGame :
CurrentEvent = DuringEndingGame(CurrentEvent);
switch (CurrentEvent.EventType)
{
}
break;
//Next two states are for initializing, need to see which side of the field we are on and waits for start from FAC
case WaitingForFAC :
if (CurrentEvent.EventType == UpdateFinish && color != 0) //After we get a color, wait for start command
{
NextState = WaitForStart;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
ES_Timer_SetTimer(7, 100);
ES_Timer_StartTimer(7);
} else if (CurrentEvent.EventType == UpdateFinish) //If we don't, update the FAC again
{
updateFAC();
}
break;
//Waits for start command from FAC
case WaitForStart :
if (CurrentEvent.EventType == GameStart)
{
//Start the timer, rev up the shooter motor, turn on LED indicator.
twomin = 0;
SetDuty(20,1);
LEDon();
NextState = ShootingTargets;
MakeTransition = True;
EntryEventKind.EventType = ES_ENTRY;
ReturnEvent = CurrentEvent;
}
case Finished :
break;
}
if ( CurrentEvent.EventType == EV_CLEAR)
{
// First pass exit messages to the lower level machines
CurrentEvent.EventType = ES_EXIT;
RunMasterSM(CurrentEvent);
// Now pass entry messages, since this is a self transition
CurrentEvent.EventType = ES_ENTRY;
RunMasterSM(CurrentEvent);
}
if (MakeTransition == True)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunMasterSM(CurrentEvent);
CurrentState = NextState; //Modify state variable
// Execute entry function for new state
// this defaults to ES_ENTRY
RunMasterSM(EntryEventKind);
printf("Changed States\r\n");
}
// in the absence of an error the top level state machine should
// always return ES_NO_EVENT
CurrentEvent.EventType = ES_NO_EVENT;
return(CurrentEvent);
}
/****************************************************************************
Function
StartMasterSM
Parameters
ES_Event CurrentEvent
Returns
nothing
Description
Does any required initialization for this state machine
Notes
Author
J. Edward Carryer, 02/06/12, 22:15
****************************************************************************/
void StartMasterSM ( ES_Event CurrentEvent )
{
// local variable to get debugger to display the value of CurrentEvent
volatile ES_Event LocalEvent = CurrentEvent;
// if there is more than 1 state to the top level machine you will need
// to initialize the state variable
//ES_Timer_SetTimer(1, 2000);
//ES_Timer_StartTimer(1);
CurrentState = WaitingForFAC;
//CurrentState = ShootingTargets;
// now we need to let the Run function init the lower level state machines
// use LocalEvent to keep the compiler from complaining about unused var
RunMasterSM(LocalEvent);
return;
}
MasterState_t QueryMasterSM(void )
{
return CurrentState;
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event DuringMaster( ES_Event Event )
{
coord botcoord;
ES_Event currEvent;
// process ES_ENTRY & ES_EXIT events
if ( Event.EventType == ES_ENTRY)
{
//Start with cloaking servo tucked into bot
updateFAC();
// CloakBot();
} else if ( Event.EventType == GameEnd)
{
//Does all game ending activities here, and passes down to lower level, too
CurrentState = Finished;
Stop();
SetDuty(0, 1);
LEDoff();
setEmitter(0);
RunShootTargetSM(Event);
RunMovementSM(Event);
RunBallSM(Event);
RunShootPowerSM(Event);
RunCommSM(Event);
Event.EventType = ES_NO_EVENT;
} else if (Event.EventType == ES_TIMEOUT && Event.EventParam == 4)
{
//Forward movement timer, stop after timeout
Stop();
RunMovementSM(Event);
} else if (Event.EventType == ES_TIMEOUT && Event.EventParam == 1)
{
//Same as timer 4, but for turns
Stop();
RunMovementSM(Event);
RunShootTargetSM(Event);
RunShootPowerSM(Event);
} else if (Event.EventType == ES_TIMEOUT && Event.EventParam == 7)
{
//Game status updater
currEvent.EventType = ES_ENTRY;
StartCommSM(currEvent, 0x3F);
if (twomin++ >= 240)
{
//two minute timer
currEvent.EventType = GameEnd;
PostMasterSM(currEvent);
}
if (twomin < -1) // For before the game, keeps updating the FAC
{
ES_Timer_SetTimer(7, 100);
ES_Timer_StartTimer(7);
} else //For after the game, 500 ms for each update
{
ES_Timer_SetTimer(7, 500);
ES_Timer_StartTimer(7);
}
} else if ( Event.EventType == UpdateFinish) //Every time the FAC is updated, run this and also pass to lower levels to tell them it finished
{
updateActive();
if (color == 0)
{
botcoord = queryShip(15);
if((botcoord.x > 127) && (botcoord.x != 255)) color = 2;
else if((botcoord.x <= 127) && (botcoord.x != 0)) color = 1;
// printf("color %d\r\n", color);
}
RunMovementSM(Event);
RunShootTargetSM(Event);
RunShootPowerSM(Event);
} else if ( Event.EventType == ES_NEW_KEY) //Keyboard debugging
{
processKeyboard(Event.EventParam);
} else if ( Event.EventType == OC1Exp) //Used for IR emitters
{
RespondToOC1();
}
else //Pass down to state machines below if it doesn't fit the other categories
{
RunShootTargetSM(Event);
RunMovementSM(Event);
RunBallSM(Event);
RunShootPowerSM(Event);
RunCommSM(Event);
// printf("%d\r\n", Event.EventParam);
}
return(Event);
}
static ES_Event DuringShootingTargets( ES_Event Event)
{
ES_Event ReturnEvent = Event;
ES_Event currEvent;
if (Event.EventType == ES_ENTRY) //Starts shoot target SM
{
currEvent.EventType = ES_ENTRY;
StartShootTargetSM(currEvent);
printf("Starting ShootTargetSM\r\n");
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringMovingToFire( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY) //Starts movement to firing position
{
printf("Going to MovingToFire\r\n");
SetDuty(15,1); //Revs up motor
if(color == 1)
{
StartMovementSM(Event, 80, 127, 270, 4, 0);
} else if (color == 2)
{
StartMovementSM(Event, 176, 127, 90, 4, 0);
}
}
else if ( Event.EventType == ES_EXIT )
{
resetCurrentSector();
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringMovingToTape( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY) //Starts movement to pre-loading position
{
SetDuty(0,1);
if(color == 1)
{
StartMovementSM(Event, 51, 127, 270, 2, 1);
} else if (color == 2)
{
StartMovementSM(Event, 205, 127, 90, 2, 1);
}
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
//Moves forward until FAC detects bot, used to get out of supply depot camoflague
static ES_Event DuringMoveOutOfCamo( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY)
{
//Requests event checker to start looking for event of FAC detection
requestFind();
DriveForward();
updateFAC();
ES_Timer_SetTimer(4, 500);
ES_Timer_StartTimer(4);
}
else if ( Event.EventType == ES_EXIT )
{
Stop();
} else if (Event.EventType == ES_TIMEOUT && Event.EventParam == 4)
{
updateFAC();
DriveForward();
ES_Timer_SetTimer(4, 500);
ES_Timer_StartTimer(4);
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringPreparingToLoad( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY)
{
//Requests event checkers for rear bump detection
requestBump();
DriveReverse();
}
else if ( Event.EventType == ES_EXIT )
{
//Stops on exit
Stop();
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringLoading( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY)
{
StartBallSM(Event);
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static ES_Event DuringEndingGame( ES_Event Event)
{
ES_Event ReturnEvent = Event;
if (Event.EventType == ES_ENTRY)
{
}
else if ( Event.EventType == ES_EXIT )
{
} else
{
}
return(ReturnEvent);
}
static void SPIInit(void )
{
//SPI init, to receive messages, manual set for line
SPIBR = (_S12_SPPR1 | _S12_SPR2 | _S12_SPR1 | _S12_SPR0);
SPICR1 |= _S12_MSTR;
SPICR1 |= (_S12_CPOL | _S12_CPHA);
SPICR1 &= ~_S12_SSOE;
//SPICR1 |= (_S12_SPIE);
SPICR2 &= ~_S12_MODFEN;
SPICR2 &= ~(_S12_SPC0);
DDRS |= _S12_SS;
SPICR1 |= _S12_SPE;
}
static void PWMInit(void )
{
/*******Set Clock A Pre-scaler/Scale******/
/******* Channels 0, 1, 4, 5 ******/
PWMPRCLK |= (_S12_PCKA2|_S12_PCKA1); //set pre-scaler to % 64
PWMSCLA = 0x02; //set scaler to %8
/*******Set Clock B Pre-scaler/Scale******/
/******* Channels 2, 3 ******/
PWMPRCLK |= (_S12_PCKB2|_S12_PCKB1); //set pre-scaler to % 64
PWMSCLB = 0x02; //set scaler to %8
/************PWM channel 0************/
PWME |= _S12_PWME0; //enable PWM channel 0
//MODRR |= _S12_MODRR0; //leave control for TIMER
PWMCLK |= _S12_PCLK0; //set use prescaled+scaled clock A
PWMPOL |= _S12_PPOL0; //Set PWM polarity to 1 (starts high)
PWMPER0 = 250; //set to drive period
PWMDTY0 = 0; //initialize w/ 0% Duty Cycle
/************PWM channel 1************/
PWME |= _S12_PWME1; //enable PWM channel 1
MODRR |= _S12_MODRR1; //set control to PWM
PWMCLK |= _S12_PCLK1; //set use prescaled+scaled clock A
PWMPOL |= _S12_PPOL1; //Set PWM polarity to 1 (starts high)
PWMPER1 = 250; //set to drive period
PWMDTY1 = 0; //initialize w/ 0% Duty Cycle
/************PWM channel 2************/
PWME |= _S12_PWME2; //enable PWM channel 2
MODRR |= _S12_MODRR2; //set control to PWM (controls RIGHT MOTOR)
PWMCLK |= _S12_PCLK2; //set use prescaled+scaled clock B
PWMPOL |= _S12_PPOL2; //Set PWM polarity to 1 (starts high)
PWMPER2 = 250; //set to drive period
PWMDTY2 = 0; //initialize w/ 0% Duty Cycle
/************PWM channel 3************/
PWME |= _S12_PWME3; //enable PWM channel 3
MODRR |= _S12_MODRR3; //set control to PWM (controls LEFT MOTOR)
PWMCLK |= _S12_PCLK3; //set use prescaled+scaled clock B
PWMPOL |= _S12_PPOL3; //Set PWM polarity to 1 (starts high)
PWMPER3 = 250; //set to drive period
PWMDTY3 = 0; //initialize w/ 0% Duty Cycle
/************PWM channel 4************/
PWME |= _S12_PWME4; //enable PWM channel 4
MODRR |= _S12_MODRR4; //set control to PWM
PWMCLK |= _S12_PCLK4; //set use prescaled+scaled clock A
PWMPOL |= _S12_PPOL4; //Set PWM polarity to 1 (starts high)
PWMPER4 = 250; //set to drive period
PWMDTY4 = 0; //initialize w/ 0% Duty Cycle
/************PWM channel 5************/
PWME |= _S12_PWME5; //enable PWM channel 5
MODRR |= _S12_MODRR5; //set control to PWM
PWMCLK |= _S12_PCLK5; //set use prescaled+scaled clock A
PWMPOL |= _S12_PPOL5; //Set PWM polarity to 1 (starts high)
PWMPER5 = 250; //set to drive period
PWMDTY5 = 0; //initialize w/ 0% Duty Cycle
}
static void TimerInit(void )
{
//Timer init
TIM0_TSCR1 = _S12_TEN;
TIM0_TSCR2 = (_S12_PR1 | _S12_PR2); //64
//Timer 4 channel 0 used for IR detection
TIM0_TCTL3 |= (_S12_EDG4A);
TIM0_TIE &= ~_S12_C4I;
TIM0_TFLG1 = _S12_C4F;
//Timer 5 channel 1 used for IR emitter
TIM0_TIOS |= (_S12_IOS5);
TIM0_TCTL1 &= ~(_S12_OL5 | _S12_OM5);
//10 ms period to start
TIM0_TC5 = TIM0_TCNT + PERIOD10;
TIM0_TFLG1 = _S12_C5F;
EnableInterrupts;
}
static void ServoInitCh1(void )
{
//Set TIM1 to Servo control
TIM1_TSCR1 |= _S12_TEN;
// enable timer system
TIM1_TSCR2 = _S12_PR1 | _S12_PR0; // 011 = 24MHz / 8 = 3MHz
//Select channels on TIM1 for output compare
TIM1_TIOS |= (_S12_IOS4 | _S12_IOS5 | _S12_IOS6 | _S12_IOS7);
//Program a rise on compare
TIM1_TCTL1 |= (_S12_OL4 | _S12_OM4);
TIM1_TCTL1 |= (_S12_OL5 | _S12_OM5);
TIM1_TCTL1 |= (_S12_OL6 | _S12_OM6);
TIM1_TCTL1 |= (_S12_OL7 | _S12_OM7);
//Set bit to toggle overflow
TIM1_TTOV |= (_S12_TOV4 | _S12_TOV5 | _S12_TOV6 | _S12_TOV7);
//Output pulse width for inital position
TIM1_TC4 = 0xFFFF - 4500;
TIM1_TC5 = 0xFFFF - 4500;
TIM1_TC6 = 0xFFFF - 4500;
TIM1_TC7 = 0xFFFF - 4500;
//For any pins to remove from timer
}
//Updates active ships
static void updateActive(void )
{
int i;
//Searches for an enemy ship until an active one is found, and sets it for the rest of the runtime
if (enemyShip == 0)
{
for (i = 3; i < 20; i++)
{
if ((queryActive(i)) && (i != 15))
{
enemyShip = i;
printf("enemy bot = %d\r\n", enemyShip);
}
}
}
//Updates active ships
for (i = 20; i < 40; i++)
{
if (queryActive(i))
{
activeShips[i] = TRUE;
} else
{
activeShips[i] = FALSE;
}
}
}
//Used for IR detection, can disable or enable when necessary
void interrupt _Vec_tim0ch4 GetPeriod(void)
{
//Keeps track of last edge time and second to last edge
static long lastEdge, secondLastEdge = 0;
ES_Event currEvent;
long period, period2, perioduS, perioduS2;
int Time;
//Clear flag, gets current timer time
TIM0_TFLG1 = _S12_C4F;
Time = TIM0_TC4;
//Calculates all periods, then converts into uS
period = Time - lastEdge;
period2 = Time - secondLastEdge;
perioduS = (period * 267)/100;
perioduS2 = (period2 * 267)/100;
//Waits for 2 legitimate periods in a row
//Had them seperate, but wasn't necessary due to mechanical design
if ((perioduS <= 14200 && perioduS >= 13800) || (perioduS2 <= 14200 && perioduS2 >= 13800) || (perioduS <= 20200 && perioduS >= 19800) || (perioduS2 <= 20200 && perioduS2 >= 19800))
{
// if (color == 1)
// {
if (align == 1)
{
//Post aligned event and turn off interrupt
TIM0_TIE &= ~_S12_C4I;
currEvent.EventType = Aligned;
PostMasterSM(currEvent);
align = 0;
} else if (align != 0 && align != 1)
{
align -= 1;
}
// }
} else
{
align = 2;
}/*else if ()
{
// if (color == 2)
// {
TIM0_TIE &= ~_S12_C4I;
currEvent.EventType = Aligned;
PostMasterSM(currEvent);
align = 0;
// }
} */
secondLastEdge = lastEdge;
lastEdge = Time;
}
/*******************************
Public Functions
*******************************/
void requestAlign(void )
{
align = 2;
TIM0_TIE |= _S12_C4I;
}
int getColor(void )
{
return color;
}
//Calculates next target to hit
int getNextTarget(void )
{
int i;
int NumberOurShips = 0;
int NumberEnemyShips = 0;
if (color == 2)
{
NumberOurShips = queryBlueShips();
NumberEnemyShips = queryRedShips();
} else if (color == 1)
{
NumberOurShips = queryRedShips();
NumberEnemyShips = queryBlueShips();
}
if(NumberOurShips >= NumberEnemyShips && ammo == 1 && enemyShip == 6) return enemyShip;
if((NumberEnemyShips+3 <= NumberOurShips) || NumberEnemyShips == 0) return enemyShip; // enemy Bot
if (color == 1)
{
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 1 && i != shipJustShot)
{
return i;
}
}
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 4 && i != shipJustShot)
{
return i;
}
}
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 2 && i != shipJustShot)
{
return i;
}
}
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 5 && i != shipJustShot)
{
return i;
}
}
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 3 && i != shipJustShot)
{
return i;
}
}
for(i = 30; i < 40; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 6 && i != shipJustShot)
{
return i;
}
}
} else if (color == 2)
{
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 1 && i != shipJustShot)
{
return i;
}
}
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 4 && i != shipJustShot)
{
return i;
}
}
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 2 && i != shipJustShot)
{
return i;
}
}
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 5 && i != shipJustShot)
{
return i;
}
}
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 3 && i != shipJustShot)
{
return i;
}
}
for(i = 20; i < 30; i++)
{
if (activeShips[i] == TRUE && queryShip(i).sector == 6 && i != shipJustShot)
{
return i;
}
}
}
return enemyShip; // enemy Bot
//return enemyShip; // if no ships left, return enemy Bot
}
//Keeps track of last ship shot, so we don't have to wait for it to go down to calculate the next target
void setLastShip(int num)
{
shipJustShot = num;
if (color == 1)
{
if (queryBlueShips() <= 1)
{
shipJustShot = 0;
}
} else
{
if (queryRedShips() <= 1)
{
shipJustShot = 0;
}
}
}
void processKeyboard(uint16_t input)
{
ES_Event currEvent;
char key = (char)input;\
static int CurrentDuty = 50;
static int BatVoltage;
static int testangle = 360;
double Current_BatVoltage;
double ShooterMotorPWM;
static double ShootingDistance = 36*32/12;
//.
int i;
static int angle = 65;
static int angle2 = 150;
coord coordi;
currEvent.EventType = ES_ENTRY;
if (key == 'a')
{
updateFAC();
printf("Update FAC\r\n");
}
if (key == '2')
{
for (i = 3; i < 40; i++)
{
coordi = queryShip(i);
printf("%d, x = %d, y = %d, r = %d, sector = %d\r\n", i, coordi.x, coordi.y, coordi.r, coordi.sector);
}
}
if (key == 'm')
{
setEmitter(1);
printf("ActivateEmitter\r\n");
}
if (key == 'n')
{
setEmitter(0);
printf("DeactivateEmitter\r\n");
}
if (key == 'q')
{
angle2 += 5;
Servo_SetAngle(BallSpinServo, angle2);
printf("angle2 %d\r\n", angle2);
}
if (key == 'w')
{
angle2 -= 5;
Servo_SetAngle(BallSpinServo, angle2);
printf("angle2 %d\r\n", angle2);
}
if (key == 'e')
{
angle2 = 170;
Servo_SetAngle(BallSpinServo, angle2);
printf("angle2 %d\r\n", angle2);
}
if (key == '3')
{
ShootingDistance -=6*32/12;
Current_BatVoltage = (double)ADS12_ReadADPin(4);
ShooterMotorPWM = (CAL_BAT_VOLTAGE/Current_BatVoltage)*(ShootingDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
printf("PWM = %d, ShootingDist = %d\r\n", (int)ShooterMotorPWM, (int)(12*ShootingDistance/32));
SetDuty(ShooterMotorPWM,1);
printf("shooting dist %4.4f \r\n", (ShootingDistance*12/32));
}
if (key == '4')
{
ShootingDistance +=6*32/12;
Current_BatVoltage = (double)ADS12_ReadADPin(4);
ShooterMotorPWM = (CAL_BAT_VOLTAGE/Current_BatVoltage)*(ShootingDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
printf("PWM = %d, ShootingDist = %d\r\n", (int)ShooterMotorPWM, (int)(ShootingDistance*12/32));
SetDuty(ShooterMotorPWM,1);
}
if (key == '5')
{
ShootingDistance =36*32/12;
Current_BatVoltage = (double)ADS12_ReadADPin(4);
ShooterMotorPWM = (CAL_BAT_VOLTAGE/Current_BatVoltage)*(ShootingDistance*SHOOTER_DISTANCE_SCALE_M + SHOOTER_DISTANCE_SCALE_B);
printf("PWM = %d, ShootingDist = %d\r\n", (int)ShooterMotorPWM, (int)(ShootingDistance*12/32));
SetDuty(ShooterMotorPWM,1);
}
if (key == '6')
{
CurrentDuty =18;
Current_BatVoltage = (double)ADS12_ReadADPin(4);
SetDuty(CurrentDuty,1);
printf("currentduty %d \r\n", CurrentDuty);
}
if (key == 'y')
{
Stop();
}
if (key == 'u')
{
DriveForward();
}
if (key == 'i')
{
DriveReverse();
}
if (key == '7')
{
RotateCW(360, 1);
}
if (key == 'b')
{
shootBall();
printf("LoadServo 112\r\n");
}
if (key == 'v')
{
Servo_SetAngle(LoadServo, 80);
}
if (key == 'd')
{
resetLoad();
printf("LoadServo 55\r\n");
}
if (key == 'k')
{
RotateCW(360, 1);
}
if (key == 'l')
{
RotateCCW(360, 1);
}
if (key == '8')
{
loadShooter();
printf("LoadShooter\r\n");
}
if (key == '9')
{
currEvent.EventType = ES_ENTRY;
StartBallSM(currEvent);
printf("Start Loading\r\n");
}
if(key == 's')
{
printf("Event to ShootPower\r\n");
currEvent.EventType = ES_ENTRY;
StartShootPowerSM(currEvent);
}
if(key == 'p')
{
printf("Aligned\r\n");
currEvent.EventType = Aligned;
PostMasterSM(currEvent);
align = 0;
}
if(key == 'c')
{
BatVoltage = ADS12_ReadADPin(4);
printf("bat volt %d \r\n", BatVoltage);
}
if(key == 'g')
{
printf("Green LED on\r\n");
LEDon();
}
if(key == 'h')
{
printf("Red LED on\r\n");
LEDoff();
}
}
/************************************
CLOAK STUFF
************************************/
void CloakBot(void)
{
Servo_SetAngle(FiducialServo,0);
ES_Timer_SetTimer(3,1000);
ES_Timer_StartTimer(3);
}
void UncloakBot(void)
{
Servo_SetAngle(FiducialServo, 30);
}
/************************************
AMMO TRACKING STUFF
************************************/
int getAmmo(void )
{
return ammo;
}
void changeAmmo(int change)
{
ES_Event currEvent;
ammo += change;
printf("Ammo = %d\r\n", ammo);
if (ammo == 0)
{
printf("Out Of Balls\r\n");
currEvent.EventType = OutOfBalls;
PostMasterSM(currEvent);
}
}
/************************************
EMITTER STUFF
************************************/
//Pass 1 in for on, 0 in for off
void setEmitter(int onoff )
{
if(onoff == 1)
{
emitterActive = 1;
TIM0_TFLG1 = _S12_C5F;
TIM0_TC5 += PERIOD30;
}
else if (onoff == 0)
{
emitterActive = 0;
PTT &= BIT1LO;
}
}
static void RespondToOC1(void)
{
TIM0_TFLG1 = _S12_C5F;
if (emitterActive == 1)
{
if (PTT & BIT1HI) //If PTT is high set it low and add 30ms to period for low time
{
PTT &= BIT1LO;
TIM0_TC5 += PERIOD30;
}
else //If PTT is low set it high and add 10ms to period for high time
{
PTT |= BIT1HI;
TIM0_TC5 += PERIOD10;
}
}
}
/************************************
IN GAME LED
************************************/
void LEDon(void)
{
PTP |= BIT1HI;
}
void LEDoff(void)
{
PTP &= ~(BIT1HI);
}