Example #1
0
int main(){
    //Initializations
    Board_init();
    Serial_init();
    Timer_init();
    if (Drive_init() != SUCCESS) {
        printf("Failed to initialize drive module.\n");
    }
    I2C_init(TILT_COMPASS_I2C_ID, I2C_CLOCK_FREQ);
    if (TiltCompass_init() != SUCCESS) {
        printf("Failed to initialize tilt compass module.\n");
    }
    ENABLE_OUT_TRIS = OUTPUT;

    while (1) {
        printf("Driving north at full speed.\n");
        Timer_new(TIMER_TEST, COMMAND_DELAY);
        Drive_forwardHeading(100, 0);

        while(!Timer_isExpired(TIMER_TEST)) {
            //wait for finish
            DELAY(1);
            Drive_runSM();
            TiltCompass_runSM();
        }
        Drive_stop();
        Drive_runSM();
        TiltCompass_runSM();

        printf("Driving south at full speed.\n");
        Timer_new(TIMER_TEST, COMMAND_DELAY);
        Drive_forwardHeading(100, 180);

        while(!Timer_isExpired(TIMER_TEST)) {
            //wait for finish
            DELAY(1);
            Drive_runSM();
            TiltCompass_runSM();
        }
        
        Drive_stop();
        Drive_runSM();
        TiltCompass_runSM();
        delayMillisecond(COMMAND_DELAY);
        printf("Waiting for retry...\n");

    }

    return SUCCESS;
}
Example #2
0
int main() {
    ENABLE_OUT_TRIS = OUTPUT;            //Set Enable output pin to be an output, fed to the AND gates
    ENABLE_OUT_LAT = MICRO;             //Initialize control to that of Microcontroller
    Board_init();
    Serial_init();
    Timer_init();
    Drive_init();
    I2C_init(I2C_BUS_ID, I2C_CLOCK_FREQ);
    TiltCompass_init();
    printf("Boat initialized.\n");

    Drive_stop();
    DELAY(5);
#ifdef MOTOR_TEST
    Drive_pivot(0);
#endif

#ifdef RUDDER_TEST
   Drive_forwardHeading(0.0, desiredHeading);
#endif
   int i = 0;
   int velocity[] = {1600, 1600, 1600, 1600, 1600};
   velocityPulse = velocity[i];
    Timer_new(TIMER_TEST,FINISH_DELAY);
    while (1) {
        if (Timer_isExpired(TIMER_TEST))
        {
            i++;
            if (i == 5){
                i = 0;
            }
            velocityPulse = velocity[i];
           printf("CURRENT VELOCITY: %d\n\n",velocityPulse);
            Timer_new(TIMER_TEST,FINISH_DELAY);

        }
         
        Drive_runSM();
        TiltCompass_runSM();
    }

    Drive_stop();

    return SUCCESS;
}
Example #3
0
File: Atlas.c Project: ddeo/sdp
/**********************************************************************
 * Function: doMasterSM
 * @return None.
 * @remark Executes one cycle of the boat's master state machine.
 * @author David Goodman
 * @date 2013.03.28 
 **********************************************************************/
static void doMasterSM() {
    checkEvents();

    #ifdef USE_TILTCOMPASS
    TiltCompass_runSM();
    #endif

    #ifdef USE_GPS
    GPS_runSM();
    #endif

    #ifdef USE_NAVIGATION
    Navigation_runSM();
    #ifdef USE_ERROR_CORRECTION
    gpsCorrectionUpdate();
    #endif
    #endif

    #ifdef USE_DRIVE
    Drive_runSM();
    #endif

    #ifdef USE_XBEE
    Xbee_runSM();
    #endif

    #ifdef USE_BAROMETER
    Barometer_runSM();
    doBarometerUpdate(); // send barometer data
    #endif

    switch (state) {
        case STATE_SETSTATION:
            doSetStationSM();

            if (event.flags.haveStartRescueMessage) {
                startRescueSM();
            }
            else if (event.flags.setStationDone) {
                if (haveOrigin)
                    startStationKeepSM();
                else
                    startOverrideSM();   
            }
            
            break;
        case STATE_SETORIGIN:
            doSetOriginSM();
            
            if (event.flags.setOriginDone)
                startOverrideSM();   
            
            break;
        case STATE_STATIONKEEP:
            doStationKeepSM();

            if (event.flags.haveStartRescueMessage)
                startRescueSM();
            else if (!haveStation)
                setError(ERROR_NO_STATION);
            break;

        case STATE_OVERRIDE:
            if (!wantOverride) {
                    //setError(ERROR_NO_ORIGIN);
                if (!haveOrigin)
                    startSetOriginSM(); // do we ant infinite startup loop?
                else if (event.flags.haveStartRescueMessage)
                    startRescueSM();
                else if (event.flags.haveSetStationMessage )
                    startSetStationSM();
                else if (haveOrigin && haveStation)
                    startStationKeepSM();
                
                // Use autonomous controls
                if (haveOrigin && (haveStation
                    || event.flags.haveStartRescueMessage)) {
                    Override_giveMicroControl();
                    DBPRINT("Micro has control.\n");
                    #ifdef USE_SIREN
                    Siren_blueLightOff();
                    #endif
                }
            }

            break;

        case STATE_RESCUE:
            doRescueSM();

            
            if (event.flags.haveStartRescueMessage) {
                startRescueSM();
            }
            else if (event.flags.haveReturnStationMessage) {
                if (haveStation)
                    startStationKeepSM();
                else 
                    setError(ERROR_NO_STATION);
            }
            // Turn off rescue siren (red)
            if (event.flags.haveError || state != STATE_RESCUE) {
                #ifdef USE_SIREN
                Siren_redLightOff();
                #endif
            }

            break;
    }
    //  ------- Caught by most states -----------
    if (state != STATE_RESCUE) {
        if (event.flags.haveSetStationMessage)
            startSetStationSM();
    }
    if (state != STATE_OVERRIDE) {
        if (event.flags.haveError) {
            startOverrideSM();
            overrideShutdown = TRUE;
        }
        if (wantOverride)
            startOverrideSM();
    }
    if (event.flags.haveResetMessage)
        resetAtlas();
}
Example #4
0
int main() {
    enum{
        forward =  0x01,
        backward = 0x02,
        idle    =  0x03,
    }drive_state;

    Board_init();
    Serial_init();
    Timer_init();
    Drive_init();
    int spd = 20;
    //Magnetometer_init();
    printf("Boat initialized.\n");
    Drive_forward(spd);
    Timer_new(TIMER_TEST,TEST_DELAY);
    drive_state = forward;
    int flag = 0;
    while (1) {
        switch(drive_state){
            case forward:
                Drive_forward(spd);
                if(Timer_isExpired(TIMER_TEST)){
                    drive_state = idle;
                    Drive_stop();
                    Timer_new(TIMER_TEST,TEST_DELAY);
                    flag = 0;
                }
                break;
            case idle:
                Drive_stop();
                if(Timer_isExpired(TIMER_TEST)){
                    if (flag == 0){
                        drive_state = backward;
                        Drive_backward(spd);
                    }else if (flag == 1){
                        drive_state = forward;
                        Drive_forward(spd);
                    }
                    Timer_new(TIMER_TEST,TEST_DELAY);
                }

                break;
            case backward:
                Drive_backward(spd);
                if(Timer_isExpired(TIMER_TEST)){
                    drive_state = idle;
                    Drive_stop();
                    Timer_new(TIMER_TEST,TEST_DELAY);
                    flag = 1;
                }
        }

            Drive_runSM();

    }

    Drive_stop();
    Drive_runSM();

    return SUCCESS;
}