char main(){
int i;
SERIAL_Init();
INTEnableSystemMultiVectoredInt();
Drive_Init();
dbprintf("\nHello World");
AD_Init(BAT_VOLTAGE);
LED_Init(LED_BANK3);
LED_OffBank(LED_BANK3, 0xf);
wait();
int pwm;
while(1) {
//Drive_Stop();
wait();
//Drive_Forward(MIN_SPEED);
printf("\nFORWARD! speed=%u pwm=%u", motorSpeed[A], motorPWMValue[A]);
//for (i = 0; i < 100; i++)
// wait();
Drive_Stop();
Drive_Pivot(left, 1);
for (i = 0; i < 1; i++)
wait();
Drive_Stop();
Drive_Pivot(right, 1);
for (i = 0; i < 1; i++)
wait();
Drive_Stop();
for (i = 0; i < 1; i++)
wait();
}
}
void EXTI9_5_IRQHandler(void)
{
if(EXTI_GetITStatus(EXTI_Line6) != RESET)
{
EXTI_ClearITPendingBit(EXTI_Line6);
EXTI->IMR &= ~EXTI_Line6;
Drive_Stop(STOP_HI_SENSOR,FROM_ISR);
}
if(EXTI_GetITStatus(EXTI_Line7) != RESET)
{
EXTI_ClearITPendingBit(EXTI_Line7);
EXTI->IMR &= ~EXTI_Line7;
Drive_Stop(STOP_LO_SENSOR,FROM_ISR);
}
}
char Drive_Init(void) {
// Set motor direction outputs
MOTOR_A_DIR_TRIS = 0;
MOTOR_B_DIR_TRIS = 0;
Drive_Stop();
InitTimer(TIMER_DRIVE,UPDATE_DELAY);
return SUCCESS;
}
void ExtEventsHandler( void *pvParameters )
{
uint16_t detection_error_delay_counter=0;
vTaskDelay(DETECTION_ERROR_DELAY);
task_watches[EXT_EVENTS_TASK].task_status=TASK_ACTIVE;
while(1)
{
// if(detection_error_delay_counter<DETECTION_ERROR_DELAY)
// {
// detection_error_delay_counter++;
// }
// else
// {
if(GPIO_ReadInputDataBit(DRIVE_EXT_EVENTS_PORT,DRIVE_ERROR)==Bit_RESET)
{
Drive_Stop(STOP_INVERTOR_ERROR,FROM_TASK);
}
else
{
EXTI->IMR |= EXTI_Line0;
}
// }
if(GPIO_ReadInputDataBit(DRIVE_EXT_EVENTS_PORT,DRIVE_LIMIT_UP)==Bit_RESET)
{
drv.limitation_flag=DRIVE_LIMITATION_ONLY_DOWN;
}
else
{
if(GPIO_ReadInputDataBit(DRIVE_EXT_EVENTS_PORT,DRIVE_LIMIT_DOWN)==Bit_RESET)
{
drv.limitation_flag=DRIVE_LIMITATION_ONLY_UP;
}
else
{
drv.limitation_flag=DRIVE_LIMITATION_NONE;
EXTI->IMR |= (EXTI_Line6|EXTI_Line7);
}
}
task_watches[EXT_EVENTS_TASK].counter++;
vTaskDelay(100);
}
}
/**
* @Function RunRamSubHSM(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 RunTapeSubHSM(ES_Event ThisEvent)
{
uint8_t makeTransition = FALSE; // use to flag transition
TapeState_t nextState;
EventStorage args;
ES_Tattle(); // trace call stack
switch (CurrentState) {
case Tape_Init:
if (ThisEvent.EventType == ES_INIT)
{
// Make sure timer isnt going to be running
ES_Timer_StopTimer(TAPE_SUB_HSM_TIMER);
// now put the machine into the actual initial state
CurrentState = Ram_Begin;
makeTransition = FALSE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case Tape_Searching:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Drive forward
Drive_Forward(MOTOR_SPEED_CRAWL);
// Set up frustration timer
ES_Timer_InitTimer(TAPE_SUB_HSM_TIMER, TAPE_FRUSTRATION_TIMER);
break;
case ES_EXIT:
// Stop
Drive_Stop();
// Stop timer
ES_Timer_StopTimer(TAPE_SUB_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == TAPE_SUB_HSM_TIMER) {
nextState = Tape_Reversing;
makeTransition = TRUE;
// Consume
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case TAPE:
args.val = ThisEvent.EventParam;
// Cancel timer for now, tape has been seen
ES_Timer_StopTimer(TAPE_SUB_HSM_TIMER);
if (args.bits.event & args.bits.type & TAPE_CENTER) {
// If center connects, begin to follow the tape
nextState = Tape_Follow_Front;
makeTransition = TRUE;
} else if ((args.bits.event & args.bits.type & TAPE_FRONT_LEFT) &&
(~args.bits.type & TAPE_FRONT_RIGHT)) {
// If front left entered tape and right is off tape
Drive_Left(MOTOR_SPEED_CRAWL);
} else if ((args.bits.event & args.bits.type & TAPE_FRONT_RIGHT) &&
(~args.bits.type & TAPE_FRONT_LEFT)) {
// If front right entered tape and left is off tape
Drive_Right(MOTOR_SPEED_CRAWL);
} else if (~args.bits.type & (TAPE_FRONT_RIGHT | TAPE_FRONT_LEFT)) {
// If front right and left are both off tape
Drive_Forward(MOTOR_SPEED_CRAWL);
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Tape_Reversing:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Start reversing
Drive_Forward(-MOTOR_SPEED_CRAWL);
// Set timer
ES_Timer_InitTimer(TAPE_SUB_HSM_TIMER, 1000);
break;
case ES_EXIT:
// Stop
Drive_Stop();
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == TAPE_SUB_HSM_TIMER) {
nextState = Tape_Turning;
makeTransition = TRUE;
// Consume
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Tape_Turning:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Start turning CW 45
Drive_TankRight(MOTOR_SPEED_EXPLORE);
// Set timer
ES_Timer_InitTimer(TAPE_SUB_HSM_TIMER, MOTOR_TURN_EX_45);
break;
case ES_EXIT:
// Stop
Drive_Stop();
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == TAPE_SUB_HSM_TIMER) {
nextState = Tape_Searching;
makeTransition = TRUE;
// Consume
ThisEvent.EventType = ES_NO_EVENT;
}
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Tape_Follow_Front:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Start driving straight
Drive_Forward(MOTOR_SPEED_CRAWL);
break;
case ES_EXIT:
// Do nothing
break;
case TAPE:
args.val = ThisEvent.EventParam;
if (args.bits.event & args.bits.type & TAPE_FRONT_LEFT) {
// If front left on tape
Drive_Left(MOTOR_SPEED_CRAWL);
} else if (args.bits.event & args.bits.type & TAPE_FRONT_RIGHT) {
// If front right on tape
Drive_Right(MOTOR_SPEED_CRAWL);
} else if (args.bits.event & ~args.bits.type &
(TAPE_FRONT_LEFT | TAPE_FRONT_RIGHT)) {
// If front left off tape
Drive_Forward(MOTOR_SPEED_CRAWL);
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Tape_Follow_Back:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
break;
case ES_EXIT:
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Tape_Done_State:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
break;
case ES_EXIT:
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
default: // all unhandled states fall into here
break;
} // end switch on Current State
if (makeTransition == TRUE) { // making a state transition, send EXIT and ENTRY
// recursively call the current state with an exit event
RunTapeSubHSM(EXIT_EVENT);
CurrentState = nextState;
RunTapeSubHSM(ENTRY_EVENT);
}
ES_Tail(); // trace call stack end
return ThisEvent;
}
/**
* @Function RunExitHSM(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 RunExitHSM(ES_Event ThisEvent)
{
uint8_t makeTransition = FALSE; // use to flag transition
ExitState_t nextState;
static uint8_t trackFlag = FALSE; // used to check if track wire has been found
static uint8_t turnedFlag = FALSE; // used to check if we have flipped 180 already
static uint8_t bounceCount = 0; // make sure we dont get stuck in align states
EventStorage args;
ES_Tattle(); // trace call stack
switch (CurrentState) {
case Exit_Init: // If current state is initial Pseudo State
if (ThisEvent.EventType == ES_INIT)// only respond to ES_Init
{
// Handle initialization of modules
// Initialize sub HSMs
// Move to real state
// These are handled in main() instead
nextState = Exit_Drive;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
;
}
break;
case Exit_Drive:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Drive forward
Drive_Straight(MOTOR_SPEED_CRAWL);
// Start timer for checking when we have driven far enough to have left the castle
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_OUTSIDE);
break;
case ES_EXIT:
// Stop the timer while we handle turns etc, it will restart on entry
ES_Timer_StopTimer(EXIT_HSM_TIMER);
Drive_Stop();
break;
case TRACK_FOUND:
trackFlag = TRUE;
// Consume
ThisEvent.EventType = ES_NO_EVENT;
break;
case BUMPER:
args.val = ThisEvent.EventParam;
// if center tripped
if (args.bits.event & args.bits.type & BUMP_CENTER) {
nextState = Exit_Straighten;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
// else if left tripped
else if (args.bits.event & args.bits.type & BUMP_LEFT) {
nextState = Exit_Align_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
// else if right tripped
else if (args.bits.event & args.bits.type & BUMP_RIGHT) {
nextState = Exit_Align_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
// Turn 180
if (turnedFlag) {
nextState = Exit_Turn90;
} else {
nextState = Exit_Turn180;
}
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Exit_Straighten:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Drive straight to recheck bumps
Drive_Straight(MOTOR_SPEED_CRAWL);
++bounceCount;
if (bounceCount == 8) {
nextState = Exit_Backup;
makeTransition = TRUE;
}
// Set the timeout
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_STRAIGHTEN);
break;
case ES_EXIT:
Drive_Stop();
// Stop timer on exit
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case TRACK_FOUND:
trackFlag = TRUE;
// Consume
ThisEvent.EventType = ES_NO_EVENT;
break;
case BUMPER:
args.val = ThisEvent.EventParam;
// if left bumper and NOT right bumper
if ((args.bits.type & BUMP_LEFT) && (~args.bits.type & BUMP_RIGHT)) {
nextState = Exit_Align_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
// else if right bumper and NOT left bumper
else if ((args.bits.type & BUMP_RIGHT) && (~args.bits.type & BUMP_LEFT)) {
nextState = Exit_Align_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
// else if its right AND left or center it will time out and move to backup
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
if (trackFlag) {
nextState = Exit_Done_Pause;
} else {
nextState = Exit_Backup;
}
makeTransition = TRUE;
// Reset our bounce count, it has resolved a collision
bounceCount = 0;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
// need a timeout for when the center cant be pressed
case Exit_Align_Left:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Turn into the wall to align
Drive_Right(-MOTOR_SPEED_MEDIUM);
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_ALIGN);
break;
case ES_EXIT:
// Stop
Drive_Stop();
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
nextState = Exit_Straighten;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
// need a timeout for when the center cant be pressed
case Exit_Align_Right:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Turn into the wall to align
Drive_Left(-MOTOR_SPEED_MEDIUM);
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_ALIGN);
break;
case ES_EXIT:
// Stop
Drive_Stop();
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
nextState = Exit_Straighten;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Exit_Backup:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
Drive_Straight(-MOTOR_SPEED_CRAWL);
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_BACKUP);
break;
case ES_EXIT:
Drive_Stop();
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
// Turn 90
nextState = Exit_Turn90;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Exit_Turn90:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
Drive_TankRight(MOTOR_SPEED_EXPLORE);
ES_Timer_InitTimer(EXIT_HSM_TIMER, MOTOR_TURN_EX_90);
break;
case ES_EXIT:
Drive_Stop();
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
nextState = Exit_Drive;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Exit_Turn180:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
Drive_TankRight(MOTOR_SPEED_EXPLORE);
// Set 180 turn flag true, don't do this state twice
turnedFlag = TRUE;
ES_Timer_InitTimer(EXIT_HSM_TIMER, MOTOR_TURN_EX_180);
break;
case ES_EXIT:
Drive_Stop();
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
nextState = Exit_Drive;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Exit_Done_Pause:
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Reverse
Drive_Straight(-MOTOR_SPEED_MEDIUM);
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_BACKUP);
break;
case ES_EXIT:
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
Drive_Stop();
ES_Timer_InitTimer(STALL_TIMER, STALL_TIME_IN_MS);
ThisEvent.EventType = ES_NO_EVENT;
} else if (ThisEvent.EventParam == STALL_TIMER) {
nextState = Exit_Done_State;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
break;
case Exit_Done_State:
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Turn 180
Drive_TankRight(MOTOR_SPEED_EXPLORE);
ES_Timer_InitTimer(EXIT_HSM_TIMER, MOTOR_TURN_EX_180 - 10);
break;
case ES_EXIT:
// Do nothing
Drive_Stop();
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
Drive_Stop();
ThisEvent.EventType = CHILD_DONE;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
break;
default: // all unhandled states fall into here
break;
} // end switch on Current State
if (makeTransition == TRUE) { // making a state transition, send EXIT and ENTRY
// recursively call the current state with an exit event
RunExitHSM(EXIT_EVENT);
CurrentState = nextState;
RunExitHSM(ENTRY_EVENT);
}
ES_Tail(); // trace call stack end
return ThisEvent;
}
