void wait(unsigned long waitTime)
{
unsigned long startTime = ES_Timer_GetTime();
unsigned long currTime= ES_Timer_GetTime();
printf("Wait %i ms\r\n",(int) waitTime);
while(currTime-startTime<waitTime && currTime>=startTime)
{
driveDebug();
currTime=ES_Timer_GetTime();
}
}
uchar Check4ControlReq(void)
{
long currTime=ES_Timer_GetTime();
ES_Event ThisEvent;
uchar retVal=0;
uchar hzvNum;
if(currTime-lastContTime > TIME_PER_PHONE_CHECK || currTime<lastContTime)
{
hzvNum=getKeyboardReq();
if(hzvNum<NUM_HZVS)
{
ThisEvent.EventType=ES_ControlBypass;
ThisEvent.EventParam=0x0A;
PostLabFSM(ThisEvent);
retVal=1;
lastContTime=currTime;
}
hzvNum=getPhoneReq(); //returns NUM_HZVS if no phone req is made
if(hzvNum<NUM_HZVS) //we got a valid hzvNum to target
{
ThisEvent.EventType=ES_ControlReq;
ThisEvent.EventParam= hzvNum;
PostLabFSM(ThisEvent);
retVal=1;
lastContTime=currTime;
}
}
return retVal;
}
uchar Check4Ack(void)
{
long currTime=ES_Timer_GetTime();
ES_Event ThisEvent;
uchar retVal=0;
if(currTime-lastRcvTime > TIME_PER_EV_CHECK || currTime<lastRcvTime)
{
if(gotAck())
{
ThisEvent.EventType=ES_Ack;
PostLabFSM(ThisEvent);
retVal=1;
lastRcvTime=currTime;
printf("ACK\r\n");
}
else if(gotControlAck())
{
ThisEvent.EventType=ES_ControlAck;
PostLabFSM(ThisEvent);
retVal=1;
lastRcvTime=currTime;
printf("CONT ACK\r\n");
}
}
return retVal;
}
boolean CheckButtonEvents(void)
{
//Local ReturnVal = False
boolean ReturnVal = False;
unsigned int CurrentTime = ES_Timer_GetTime();
//Get current button state
char CurrentButtonState = PTP&BIT0HI;
//If more than the debounce interval has elapsed since the last sampling
if((CurrentTime-TimeOfLastButtonSample)>DEBOUNCE_TIME)
{
//puts("latch");
//If the current button state is different from the LastButtonState
if ((CurrentButtonState != LastButtonState) &&(CurrentButtonState != 0))
{
//PostEvent ButtonDown (whether down or up)
ES_Event ThisEvent;
ThisEvent.EventType = BUTTON_PRESSED;
ThisEvent.EventParam = 1;
ES_PostList00(ThisEvent);
//printf("button fired.\n\r");
//Record current time as TimeOfLastSample
TimeOfLastButtonSample = ES_Timer_GetTime();
//Set ReturnVal = True
ReturnVal = True;
//Set LastButtonState to the current button state
//Endif
}
}
// update variables
LastButtonState = CurrentButtonState;
//Return ReturnVal
return ReturnVal;
//End of CheckButtonEvents
}
/****************************************************************************
Function
generateNewKey
Description
Randomly generates a new, 32 byte key
****************************************************************************/
uint8_t * GenerateNewKey( void){
currentCipherPosition = 0;
uint16_t TimeSeed;
TimeSeed = ES_Timer_GetTime();
srand(TimeSeed);
for(int i = 0; i < KEY_LENGTH; i ++){
uint8_t nextByte = rand();
*(encryptionKey + i) = nextByte;
}
return encryptionKey;
}
void initCheckers(void)
{
lastHandshake=ES_Timer_GetTime();
lastRcvTime=ES_Timer_GetTime();
classifyStartTime=ES_Timer_GetTime();
currHit[LEFT]=0;
currHit[RIGHT]=0;
bg[LEFT]=0;
bg[RIGHT]=0;
speedState[0]=0;
speedState[1]=0;
currHit[0]=ES_Timer_GetTime();
currHit[1]=ES_Timer_GetTime();
prevHit[0]=ES_Timer_GetTime();
prevHit[1]=ES_Timer_GetTime();
}
uchar Check4Handshake(void)
{
long currTime=ES_Timer_GetTime();
ES_Event ThisEvent;
uchar retVal=0;
if(currTime-lastHandshake > TIME_PER_EV_CHECK || currTime<lastRcvTime)
{
if(gotHandshake())
{
ThisEvent.EventType=ES_Handshake;
PostLabFSM(ThisEvent);
retVal=1;
lastRcvTime=currTime;
printf("Handshake\r\n");
}
}
return retVal;
}
char getBucket(uchar i)
{
long currTime=ES_Timer_GetTime();
long dt=currHit[i]-prevHit[i];
if(currTime-currHit[i]>1000 || currHit[i]>currTime) //how long has passed since last hit
{
return 0; //havent seen this bucket in awhile
}
else
{
if(dt<300) //FAST hit time
return 3;
else if(dt<500) //MED hit time.
return 2;
else //SLOW hit time
return 1;
}
}
/**
* @Function RunTimerService(ES_Event ThisEvent)
* @param ES_Event - the event to process
* @return ES_NO_EVENT or ES_ERROR
* @brief accepts the timer events and updates the state arrays
* @author Max Dunne 2013.01.04 */
ES_Event RunTimerService(ES_Event ThisEvent) {
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
switch (ThisEvent.EventType) {
case ES_INIT:
break;
case ES_TIMEOUT:
case ES_TIMERACTIVE:
case ES_TIMERSTOPPED:
UserTimerStates[ThisEvent.EventParam] = ThisEvent.EventType;
break;
default:
//ReturnEvent.EventType = ES_ERROR;
break;
}
#ifdef TIMER_SERVICE_TEST
{
uint8_t i;
switch (timerServiceTestingState) {
case init:
if (ThisEvent.EventType == ES_INIT) {
printf("Timer Module INITED succesfully\r\n");
break;
}
printf("Timer %d had event %d at time %d\r\n", ThisEvent.EventParam, ThisEvent.EventType, ES_Timer_GetTime());
if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMERACTIVE)) {
timerServiceTestingState = expired;
printf("Testing timer user functions [expired][stopped][active]{state}\r\n");
}
break;
case expired:
for (i = 0; i < TIMERS_USED; i++) {
printf("(%d):[%d,%d,%d]{%d} ", i, IsTimerExpired(i), IsTimerStopped(i), IsTimerActive(i), GetUserTimerState(i));
}
printf("\r\n");
if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMEOUT)) {
timerServiceTestingState = runstop;
ES_Timer_InitTimer(0, 500);
for (i = 1; i < TIMERS_USED; i++) {
ES_Timer_SetTimer(i, 1000);
ES_Timer_StartTimer(i);
}
}
break;
case runstop:
printf("Timer %d had event %d at time %d\r\n", ThisEvent.EventParam, ThisEvent.EventType, ES_Timer_GetTime());
if ((ThisEvent.EventParam == (0)) && (ThisEvent.EventType == ES_TIMEOUT)) {
for (i = 1; i < TIMERS_USED; i++) {
ES_Timer_StopTimer(i);
}
}
if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMERSTOPPED)) {
printf("Testing of User Timer Functions is complete.\r\n");
}
break;
default:
ReturnEvent.EventType = ES_ERROR;
break;
}
}
#endif
//ES_Tail();
return ReturnEvent;
}
/****************************************************************************
Function
RunDisarmFSM
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
The state machine for the overall disarming process - state machine for
keypad, etc. are elsewhere
Notes
****************************************************************************/
ES_Event RunDisarmFSM( ES_Event ThisEvent )
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
// initial state: all the LEDs are off.
static char LEDs[8] = {OFF, OFF, OFF, OFF, OFF, OFF , OFF, OFF};
static bool tower_rotate_direction = true;
switch ( CurrentState )
{
case Armed :
switch ( ThisEvent.EventType ) {
case ES_INIT :
// intializing timer to be at 2mS rate (for 60 second count down)
ES_Timer_Init(ES_Timer_RATE_2mS);
printf("Arming...\r\n");
// sets the armed line to +5V
setArmed();
printf(" Setting all tower LEDS off...\r\n");
LEDShiftRegInit();
// all LEDs are off when armed
setLED (LEDs);
printf(" Generating random passwords...\r\n");
// generate random password for keyboard input
randomizePasswords();
printArmedMessage();
printf(" Lowering flag...\r\n");
// lower the flag when armed
lowerFlag();
printf(" Locking the keys\r\n");
// lock the key when armed
lockKeys();
printf(" Setting tower to 0...\r\n\r\n");
// the tower doesn't lean when armed
setTowerToZero();
printf("STATE: Armed\r\n\r\n");
rotateTowerLeft();
ES_Timer_InitTimer(PANIC_TIMER, 350);
// demonstrate panic and start timing
ES_Timer_StartTimer(PANIC_TIMER);
break;
case THREE_HANDS_ON :
printf("EVENT: Three hands detected.\r\n");
printf(" Setting Tower Tier 1 LED on...\r\n");
LEDs[Tier1] = ON;
// light up the LED for bottom layer to show success in task 1
setLED(LEDs);
printf(" Begin printing LCD passcode...\r\n");
resetLCDmessage();
printLCDmessage();
// sends out a message every 2 seconds (2mS timer rate)
ES_Timer_InitTimer(MESSAGE_TIMER, 1000);
ES_Timer_StartTimer(MESSAGE_TIMER);
printf(" Starting 60s disarm timer...\r\n");
//the timer can only go up to 32000
ES_Timer_InitTimer(DISARM_TIMER, 30000);
ES_Timer_StartTimer(DISARM_TIMER);
//begins to run the timing motor
unwindTimingMotor();
// grabs current time to store for rewinding the timer motor
startTime = ES_Timer_GetTime();
ES_Event ThisEvent;
ThisEvent.EventType = PLAY_TRACK;
// play sound track 01
ThisEvent.EventParam = 1;
PostAdafruitAudioService(ThisEvent);
printf(" Transitioning to Stage 1...\r\n\r\n");
CurrentState = Stage1;
printf("STATE: Stage1\r\n\r\n");
break;
case ES_TIMEOUT :
// if panic timer expires
if (ThisEvent.EventParam == PANIC_TIMER) {
if (tower_rotate_direction) {
tower_rotate_direction = false;
rotateTowerRight();
} else {
tower_rotate_direction = true;
rotateTowerLeft();
}
ES_Timer_InitTimer(PANIC_TIMER, 350);
ES_Timer_StartTimer(PANIC_TIMER);
}
// if timing motor rewind timer expires
if (ThisEvent.EventParam == REWIND_TIMER) {
// stop timing motor when the ball reaches the top
stopTimingMotor();
}
default:
;
}
break;
case Stage1 :
switch ( ThisEvent.EventType )
{
case ES_TIMEOUT :
if (ThisEvent.EventParam == DISARM_TIMER) {
// if the disarm timer expires
printf("EVENT: Time has run out!\r\n");
printTimeUp();
// gets the time when the game ended
endTime = ES_Timer_GetTime();
// calculate how much rewinding needs to be done
ES_Timer_InitTimer(REWIND_TIMER, (endTime-startTime)/2);
ES_Timer_StartTimer(REWIND_TIMER);
// begins rewinding the clock motor
rewindTimingMotor();
ThisEvent.EventType = ES_INIT;
PostDisarmFSM(ThisEvent);
// go back to armed state
CurrentState = Armed;
}
// if vibration timer expires
if (ThisEvent.EventParam == VIBRATION_TIMER) {
printf(" Turning off vibration pulse...\r\n\r\n");
vibrationMotorOff();
}
// if message timer expires
if (ThisEvent.EventParam == MESSAGE_TIMER) {
printf("EVENT: Printing out the next message...\r\n");
printLCDmessage();
ES_Timer_InitTimer(MESSAGE_TIMER, 1000);
ES_Timer_StartTimer(MESSAGE_TIMER);
}
break;
// one or more hands have been released
case THREE_HANDS_OFF :
printf("EVENT: One or more hands have been released.\r\n");
printf(" Clearing the LCD screen\r\n");
printArmedMessage();
printf(" Setting Tower Tier 1 LED off...\r\n");
// task 1 is not completed
LEDs[Tier1] = OFF;
// turn off the LED for bottom layer
setLED(LEDs);
printf(" Transitioning to Stage1_Stagnated...\r\n\r\n");
CurrentState = Stage1_Stagnated;
printf("STATE: Stage1_Stagnated\r\n\r\n");
break;
// if the user entered the correct password
case CORRECT_PASSWORD_ENTERED :
printf("EVENT: The correct password has been entered.\r\n");
printf(" Unlocking the keys\r\n");
// unlock the key and move to task 3
unlockKeys();
printAuthorizedMessage();
printf(" Setting Tower Tier 2 LED on...\r\n");
LEDs[Tier2] = ON;
// turn on LED on the second tier to show success
setLED(LEDs);
printf(" Playing audio: Wahoo!...\r\n");
ES_Event ThisEvent;
ThisEvent.EventType = PLAY_TRACK;
// play sound track 01
ThisEvent.EventParam = 1;
PostAdafruitAudioService(ThisEvent);
printf(" Transitioning to Stage2...\r\n\r\n");
// set the current state to state 2
CurrentState = Stage2;
printf("STATE: Stage2\r\n\r\n");
break;
// if the user entered incorrect password
case INCORRECT_PASSWORD_ENTERED :
printf("EVENT: The incorrect password has been entered.\r\n");
printIncorrectMessage();
ES_Timer_InitTimer(MESSAGE_TIMER, 1000);
ES_Timer_StartTimer(MESSAGE_TIMER);
printf(" Generating vibration pulse...\r\n");
break;
default :
;
}
break;
case Stage1_Stagnated :
switch ( ThisEvent.EventType )
{
case ES_TIMEOUT :
// if disarm timer expires
if (ThisEvent.EventParam == DISARM_TIMER) {
printf("EVENT: Time has run out!\r\n");
printTimeUp();
endTime = ES_Timer_GetTime();
// calculate how much rewinding needs to be done
ES_Timer_InitTimer(REWIND_TIMER, (endTime-startTime)/2);
ES_Timer_StartTimer(REWIND_TIMER);
// begins rewinding the timing motor
rewindTimingMotor();
ThisEvent.EventType = ES_INIT;
PostDisarmFSM(ThisEvent);
// go back to armed state
CurrentState = Armed;
}
break;
// if all three tape sensors are covered
case THREE_HANDS_ON :
printf("EVENT: Three hands detected.\r\n");
printf(" Setting Tower Tier 1 LED on...\r\n");
LEDs[Tier1] = ON;
// light up the LED for bottom layer to show success in task 1
setLED(LEDs);
printf(" Begin printing LCD passcode...\r\n");
resetLCDmessage();
printLCDmessage();
// print message from LCD
ES_Timer_InitTimer(MESSAGE_TIMER, 1000);
ES_Timer_StartTimer(MESSAGE_TIMER);
// play feedback audio wahoo
ES_Event ThisEvent;
ThisEvent.EventType = PLAY_TRACK;
// play sound track 01
ThisEvent.EventParam = 1;
PostAdafruitAudioService(ThisEvent);
printf(" Transitioning to Stage1...\r\n\r\n");
// set current stage to stage 1
CurrentState = Stage1;
printf("STATE: Stage1\r\n\r\n");
break;
default :
;
}
break;
case Stage2 :
switch ( ThisEvent.EventType )
{
case ES_TIMEOUT :
// if the disarm timer expires
if (ThisEvent.EventParam == DISARM_TIMER) {
printf("EVENT: Time has run out!\r\n");
printTimeUp();
// gets the time when the game ended
endTime = ES_Timer_GetTime();
// calculate how much rewinding needs to be done
ES_Timer_InitTimer(REWIND_TIMER, (endTime-startTime)/2);
ES_Timer_StartTimer(REWIND_TIMER);
// begins rewinding the clock moto
rewindTimingMotor(); r
ThisEvent.EventType = ES_INIT;
PostDisarmFSM(ThisEvent);
CurrentState = Armed;
}
break;
// if the key is inserted
case KEY_INSERTED:
printf("EVENT: Key has been inserted.\r\n");
printf(" Setting Tower Tier 3 LED on...\r\n");
LEDs[Tier3] = ON;
// light up LED on tier 3 to show successful completion of task 3
setLED(LEDs);
printf(" Setting Dial LED on...\r\n");
LEDs[pot] = ON;
setLED(LEDs);
printf(" Playing audio: Wahoo!...\r\n");
ES_Event ThisEvent;
ThisEvent.EventType = PLAY_TRACK;
// play sound track 01
ThisEvent.EventParam = 1;
PostAdafruitAudioService(ThisEvent);
printf("Initializing the pot value...\r\n");
setPotZero();
printf(" Transitioning to Stage3...\r\n\r\n");
// set current state to state 3
CurrentState = Stage3;
printf("STATE: Stage3\r\n\r\n");
break;
default :
;
}
break;
case Stage3 :
switch ( ThisEvent.EventType )
{
case ES_TIMEOUT :
// if the disarm timer expires
if (ThisEvent.EventParam == DISARM_TIMER) {
printf("EVENT: Time has run out!\r\n");
printTimeUp();
endTime = ES_Timer_GetTime();
// calculate how much rewinding needs to be done
ES_Timer_InitTimer(REWIND_TIMER, (endTime-startTime)/2);
ES_Timer_StartTimer(REWIND_TIMER);
// begins rewinding the clock motor
rewindTimingMotor();
ThisEvent.EventType = ES_INIT;
PostDisarmFSM(ThisEvent);
// set current state to armed
CurrentState = Armed;
}
if (ThisEvent.EventParam == FAST_LEDS) {
printf("EVENT: \r\n");
ThisEvent.EventType = CORRECT_VALUE_DIALED;
PostDisarmFSM(ThisEvent);
}
break;
// if the pot is dialed to correct value
case CORRECT_VALUE_DIALED :
printf("EVENT: The correct pot value has been dialed.\r\n");
// Sets the armed line to 0V
setUnarmed();
printf(" Setting Tower Tier 4-6 LED on with delay...\r\n");
ES_Timer_InitTimer(FAST_LEDS, 150);
static int i = Tier4;
if (i<=Tier6){
printf("\n\r looping for LED i + %d\n\r", i);
// turn on all the remaining LEDs one by one
LEDs[i] = 0;
setLED(LEDs);
ES_Timer_StartTimer(FAST_LEDS);
i++;
break;
}
printf(" Raising the flag...\r\n");
// raise flag to show hope and joy
raiseFlag();
printf(" Playing audio: victory song...\r\n");
ES_Event ThisEvent;
ThisEvent.EventType = PLAY_TRACK;
// play sound track 03
ThisEvent.EventParam = 3;
PostAdafruitAudioService(ThisEvent);
printf(" Starting 30s post-disarm timer...\r\n");
ES_Timer_InitTimer(POST_DISARM_TIMER, 30000);
printf(" Raising ball and feather...\r\n");
// stop falling ball
stopTimingMotor();
// get the time when the disarment ends and rearm DDM
endTime = ES_Timer_GetTime();
ES_Timer_InitTimer(REWIND_TIMER, (endTime-startTime)/2);
ES_Timer_StartTimer(REWIND_TIMER);
// begins rewinding the clock motor
rewindTimingMotor();
printf(" Transitioning to Stage4...\r\n\r\n");
// set current stage to stage 4
CurrentState = Stage4;
printf("STATE: Stage4\r\n\r\n");
break;
default :
;
}
break;
case Stage4 :
switch ( ThisEvent.EventType )
{
case ES_TIMEOUT :
// if timer expires after successful disarment
if (ThisEvent.EventParam == POST_DISARM_TIMER) {
printf("EVENT: Post-disarm timer expired.\r\n");
ThisEvent.EventType = ES_INIT;
PostDisarmFSM(ThisEvent);
// set current state to armed
CurrentState = Armed;
}
// if rewind timer expires
if (ThisEvent.EventParam == REWIND_TIMER) {
// stop timing motor rewinding
stopTimingMotor();
}
break;
default :
;
}
break;
default :
;
}
return ReturnEvent;
}
void PathPlanner(void)
{
if(QueryTankSM()!=DISABLED)
{
static float CurrentPosition[3] = {0,0,0};
static float CurrentWallLine[2];
static int CurrentWallAngle;
static ActivityState_t CurrentGoal;
ES_Event ThinkTankEvent;
ES_Event GameMonitorEvent;
srand(ES_Timer_GetTime());
if( CurrentGoal != QueryActivityControlSM())
{
//printf("clearing checkpoints\n\r");
wigglecounter = 0;
BinCheckPoints = 0;
WallCheckPoints = 0;
// GOOD?
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
CurrentGoal = QueryActivityControlSM();
} else
{
//ES_Event TankEvent;
//CurrentGoal = QueryActivityControlSM();
CurrentWallLine[0]= GetWallSlope();
CurrentWallLine[1]= GetWallIntercept();
CurrentWallAngle = GetWallAngle();
UpdateCurrentPosition();
CurrentPosition[X_COORDINATE] = GetX();
CurrentPosition[Y_COORDINATE] = GetY();
CurrentPosition[THETA] = GetTheta();
//UpdateWallPoints(); // before getting staging points for wall
switch (CurrentGoal)
{
//ES_Event ActionEvent;
case RASTER :
{
static char i;
//ThinkTankEvent.EventType = GO_COMMAND;
//ThinkTankEvent.EventParam = 100;
//Calibration test code
/*
if (i < 5)
{
ThinkTankEvent.EventType = GO_COMMAND;
ThinkTankEvent.EventParam = STEP_DISTANCE;
i++;
} else if (i<6)
{
ThinkTankEvent.EventType = TURN_COMMAND;
ThinkTankEvent.EventParam = 1080;
i++;
} else if (i<7)
{
ThinkTankEvent.EventType = TURN_COMMAND;
ThinkTankEvent.EventParam = -1080;
i++;
} else
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
i=0;
}
*/
// go forward thrice, turn randomly once, unless threatened by wall, then flee for safety.
if (CurrentLocationIsSafe( CurrentPosition[0], CurrentPosition[1], CurrentPosition[2], CurrentWallAngle) == True) //our current location is a safe one
{
if (i < 3)
{
if (SafeToTravel((float)STEP_DISTANCE/100.,CurrentPosition[0],CurrentPosition[1],CurrentPosition[2], CurrentWallAngle) == True)
{
ThinkTankEvent.EventType = GO_COMMAND;
ThinkTankEvent.EventParam = STEP_DISTANCE;
//SetMotorTimer(STEP_DISTANCE);
} else
{
ThinkTankEvent.EventType = TURN_COMMAND;
ThinkTankEvent.EventParam = (rand()%360) - 180;
//SetMotorTimer(angle);
}
i++;
} else if (i<4)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
i++;
} else
{
ThinkTankEvent.EventType = TURN_COMMAND;
ThinkTankEvent.EventParam = (rand()%360) - 180;
//SetMotorTimer(angle);
i = 0;
}
} else
{
//go to safe point
ThinkTankEvent = GenerateNextStep_EscapeWallPath(CurrentPosition,CurrentWallAngle);
}
}
break;
case CONTROL_LEFT_WALL :
//printf("Wall Third Point is x: %d y: %d \n\r", PushPointX(),PushPointY());
/*
if(GetSeparationDistance( -- parameters for push point -- ) < 6)
{
WallCheckPoints = 2;
}
*/
if (WallCheckPoints < 1)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = WallFirstPointX();
GoalPoint[1] = WallFirstPointY();
//NextGoalPoint[0] = WallStagingPointX(LEFT);
//NextGoalPoint[1] = WallStagingPointY(LEFT);
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (WallCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = WallStagingPointX(LEFT);
GoalPoint[1] = WallStagingPointY(LEFT);
//NextGoalPoint[0] = PushPointX();
//NextGoalPoint[1] = PushPointY();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (WallCheckPoints < 3)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = PushPointX();
GoalPoint[1] = PushPointY();
// change this!!
//NextGoalPoint[0] = PushPointX();
//NextGoalPoint[1] = PushPointY();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else
{
// THINK THROUGH THIS
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
//CurrentGoal = STAY_PUT (will start process over again)
//WallCheckPointThree = False;
}
break;
case CONTROL_RIGHT_WALL :
//printf("Wall Third Point is x: %d y: %d \n\r", PushPointX(),PushPointY());
/*
if(GetSeparationDistance( -- parameters for staging point -- ) < 8)
{
WallCheckPoints = 2;
}
*/
if (WallCheckPoints < 1)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = WallFirstPointX();
GoalPoint[1] = WallFirstPointY();
//NextGoalPoint[0] = WallStagingPointX(LEFT);
//NextGoalPoint[1] = WallStagingPointY(LEFT);
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (WallCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = WallStagingPointX(RIGHT);
GoalPoint[1] = WallStagingPointY(RIGHT);
//NextGoalPoint[0] = PushPointX();
//NextGoalPoint[1] = PushPointY();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (WallCheckPoints < 3)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = PushPointX();
GoalPoint[1] = PushPointY();
// change this!!
//NextGoalPoint[0] = PushPointX();
//NextGoalPoint[1] = PushPointY();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else
{
// THINK THROUGH THIS
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
//CurrentGoal = STAY_PUT (will start process over again)
//WallCheckPointThree = False;
}
break;
case DEPOSIT_IN_BIN_ONE :
if (BinCheckPoints < 1)
{
ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = StagingPointForBinOneX;
GoalPoint[1] = StagingPointForBinOneY;
//NextGoalPoint[0] = BinOneDepositX;
//NextGoalPoint[1] = BinOneDepositY;
GameMonitorEvent.EventType = START_AVERAGING;
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 3)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 4)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 5)
{
GameMonitorEvent.EventType = STOP_AVERAGING;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
BinCheckPoints++;
} else if (BinCheckPoints < 6)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = DepositPointForBinOneX;
GoalPoint[1] = DepositPointForBinOneY;
//UpdateCurrentPosition();
//CurrentPosition[X_COORDINATE] = GetX();
//CurrentPosition[Y_COORDINATE] = GetY();
//CurrentPosition[THETA] = GetTheta();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND); /* checkpoint */
} else
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
Drive(GENTLE_FORWARD);
}
break;
case DEPOSIT_IN_BIN_TWO :
if (BinCheckPoints < 1)
{
ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = StagingPointForBinTwoX;
GoalPoint[1] = StagingPointForBinTwoY;
//NextGoalPoint[0] = BinOneDepositX;
//NextGoalPoint[1] = BinOneDepositY;
GameMonitorEvent.EventType = START_AVERAGING;
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 3)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 4)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 5)
{
GameMonitorEvent.EventType = STOP_AVERAGING;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
BinCheckPoints++;
} else if (BinCheckPoints < 6)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = DepositPointForBinTwoX;
GoalPoint[1] = DepositPointForBinTwoY;
//UpdateCurrentPosition();
//CurrentPosition[X_COORDINATE] = GetX();
//CurrentPosition[Y_COORDINATE] = GetY();
//CurrentPosition[THETA] = GetTheta();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND); /* checkpoint */
} else
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
Drive(GENTLE_FORWARD);
}
break;
case DEPOSIT_IN_BIN_THREE :
if (BinCheckPoints < 1)
{
ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = StagingPointForBinThreeX;
GoalPoint[1] = StagingPointForBinThreeY;
//NextGoalPoint[0] = BinOneDepositX;
//NextGoalPoint[1] = BinOneDepositY;
GameMonitorEvent.EventType = START_AVERAGING;
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 3)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 4)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 5)
{
GameMonitorEvent.EventType = STOP_AVERAGING;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
BinCheckPoints++;
} else if (BinCheckPoints < 6)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = DepositPointForBinThreeX;
GoalPoint[1] = DepositPointForBinThreeY;
//UpdateCurrentPosition();
//CurrentPosition[X_COORDINATE] = GetX();
//CurrentPosition[Y_COORDINATE] = GetY();
//CurrentPosition[THETA] = GetTheta();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND); /* checkpoint */
} else if (BinCheckPoints < 7)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
Drive(GENTLE_FORWARD);
wigglecounter++;
if (30 < wigglecounter)
{
ES_Event DummyEvent;
DummyEvent.EventType = DEPOSIT_COMMAND;
DummyEvent.EventParam = 1;
ES_PostList00(DummyEvent);
//printf("dpt tmr end - DUMP\n\r");
BinCheckPoints++;
wigglecounter = 0;
}
} else
{
static int done_wiggling;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_WIGGLE_TIME;
if(done_wiggling < 22)
{
if (wigglecounter == 0)
{
float param = 50;
Drive(0);
Rotate(param);//*GENTLE_FORWARD);
puts("-");
ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND; // 4?
} else if (wigglecounter == 1)
{
float param = -75;
puts("+");
Drive(0);
Rotate(param);//GENTLE_FORWARD);
} else if(wigglecounter == 2)
{
float param = 50;
Drive(0);
Rotate(param);//*GENTLE_FORWARD);
puts("-");
ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND;
} else if(wigglecounter == 3)
{
float param = -75;
puts("+");
Drive(0);
Rotate(param);//GENTLE_FORWARD);
wigglecounter = 0;
}
} else if (done_wiggling == 6)
{
ES_Event DummyEvent;
Drive(0);
DummyEvent.EventType = DEPOSIT_COMPLETE;
DummyEvent.EventParam = 1;
//ES_PostList02(DummyEvent);
}
wigglecounter++;
done_wiggling++;
}
break;
case DEPOSIT_IN_BIN_FOUR :
if (BinCheckPoints < 1)
{
ThinkTankEvent = GenerateNextStep_RadialPath(CurrentPosition,RADIUS_OF_SAFETY-2*HALF_BOT_LENGTH,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 2)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = StagingPointForBinFourX;
GoalPoint[1] = StagingPointForBinFourY;
//NextGoalPoint[0] = BinOneDepositX;
//NextGoalPoint[1] = BinOneDepositY;
GameMonitorEvent.EventType = START_AVERAGING;
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_POSITION_FOUND);
} else if (BinCheckPoints < 3)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 4)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_POSITION_FOUND;
BinCheckPoints++;
} else if (BinCheckPoints < 5)
{
GameMonitorEvent.EventType = STOP_AVERAGING;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
BinCheckPoints++;
} else if (BinCheckPoints < 6)
{
float GoalPoint[2];
//float NextGoalPoint[2];
GoalPoint[0] = DepositPointForBinFourX;
GoalPoint[1] = DepositPointForBinFourY;
//UpdateCurrentPosition();
//CurrentPosition[X_COORDINATE] = GetX();
//CurrentPosition[Y_COORDINATE] = GetY();
//CurrentPosition[THETA] = GetTheta();
ThinkTankEvent = GenerateNextStep_GoalPoint(CurrentPosition,GoalPoint,WAIT_FOR_A_TENTH_SECOND); /* checkpoint */
} else if (BinCheckPoints < 7)
{
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_TENTH_SECOND;
Drive(GENTLE_FORWARD);
wigglecounter++;
if (30 < wigglecounter)
{
ES_Event DummyEvent;
DummyEvent.EventType = DEPOSIT_COMMAND;
DummyEvent.EventParam = 1;
ES_PostList00(DummyEvent);
//printf("dpt tmr end - DUMP\n\r");
BinCheckPoints++;
wigglecounter = 0;
}
} else
{
static int done_wiggling;
ThinkTankEvent.EventType = WAIT_COMMAND;
ThinkTankEvent.EventParam = WAIT_FOR_A_WIGGLE_TIME;
if(done_wiggling < 22)
{
if (wigglecounter == 0)
{
float param = 50;
Drive(0);
Rotate(param);//*GENTLE_FORWARD);
puts("-");
ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND; // 4?
} else if (wigglecounter == 1)
{
float param = -75;
puts("+");
Drive(0);
Rotate(param);//GENTLE_FORWARD);
} else if(wigglecounter == 2)
{
float param = 50;
Drive(0);
Rotate(param);//*GENTLE_FORWARD);
puts("-");
ThinkTankEvent.EventParam = 3*WAIT_FOR_A_TENTH_SECOND;
} else if(wigglecounter == 3)
{
float param = -75;
puts("+");
Drive(0);
Rotate(param);//GENTLE_FORWARD);
wigglecounter = 0;
}
} else if (done_wiggling == 6)
{
ES_Event DummyEvent;
Drive(0);
DummyEvent.EventType = DEPOSIT_COMPLETE;
DummyEvent.EventParam = 1;
//ES_PostList02(DummyEvent);
}
wigglecounter++;
done_wiggling++;
}
break;
}
}
/*
switch(ThinkTankEvent.EventType)
{
case(TURN_COMMAND) :
printf("Current Event is TURN_COMMAND\n\r");
break;
case(GO_COMMAND) :
printf("Current Event is GO_COMMAND\n\r");
break;
case(WAIT_COMMAND) :
printf("Current Event is WAIT_COMMAND\n\r");
break;
case(ARRIVED) :
printf("Current Event is whimsical.\n\r");
default:
printf("epic fail\n\r");
}
*/
if(QueryTankSM()!=DISABLED)
ES_PostList00(ThinkTankEvent);
ES_PostList04(ThinkTankEvent);
ES_PostList03(GameMonitorEvent);
printf("We're at X = %f, Y= %f, and Theta = %f, Wall is at %d\n\r", CurrentPosition[0],CurrentPosition[1],CurrentPosition[2],CurrentWallAngle);
//ES_PostList02(TankEvent);
//printf("event param = %f",ThinkTankEvent.EventParam);
}
}
/**
* @Function RunTemplateSubHSM(ES_Event ThisEvent)
* @param ThisEvent - the event (type and param) to be responded.
* @return Event - return event (type and param), in general should be ES_NO_EVENT
* @brief This function is where you implement the whole of the heirarchical state
* machine, as this is called any time a new event is passed to the event
* queue. This function will be called recursively to implement the correct
* order for a state transition to be: exit current state -> enter next state
* using the ES_EXIT and ES_ENTRY events.
* @note Remember to rename to something appropriate.
* The lower level state machines are run first, to see if the event is dealt
* with there rather than at the current level. ES_EXIT and ES_ENTRY events are
* not consumed as these need to pass pack to the higher level state machine.
* @author J. Edward Carryer, 2011.10.23 19:25
* @author Gabriel H Elkaim, 2011.10.23 19:25 */
ES_Event RunAlignCollect_SubHSM(ES_Event ThisEvent) {
uint8_t makeTransition = FALSE; // use to flag transition
SubAlignCollect_SubHSMState_t nextState; // <- change type to correct enum
ES_Tattle(); // trace call stack
ES_Event Tcontrol;
static int aligning_start_time, aligning_end_time;
static int aligning_difference_time = 0;
static int back_tape_flag = 0;
switch (CurrentState) {
case InitPSubState: // If current state is initial Psedudo State
if (ThisEvent.EventType == ES_INIT)// only respond to ES_Init
{
nextState = Buffer_State;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case Buffer_State:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
//PivotRight(newspeed);
break;
case ES_EXIT:
break;
case FORCED:
nextState = Checking;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
// ThisEvent.EventParam = ThisEvent.EventParam;
break;
case ES_TIMEOUT:
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
break;
default:
break;
}
}
break;
case Checking:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotRight(speed*1.2);//(speed*.5);
// TurnRight(speed);
// StraightForward(speed);
// ES_Timer_InitTimer(replungetimer, 300);
break;
case ES_EXIT:
break;
case TAPE:
if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
printf("\nback on\n");
aligning_start_time = ES_Timer_GetTime();
back_tape_flag = 1;
int i;
for(i = 0; i < 100000; i++)
{
asm("nop");
}
// nextState = Pivoting_Right_AC;
// makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if (((ThisEvent.EventParam & 0b000001) == 0b000000) && back_tape_flag) { // Back tape off T
aligning_end_time = ES_Timer_GetTime();
FullStop();
printf("\nback tape off\n");
int i;
for(i = 0; i < 100000; i++)
{
asm("nop");
}
aligning_difference_time = aligning_end_time - aligning_start_time;
// printf("\n\naligning_diff_time %d\n", aligning_difference_time);
nextState = Pivoting_Left_AC;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}// if ((ThisEvent.EventParam & 0b110000) == 0b110000) { // LL, L: if miss most of the stem/land on T top
// nextState = Pivoting_Right_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// else if ((ThisEvent.EventParam & 0b111100) == 0b111100) { // LL, L,F, R: if hit T stem head-on
// nextState = Turning_Right_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL entry
// nextState = Turning_Left_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
else {
ThisEvent.EventType = ES_NO_EVENT;
}
// else if ((ThisEvent.EventParam & T_Left) == T_Left) {
// nextState = Pivoting_Right;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else if ((ThisEvent.EventParam & T_Right) == T_Right) {
// nextState = Pivoting_Left;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
break;
case ES_TIMEOUT:
// nextState = Pivoting_Right_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
break;
default:
break;
}
}
break;
case Pivoting_Right_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotRight(tspeed * 1);
ES_Timer_InitTimer(replungetimer, 900);
break;
// case TAPE:
// if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
// nextState = Reverse_Adjusting_Right;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// else {
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// nextState = Lost;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
if (ThisEvent.EventParam == replungetimer) {
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
}
break;
case Pivoting_Left_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotLeft_WH(speed * .7);
int spintime = aligning_difference_time / 2;
// printf("\n\n%dcheck spin time\n", spintime);
ES_Timer_InitTimer(replungetimer, spintime);
break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// nextState = Lost;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
if (ThisEvent.EventParam == replungetimer) {
FullStop();
// printf("\n\ncheck\n");
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
}
break;
case Turning_Left_AC:
//ThisEvent = Run_AA_LineFollowing_SubHSM(ThisEvent); // run sub-state machine for this state
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
TurnLeft(speed);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b000100) == 0b000100) { // R
nextState = Turning_Right_AC; //TODO: straight on
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// ThisEvent.EventType = ES_NO_EVENT;
default: break;
}
}
break;
case Turning_Right_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
TurnRight(speed);
break;
// case BUMPED:
// nextState = Wall_Hugging;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// break;
case TAPE:
if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// nextState = Lost;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
break;
default: break;
}
}
break;
case Stopping_AC:
FullStop();
break;
case Reversing_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
StraightReverse(speed * 1.3);
ES_Timer_InitTimer(ammofindtimer, ammofindtimervalue);
break;
case ES_EXIT:
ES_Timer_StopTimer(ammofindtimer);
ES_Timer_StopTimer(portalsearchingtimer);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == ammofindtimer) {
ES_Timer_InitTimer(portalsearchingtimer, 600);
StraightForward(speed);
ThisEvent.EventType = ES_NO_EVENT;
} else if (ThisEvent.EventParam == portalsearchingtimer) {
FullStop();
Tcontrol.EventType = AMMO;
Tcontrol.EventParam = 0;
ThisEvent.EventType = ES_NO_EVENT;
PostDagobotHSM(Tcontrol);
}
break;
default:
break;
}
}
break;
case Forwarding_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
StraightForward(speed);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b001000) == 0b000000) { // Front off the T
nextState = Reversing_AC;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
break;
case ES_TIMEOUT:
break;
default:
break;
}
}
break;
case Reverse_Adjusting_Right:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// GentleLeft(-6 - .3); // backwards left = front of robot pivoting right
GentleRight(-6 - .3);
ES_Timer_InitTimer(replungetimer, replungetimervalue);
break;
// case TAPE:
// if ((ThisEvent.EventParam & 0b111110) == 0b000000) { // F
// nextState = Stopping_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else {
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
case ES_EXIT:
ES_Timer_StopTimer(replungetimer);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == replungetimer) {
nextState = Forwarding_AC;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
}
break;
case Adjusting_Right_AC:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
GentleRight(speed);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b011100) == 0b011100) { // L, C, R
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
break;
}
}
break;
case Reverse_Turning_Left:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
//ES_Timer_InitTimer(pivotingtimer, pivotingtimervalue);
ReverseLeft(speed); // fiz thinks this means reverse right // previously 5
ES_Timer_InitTimer(ammofindtimer, ammofindtimervalue);
break;
// case TAPE:
// if ((ThisEvent.EventParam & 0b011100) == 0b011100){ // L, C, R
// nextState = Reverse_Adjusting_Right;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
//
case ES_TIMEOUT:
if (ThisEvent.EventParam == ammofindtimer) {
Tcontrol.EventType = MISSED_T;
Tcontrol.EventParam = 0;
PostDagobotHSM(Tcontrol);
ThisEvent.EventType = ES_NO_EVENT;
}
// else if (plungecount == 4){
// debug("Oh, shit!");
// nextState = Stopping_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// //TODO: Deal with later!
// // post event to pop into higher state
// }
break;
case ES_EXIT:
ES_Timer_StopTimer(ammofindtimer);
break;
}
}
break;
default: // all unhandled states fall into here
break;
} // end switch on Current State
if (makeTransition == TRUE) {
RunAlignCollect_SubHSM(EXIT_EVENT); // <- rename to your own Run function
CurrentState = nextState;
RunAlignCollect_SubHSM(ENTRY_EVENT); // <- rename to your own Run function
}
ES_Tail(); // trace call stack end
return ThisEvent;
}
uchar Check4Drum(void)
{
static uchar timePerDrumCheck=50;
long currTime=ES_Timer_GetTime();
static uchar state =0; //0 is noiseDataCollection, 1 is Drumming
static int numPts=0;
static uchar lastRevState;
static uchar currRevState;
static uint drum[2];
static uint hits[2];
static int i;
static uchar recording=0;
static uchar hitInc=0;
if(! (currTime-lastDrumTime > timePerDrumCheck || currTime<lastDrumTime) )
return 0; //exit immediately if not enough time has passed
drum[LEFT]=(int) ADS12_ReadADPin(drumLPin);
drum[RIGHT]=(int) ADS12_ReadADPin(drumRPin);
if(state==0)
{
if(!recording)
{
if(numPts>=NUM_IN_A_ROW)
{
recording=1;
numPts=0;
bg[0]=0;
bg[1]=0;
}
if(absDiff(drum[LEFT],bg[LEFT]) > 10 || absDiff(drum[RIGHT],bg[RIGHT]) > 10 || bg[LEFT]<300 || bg[RIGHT] < 300)
{
printf("discard %i %i\r\n",bg[LEFT],bg[RIGHT]);
bg[LEFT]=drum[LEFT];
bg[RIGHT]=drum[RIGHT];
}
else
{
numPts++; //uncommented below
}
}
if(recording) //recording
{
bg[0]+=drum[0];
bg[1]+=drum[1];
bgSaves[numPts]=drum[LEFT]; //11 to 30 ->0 to 19
bgSaves[numPts+NUM_SAMPLES]=drum[RIGHT]; //11 to 30 -> 20 -> 39
printf("pts: %lu, %lu\r\n", bg[0], bg[1]);
numPts++;
}
//numPts++;
//------------CalcBGNoise----------------------
if(numPts>=NUM_SAMPLES)//actually 20 points here because we ignore first 10 (see above) (2.5 seconds total because thsi is 20Hz function)
{
bg[LEFT]/=numPts; //calculate background noise
bg[RIGHT]/=numPts;
printf("bg is: %lu, %lu\r\n",bg[LEFT],bg[RIGHT]);
state=1;
timePerDrumCheck=5;
hits[LEFT]=0;
hits[RIGHT]=0;
lastRevState=revDown();
bgDone=1;
}
}
//-----------CalcBGNoiseComplete--------------
//-------ReadAnalogDrumHit-------------
else if(state==1)
{
currRevState=revDown();
for(i=0;i<2;i++) //Check if we have any good analog readings
{
if(currRevState!=lastRevState)
{
currHit[i]=0; //change direction, immediately set currHit to 0 so it wont trigger.
}
//if(absDiff(drum[i],bg[i]) > NOISE_THRESHOLD) //noise thresholding
if(absDiff(drum[i],600) > NOISE_THRESHOLD) //noise thresholding HARDCODE
{
//printf("%i-%i\r\n",i,drum[i]);
if(!hitInc)
{
hitInc=1;
classifyStartTime=currTime; //starting classification
}
hits[i]++;
}
}
//-------ReadAnalogDrumHitComplete-----
//----Classify hit---
if( ((currTime>classifyStartTime + CLASSIFY_TIME)||classifyStartTime>currTime) && hitInc) //timer expired
{
hitInc=0; //end classify period
if(hits[0]>=MIN_HITS_PER_CLASSIFY || hits[1] >= MIN_HITS_PER_CLASSIFY)
{ //begin classifying
printf("class ify %i,%i",hits[LEFT],hits[RIGHT]);
if(hits[LEFT] > hits[RIGHT]) //if there are more lefts than rights, good we know what this is.
{
registerHit(LEFT,currTime);
if(hits[RIGHT] > hits[LEFT]/2) //probably hit both at once
{
registerHit(RIGHT,currTime);
}
}
else
{
registerHit(RIGHT,currTime);
if(hits[LEFT] > hits[RIGHT]/2) //probably hit both at once
{
registerHit(LEFT,currTime);
}
}
}
hits[LEFT]=0;
hits[RIGHT]=0;
}
//----Classify Hit END---
}
lastRevState=currRevState;
lastDrumTime=currTime;
return 0;
}
uchar getPhoneReq(void)
{
static uchar ct=0;
static long lastCtTime=0;
static uchar firstDigit=0;
long currTime=ES_Timer_GetTime();
static uchar state=1;
static uchar counting=0;
uchar retVal=NUM_HZVS;
static uchar lastPhoneState=1; //default is HI with pulldown
uchar currPhoneState= phoneContact();
if( !currPhoneState && lastPhoneState==1) //looking for falls
{
ct++; //count the rise (opens)
lastCtTime=currTime;
printf("PHONE TICK\r\n");
}
lastPhoneState= currPhoneState; //set lastPhoneState
if(handsetContact()) //its 5
{
ct=0; //they put the handset down
state=1; //reset state
//printf("handset cont\r\n");
}
if((currTime-lastCtTime > TIME_AFTER_LAST_TICK || lastCtTime>currTime) && ct>0)
{
if(state==1)
{
if(!(ct==1 || ct==10)) //got a number greater than 1 on first shot, return it
{
retVal=ct;
} else //got a 1 or 0, store it and move to next state
{
firstDigit=ct;
state=2; //going to return numHZVs
}
}
else //state is 2nd digit
{
if(firstDigit==10)
firstDigit=0; //10 counts is actually a zero.
if(ct==10)
ct=0;
retVal=10*firstDigit+ct; //save boat number
if(retVal>=NUM_HZVS) //bad boat num
{
//printf("bad boat number %i\r\n", (int) retVal);
ct=0;
state=1;
return NUM_HZVS; //exit early
}
else //good boat num
{
ct=0;
state=1; //will return retVal
}
}
ct=0;
}
/* if(retVal!=16)
printf("retval%i\r\n",retVal);*/
return retVal;
}
//#else
boolean Check4HopperStall(void)
{
// local current variables for comparisons.
int CurrentLeft = GetLeftCount();
int CurrentRight = GetRightCount();
boolean RightIsCurrentlyStalled=False;
boolean LeftIsCurrentlyStalled=False;
int CurrentTime=ES_Timer_GetTime();
// if the left count has changed, note the time.
if (CurrentLeft!=LastLeft)
{
LastTimeLeftChange = ES_Timer_GetTime();
LastLeft = CurrentLeft;
}
// if the right count has changed, note the time.
if (CurrentRight!=LastRight)
{
LastTimeRightChange = ES_Timer_GetTime();
LastRight = CurrentRight;
}
// if the time since the last change in a moter is greater than
// stall time, we'll say that the motor is in a stall state,
// provided that we are actively trying to drive it with a non-zero
// PWM value.
if(((CurrentTime-LastTimeRightChange)>STALL_TIME)&&(GetPWMRightHopper()>10))
{
RightIsCurrentlyStalled = True;
} else
{
RightIsCurrentlyStalled = False;
}
if (((CurrentTime-LastTimeLeftChange)>STALL_TIME)&&(GetPWMRightHopper()>10))
{
LeftIsCurrentlyStalled = True;
//printf(".");
} else
{
LeftIsCurrentlyStalled = False;
}
// if either stall state has changed
if (((RightIsCurrentlyStalled!=RightIsStalled) || (LeftIsCurrentlyStalled!=LeftIsStalled)) )
//&& ((QueryHopperDriveSM()==GOING_TO_HIGH_POSITION_NEITHERHIGH)||
//(QueryHopperDriveSM()==GOING_TO_HIGH_POSITION_RIGHTHIGH)||
//(QueryHopperDriveSM()==GOING_TO_LOW_POSITION_NEITHERLOW)))
{
//printf(".");
// Post the appropriate event.
if ((RightIsCurrentlyStalled==True)&&(LeftIsCurrentlyStalled==True))
{
ES_Event StallEvent;
StallEvent.EventType = SCONTROL_UPDATE;
StallEvent.EventParam = BOTH;
ES_PostList00(StallEvent);
} else if (RightIsCurrentlyStalled==True)
{
ES_Event StallEvent;
StallEvent.EventType = SCONTROL_UPDATE;
StallEvent.EventParam = RIGHT;
ES_PostList00(StallEvent);
} else if (LeftIsCurrentlyStalled==True)
{
ES_Event StallEvent;
StallEvent.EventType = SCONTROL_UPDATE;
StallEvent.EventParam = LEFT;
ES_PostList00(StallEvent);
} else
{
// Don't post a "no longer stalled" event. I think just not posting has the same effect.
}
// Update Variables
LeftIsStalled = LeftIsCurrentlyStalled;
RightIsStalled = RightIsCurrentlyStalled;
// Return true.
return True;
} else
{
// no change to report.
return False;
}
}
void InitializeCheckers( void)
{
LastBallStatus = 0x00;
LastBin1Status = 0x00;
LastBin2Status = 0x00;
LastBin3Status = 0x00;
LastBin4Status = 0x00;
LastWallAngleStatus = 0x00;
LastTimeRightChange = ES_Timer_GetTime();
LastTimeLeftChange = ES_Timer_GetTime();
// Stepper/IR init
LastHoneStatus_Bin1 = False;
LastRStatus_Bin1 = False;
LastLStatus_Bin1 = False;
LastHoneStatus_Bin2 = False;
LastRStatus_Bin2 = False;
LastLStatus_Bin2 = False;
LastHoneStatus_Bin3 = False;
LastRStatus_Bin3 = False;
LastLStatus_Bin3 = False;
LastHoneStatus_Bin4 = False;
LastRStatus_Bin4 = False;
LastLStatus_Bin4 = False;
LastAnyBeacons = False;
LastTapeStatus = False;
Beacon_Bin1_R = False;
Beacon_Bin1_L = False;
Beacon_Bin2_R = False;
Beacon_Bin2_L = False;
Beacon_Bin3_R = False;
Beacon_Bin3_L = False;
Beacon_Bin4_R = False;
Beacon_Bin4_L = False;
LastLeft = 0;
LastRight = 0;
LastXPosition = 0;
LastYPosition = 0;
LastBotOrientation = 0;
LastLeft = GetLeftCount();
LastRight = GetRightCount();
LastTimeRightChange=ES_Timer_GetTime();
LastTimeLeftChange=ES_Timer_GetTime();
LastDelta=LastRight-LastLeft;
RightIsStalled=False;
LeftIsStalled=False;
WallThreatState = False;
LastButtonState = PTP&BIT0HI;
TimeOfLastButtonSample = ES_Timer_GetTime();
}
