/* main.c * * This file contains the intiailization/cleanup code and the state machine for * the control of the robot. Please refer to the inline comments below. */ // Includes #include "constants.h" #include "events.h" #include "motor.h" #include "beacon.h" /*---------------- Function Prototypes ----------------------*/ static void StateMachine(SERVICE_PARAM); static void InitializeModules(void); static void CleanupModules(void); static unsigned char TestCenterBeacon(EVENT_PARAM); static unsigned char TestActiveBeacon(EVENT_PARAM); static unsigned char TestSwitch(void); static unsigned char TestTurretTimerExpired(EVENT_PARAM); /*---------------- Module Code -----------------------------*/ int main (void) { // Initialize all modules InitializeModules(); dprintf("Running code...\n"); /* register each pair of Events and Services routines*/ SES_Register(TestCenterBeacon, StateMachine); SES_Register(TestActiveBeacon, StateMachine); SES_Register(TestTurretTimerExpired, StateMachine); // There's no need to poll for events etc., until the robot // has been activated. Hence we may simply poll for robot // activation (rather than using a separate state). while (!TestSwitch()) { /* Idle... */ } /* enter an infinite loop and handle the events */ while (kTrue) { SES_HandleEvents(); } // Cleanup and exit CleanupModules(); return 0; } /************************************************************************* * InitializeModules * * This code simply initializes the modules which are used by subsequent code. *************************************************************************/ static void InitializeModules(void) { // All necessary modules are initialized here /* Initialize the SES module to operate in round robin mode, with no * timer delay. */ SES_Init(SES_ROUND_ROBIN, SES_NO_UPDATE); TMR_Init(TMR_RATE_4MS); // Initialize timer module AD_Init(); // Initialize the AD module PWM_Init(); // Initialize the PWM module } /************************************************************************* * CleanupModules * * This code cleans up state associated with any active modules. *************************************************************************/ static void CleanupModules(void) { PWM_End(); // Shutdown the PWM module } /************************************************************************* * StateMachine * * This code serves as the core of the robot's behavior. It simulate a DFA * (deterministic finite automata) which responds to certain inputs in a * predictable manner, given its current state. The passed SERVICE_PARAM has * its values set by the event which triggers execution of this routine. * The possible set of events is as follows: * * kEventCenterAdjustment - Change in the alignment between the robot and * the center beacon. * kEventTimerExpired - A timer has expired. * kEventActiveBeaconChanged - The active beacon has changed. * * The set of corresponding current states is: * * kStateFindingCenter - Robot is searching for the center beacon while * spinning in place. * kStateReversing - The robot is backing up, waiting for a timer * to expire and halt the reversal. * kStatePoweringUp - The robot is energizing its motor in hope to * warm the motors and overcome any initial mechanical * friction, etc. * kStateShootBalls - The robot tracks the active beacon and launches * balls to the various baskets. * * The cartesian product of these two sets yields all possible (state, event) * pairs, of which only a subset are covered below. Note that two pieces of * data may be passed from the event handler using the scheme described in events.h. *************************************************************************/ static void StateMachine(SERVICE_PARAM) { static StateType curState = kStateFindingCenter; StateType nextState = curState; unsigned int eventInfo = GET_SHARED_WORD(); unsigned char curEvent = GET_EVENT(eventInfo); Orientation newOrientation; static BeaconCode lastBeacon = kCenterBeacon; BeaconCode newBeacon; unsigned int i, j; switch (curState) { case kStateFindingCenter: /* If our orientation relative to the center has changed, then * react accordingly. If we have found the center, stop rotation and * reverse; otherwise, keep rotating right (we always rotate the robot * right in order to simplify the rotation problem). */ switch (curEvent) { case kEventCenterAdjustment: newOrientation = GET_PARAM(eventInfo); if (newOrientation == kOrientationCentered) { dprintf("Hehe\n"); RotateRobot(kDirectionNone); nextState = kStateReversing; ReverseRobot(kReverseTime); } else { // Keep rotating right... RotateRobot(kDirectionRight); } break; default: dprintf("Unknown event %d in state \"Finding Center\"!\n", curEvent); break; } break; case kStateReversing: /* If the reversal timer expired, then stop reversal and begin to warm * the pitching motor. */ switch (curEvent) { case kEventTimerExpired: // Stop moving and rotate to center RotateRobot(kDirectionNone); nextState = kStatePoweringUp; PWM_SetPeriod(kPWMPeriod); // Set the period // Enable pitching wheel PWM_SetSpeed(kPitchingRunCenter, kPitchingSpeedChan); TMR_StopTimer(kGeneralTimer); TMR_InitTimer(kGeneralTimer, kPitchingPowerUpTime); break; default: dprintf("Unknown event in state \"Reversing\"!\n"); break; } break; case kStatePoweringUp: /* Once the warmup period timer expires, begin turret rotation and begin to * shoot balls at the baskets. Note that we rotate the turret from the right * to the center here, which assumes a mechanical starting position for the * turret. This was initially done to avoid exerting pressure against the * center solenoids until they were absolutely needed. During this rotation, we * simply block further execution (we have no desire to consider any other * events). We then start the loader mechanism, which runs continuously here * on; note we run it at maximum speed for a short period to overcome initial * friction before reverting to the desired speed. */ switch (curEvent) { case kEventTimerExpired: RotateTurret(kCenterBeacon, kRightBeacon); BlockingCode(kTurretBootstrap); TMR_StopTimer(kGeneralTimer); // Set by RotateTurret TMR_ClearTimerExpired(kGeneralTimer); PWM_SetSpeed(kColumnMaxSpeed, kColumnSpeedChan); /* Allow the column to power up */ BlockingCode(kColumnPowerUpTime); PWM_SetSpeed(kColumnRunMotor, kColumnSpeedChan); nextState = kStateShootBalls; break; default: dprintf("Uknown event in state \"Powering up\"!\n"); break; } break; case kStateShootBalls: /* Here we rotate the turret in response to a change in the active beacon. * Note that the turret rotation motor is shutoff in response to a timer * event that is initiated in the RotateTurret routine. */ switch (curEvent) { case kEventActiveBeaconChanged: // Invoke a rotation newBeacon = GET_PARAM(eventInfo); RotateTurret(newBeacon, lastBeacon); lastBeacon = newBeacon; break; case kEventTimerExpired: // Stop running the turret rotation motor RotateTurret(lastBeacon, lastBeacon); break; default: dprintf("Unknown event in state \"Shooting Balls\"\n"); break; } break; default: dprintf("Invalid state 0x%x!\n", curState); break; } // Update current state curState = nextState; } /************************************************************************* * TestCenterBeacon * * This code assumes an initially unknown orientation such that the first * invokation of the test will trigger an event and force the robot to begin * rotating in search of the center. An event is triggered only if the * orientation status changes. ************************************************************************/ static unsigned char TestCenterBeacon(EVENT_PARAM) { static Orientation lastOrientation = kOrientationUnknown; Orientation curOrientation = FindCenter(); unsigned int eventInfo = 0; if (curOrientation != lastOrientation) { SET_EVENT(eventInfo, kEventCenterAdjustment); SET_PARAM(eventInfo, curOrientation); SET_SHARED_WORD_TO(eventInfo); lastOrientation = curOrientation; return kTrue; } return kFalse; } /************************************************************************* * TestTurretTimerExpired * * This flags an event whenever it is detected that the timer has expired. ************************************************************************/ static unsigned char TestTurretTimerExpired(EVENT_PARAM) { unsigned int eventInfo = 0; if (TMR_IsTimerExpired(kGeneralTimer)) { SET_EVENT(eventInfo, kEventTimerExpired); SET_SHARED_WORD_TO(eventInfo); TMR_ClearTimerExpired(kGeneralTimer); return kTrue; } return kFalse; } /************************************************************************* * TestActiveBeacon * * An event is recorded whenever the active beacon changes. The previously * active beacon is cached in a static variable such that only changes * will fire an event (and result in a turret rotation). ************************************************************************/ static unsigned char TestActiveBeacon(EVENT_PARAM) { static BeaconCode lastBeacon = kCenterBeacon; BeaconCode currentBeacon = FindActiveBeacon(lastBeacon); unsigned int eventInfo = 0; if (currentBeacon != lastBeacon) { SET_EVENT(eventInfo, kEventActiveBeaconChanged); SET_PARAM(eventInfo, currentBeacon); SET_SHARED_WORD_TO(eventInfo); lastBeacon = currentBeacon; return kTrue; } return kFalse; } /************************************************************************* * TestSwitch * * This routine simply checks the A/D input for the switch; when it goes * high, it must be the case that the switch has been closed. ************************************************************************/ static unsigned char TestSwitch(void) { int switchLevel = AD_Read(kSwitchInput); if (switchLevel > (kMaxControlLevel / 2)) return kTrue; return kFalse; }