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;
}
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;
}
/**********************************************************************
* 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();
}
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;
}
