Scribd Logo
Upload

Welcome to Scribd!

  • Aligningtobeacon

    Uploaded by

    api-398062839
    123 views6 pages

    Original Title

    aligningtobeacon

    Copyright

    © © All Rights Reserved

    Available Formats

    PDF, TXT or read online from Scribd

    Did you find this document useful?

    Is this content inappropriate?

    Report this Document

    Copyright:

    © All Rights Reserved
    Download as PDF, TXT or read online from Scribd
    Flag for inappropriate content
    123 views6 pages

    Aligningtobeacon

    Uploaded by

    api-398062839

    Copyright:

    © All Rights Reserved
    Download as PDF, TXT or read online from Scribd
    Flag for inappropriate content
    of 6

    /****************************************************************************

    Module
    AligningToBeacon.c

    ****************************************************************************/
    /*----------------------------- Include Files -----------------------------*/
    // Basic includes for a program using the Events and Services Framework
    #include "ES_Configure.h"
    #include "ES_Framework.h"

    /* 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 "AligningToBeacon.h"
    #include "BeaconIRInputCaptureModule.h"
    #include "MotorDrive.h"
    #include "MasterSM.h"
    /*----------------------------- Module Defines ----------------------------*/
    // define constants for the states for this machine
    // and any other local defines

    #define ENTRY_STATE STOPPING


    #define COMPENSATE_TIME_MS (200)
    #define COMPENSATE_SPEED (30)
    #define COMPENSATE_ANGLE (2)
    #define ALIGNING_SPEED (23)
    #define STOPPING_TIME (300)

    /*---------------------------- Module Functions ---------------------------*/


    /* prototypes for private functions for this machine, things like during
    functions, entry & exit functions.They should be functions relevant to the
    behavior of this state machine
    */
    static ES_Event_t DuringStopping(ES_Event_t Event);
    static ES_Event_t DuringRotating(ES_Event_t Event);
    static ES_Event_t DuringCompensating(ES_Event_t Event);

    /*---------------------------- Module Variables ---------------------------*/


    // everybody needs a state variable, you may need others as well
    static AligningToBeaconState_t CurrentState;

    static uint32_t PeriodArray[2][4] = { { 600, 700, 500, 800 }, { 500,


    800, 600, 700 } };

    static uint8_t Direction = 0;

    /*------------------------------ Module Code ------------------------------*/


    /****************************************************************************
    Function
    RunTemplateSM

    Parameters
    ES_Event_t: the event to process

    Returns
    ES_Event_t: an event to return

    Description
    add your description here
    Notes
    uses nested switch/case to implement the machine.
    Author
    J. Edward Carryer, 2/11/05, 10:45AM
    ****************************************************************************/
    ES_Event_t RunAligningToBeacon(ES_Event_t CurrentEvent)
    {
    bool MakeTransition = false;/* are we making a state
    transition? */
    AligningToBeaconState_t NextState = CurrentState;
    ES_Event_t EntryEventKind = { ES_ENTRY, 0 }; // default to normal
    entry to new state
    ES_Event_t ReturnEvent = CurrentEvent; // assume we are not
    consuming event

    switch (CurrentState)
    {
    case STOPPING:
    {
    ReturnEvent = CurrentEvent = DuringStopping(CurrentEvent);
    //process any events
    if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
    {
    if ((CurrentEvent.EventType == ES_TIMEOUT) && (CurrentEvent.EventParam ==
    MOVEMENT_TIMER))
    {
    NextState = ROTATING;
    MakeTransition = true;
    ReturnEvent.EventType = ES_NO_EVENT;
    }
    }
    }
    break;
    case ROTATING: // If current state is state one
    { // Execute During function for state one. ES_ENTRY & ES_EXIT are
    // processed here allow the lower level state machines to re-map
    // or consume the event
    ReturnEvent = CurrentEvent = DuringRotating(CurrentEvent);
    //process any events
    if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
    {
    switch (CurrentEvent.EventType)
    {
    case FOUND_IR: //If we found IR!
    { //ES_Timer_InitTimer(MOVEMENT_TIMER,STOPPING_TIME);
    //StopDrive();
    //diable interrupts for IR
    DisableBeaconIR();
    //break;
    //case ES_TIMEOUT : //
    //if(CurrentEvent.EventParam == MOVEMENT_TIMER)
    //{
    NextState = COMPENSATING;
    MakeTransition = true;

    //tell the state machine above that we're aligned


    ReturnEvent.EventType = ES_NO_EVENT;
    //don't worry about making a transition, as the state machine above
    should stop aligning.
    // }
    }
    break;
    }
    }
    }
    break;

    case COMPENSATING: // If current state is state one


    { // Execute During function for state one. ES_ENTRY & ES_EXIT are
    // processed here allow the lower level state machines to re-map
    // or consume the event
    ReturnEvent = CurrentEvent = DuringCompensating(CurrentEvent);
    //process any events
    if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
    {
    if (CurrentEvent.EventType == ENCODER_LIMIT_REACHED)
    {
    //stop drive
    StopDrive();
    //tell the state machine above that we're aligned
    ReturnEvent.EventType = ALIGNED_TO_BEACON;
    //don't worry about making a transition, as the state machine above should
    stop aligning.
    }
    }
    }
    break;
    }
    //If we are making a state transition
    //We shouldn't enter this ever for this state machine. I'm keeping it for code
    consistency.
    if (MakeTransition == true)
    {
    // Execute exit function for current state
    CurrentEvent.EventType = ES_EXIT;
    RunAligningToBeacon(CurrentEvent);

    CurrentState = NextState; //Modify state variable

    // Execute entry function for new state


    // this defaults to ES_ENTRY
    RunAligningToBeacon(EntryEventKind);
    }
    return ReturnEvent;
    }

    /****************************************************************************
    Function
    StartTemplateSM

    Parameters
    None

    Returns
    None

    Description
    Does any required initialization for this state machine
    Notes

    Author
    J. Edward Carryer, 2/18/99, 10:38AM
    ****************************************************************************/
    void StartAligningToBeacon(ES_Event_t CurrentEvent)
    {
    if (CurrentEvent.EventType != ES_ENTRY)
    {
    printf("WARNING: the alignign to beacon state machine was started by a
    non-ES_ENTRY event.\r\n");
    }
    printf("In \"StartAligningTobeacon\" we are looking for a beacon with period of
    %d\r\n", CurrentEvent.EventParam);
    // to implement entry to a history state or directly to a substate
    // you can modify the initialization of the CurrentState variable
    // otherwise just start in the entry state every time the state machine
    // is started
    if (ES_ENTRY_HISTORY != CurrentEvent.EventType)
    {
    CurrentState = ENTRY_STATE;
    }
    // call the entry function (if any) for the ENTRY_STATE
    RunAligningToBeacon(CurrentEvent);
    }

    /****************************************************************************
    Function
    QueryTemplateSM

    Parameters
    None

    Returns
    TemplateState_t The current state of the Template state machine

    Description
    returns the current state of the Template state machine
    Notes

    Author
    J. Edward Carryer, 2/11/05, 10:38AM
    ****************************************************************************/
    AligningToBeaconState_t QueryAligningToBeacon(void)
    {
    return CurrentState;
    }

    /***************************************************************************
    private functions
    ***************************************************************************/
    static ES_Event_t DuringStopping(ES_Event_t Event)
    {
    ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events


    if ((Event.EventType == ES_ENTRY) ||
    (Event.EventType == ES_ENTRY_HISTORY))
    {
    StopDrive();
    ES_Timer_InitTimer(MOVEMENT_TIMER, STOPPING_TIME);
    EnableBeaconIR(PeriodArray[QueryTeamBit()][Event.EventParam]);
    }
    else if (Event.EventType == ES_EXIT)
    {
    ES_Timer_StopTimer(MOVEMENT_TIMER);
    }
    else
    // do the 'during' function for this state
    {
    //we don't need to do anything here.
    }
    return ReturnEvent;
    }

    static ES_Event_t DuringRotating(ES_Event_t Event)


    {
    ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption
    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ((Event.EventType == ES_ENTRY) ||
    (Event.EventType == ES_ENTRY_HISTORY))
    {
    //TODO, make sure drive library uses PID to make sure it turns wells
    if (Direction)
    {
    DriveCWStraight(ALIGNING_SPEED, false, 0);
    }
    else
    {
    DriveCCWStraight(ALIGNING_SPEED, false, 0);
    }
    //Enable IR Beacon Interrupt with period coming from ES_ENTRY event
    //EnableBeaconIR(PeriodArray[QueryTeamBit()][Event.EventParam]);
    printf("In \"DuringRotating\" we initialized the beacon ir interrupt to look for
    a period of %d\r\n", Event.EventParam);
    }
    else if (Event.EventType == ES_EXIT)
    {
    printf("EXIT EVENT INTO DuringRotating\r\n");
    //stop drive
    StopDrive();
    //diable interrupts for IR
    DisableBeaconIR();
    //tell the state machine above that we're aligned
    }
    else
    // do the 'during' function for this state
    {
    //we don't need to do anything here.
    }
    return ReturnEvent;
    }

    static ES_Event_t DuringCompensating(ES_Event_t Event)


    {
    ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events


    if ((Event.EventType == ES_ENTRY) ||
    (Event.EventType == ES_ENTRY_HISTORY))
    {
    //TODO, make sure drive library uses PID to make sure it turns wells
    if (Direction)
    {
    DriveCCWStraight(COMPENSATE_SPEED, true, COMPENSATE_ANGLE);
    }
    else
    {
    DriveCWStraight(COMPENSATE_SPEED, true, COMPENSATE_ANGLE);
    }
    }
    else if (Event.EventType == ES_EXIT)
    {
    StopDrive();
    }
    else
    // do the 'during' function for this state
    {
    //we don't need to do anything here.
    }
    return ReturnEvent;
    }
    // CCW = 0, CW = 1
    void SetAligningDirection(uint8_t Dir)
    {
    Direction = Dir;
    }

    You might also like