int main(void) {
// Initialize the UART,Timers, and I2C1v
Board_init();
Board_configure(USE_SERIAL | USE_LCD | USE_TIMER);
dbprint("Starting encoders...\n");
I2C_init(I2C_ID, I2C_CLOCK_FREQ);
Encoder_init();
Interface_init();
Timer_new(TIMER_TEST, PRINT_DELAY );
while (!Timer_isExpired(TIMER_TEST)) {
Encoder_runSM();
}
// Not working?
//Encoder_setZeroPitch();
//Encoder_setZeroYaw();
dbprint("Encoders initialized.\n");
DELAY(STARTUP_DELAY)
Timer_new(TIMER_TEST, PRINT_DELAY );
bool clearedCalibrateMessage = TRUE;
Timer_new(TIMER_TEST2,CALIBRATE_HOLD_DELAY);
//Interface_waitLightOnTimer(CALIBRATE_HOLD_DELAY);
Interface_waitLightOnTimer(CALIBRATE_HOLD_DELAY);
LCD_setPosition(0,0);
dbprint("Encoders:\n");
while(1) {
if (Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(1,0);
dbprint(" P=%.1f,\n Y=%.1f\n",Encoder_getPitch(), Encoder_getYaw());
Timer_new(TIMER_TEST, PRINT_DELAY );
}
if (Interface_isOkPressed() && Timer_isExpired(TIMER_TEST2)) {
Encoder_setZeroPitch();
Encoder_setZeroYaw();
LCD_setPosition(3,0);
dbprint("Zeroed encoders.\n");
clearedCalibrateMessage = FALSE;
Interface_waitLightOnTimer(CALIBRATE_HOLD_DELAY);
Interface_readyLightOff();
Timer_new(TIMER_TEST2,CALIBRATE_HOLD_DELAY);
}
if (Timer_isExpired(TIMER_TEST2) && !clearedCalibrateMessage) {
clearedCalibrateMessage = TRUE;
LCD_setPosition(3,0);
dbprint(" ");
Interface_readyLightOn();
}
Encoder_runSM();
Interface_runSM();
}
return (SUCCESS);
}
int main(void)
{
Board_init();
Serial_init();
LCD_init();
Timer_init();
printf("Testing the LCD display.\nYou should see: Hello World!\n");
LCD_writeString("Hello World!\nLine2\nLine3\nLine4\n");
Timer_new(TIMER_TEST,WRITE_DELAY);
while (!Timer_isExpired(TIMER_TEST)) {
asm("nop");
}
printf("Overwriting line 1\n");
LCD_writeString("Testing overtop line1.\n");
Timer_new(TIMER_TEST,WRITE_DELAY);
while (!Timer_isExpired(TIMER_TEST)) {
asm("nop");
}
printf("Clearing Display\n");
LCD_clearDisplay();
Timer_new(TIMER_TEST,WRITE_DELAY);
while (!Timer_isExpired(TIMER_TEST)) {
asm("nop");
}
LCD_writeString("Here.we.go\n");
LCD_writeString("CLEARED!\n");
LCD_writeString("CLEARED2!\n");
LCD_writeString("CLEARED3!\n");
Timer_new(TIMER_TEST,WRITE_DELAY);
while (!Timer_isExpired(TIMER_TEST)) {
asm("nop");
}
LCD_clearDisplay();
LCD_writeString("N: E: \n");
char tmp[10];
LCD_setPosition(0,3);
sprintf(tmp,"%.2f",12.5f);
LCD_writeString(tmp);
LCD_setPosition(0,13);
sprintf(tmp,"%.2f",-0.32f);
LCD_writeString(tmp);
return SUCCESS;
}
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;
}
void Accelerometer_runSM() {
if (Timer_isExpired(TIMER_ACCELEROMETER)) {
updateReadings();
Timer_new(TIMER_ACCELEROMETER, UPDATE_DELAY);
}
}
int main(void) {
//initializations
Board_init();
Board_configure(USE_SERIAL | USE_LCD | USE_TIMER);
Interface_init();
//I2C_init(I2C_ID, I2C_CLOCK_FREQ);
dbprint("Interface online.\n");
DELAY(STARTUP_DELAY);
Timer_new(TIMER_TEST, DEBUG_PRINT_DELAY);
//cycle and check if buttons are pressed, if so, turn light on for 3 seconds
while(1) {
#ifdef DO_STUFF
//check to see which button is pressed
if(Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(0,0);
dbprint("Ok=%d, C=%d, R=%d, S=%x,\n Reset=%x, SS=%x\n",
Interface_isOkPressed(), Interface_isCancelPressed(),
Interface_isRescuePressed(), Interface_isStopPressed(),
Interface_isResetPressed(), Interface_isSetStationPressed());
Timer_new(TIMER_TEST, DEBUG_PRINT_DELAY);
}
Interface_runSM();
#endif
}
return SUCCESS;
}
/**********************************************************************
* Function: doStationKeepSM
* @return None
* @remark Steps into the station keeping state machine.
**********************************************************************/
static void doStationKeepSM() {
switch (subState) {
case STATE_STATIONKEEP_RETURN:
// Driving to the station
if (event.flags.navigationDone) {
subState = STATE_STATIONKEEP_IDLE;
Timer_new(TIMER_STATIONKEEP,STATION_KEEP_DELAY);
DBPRINT("Arrived at station.\n");
}
// TODO obstacle detection
break;
case STATE_STATIONKEEP_IDLE:
// Wait to float away from the station
if (Timer_isExpired(TIMER_STATIONKEEP)) {
// Check if we floated too far away from the station
if (Navigation_getLocalDistance(&nedStation) > STATION_TOLERANCE_MAX) {
startStationKeepSM(); // return to station
return;
}
Timer_new(TIMER_STATIONKEEP, STATION_KEEP_DELAY);
}
break;
}
if (event.flags.navigationError) {
setError((error_t)Navigation_getError());
}
}
/**********************************************************************
* Function: doSetOriginSM
* @return None
* @remark Steps into the set origin state machine.
**********************************************************************/
static void doSetOriginSM() {
// Waiting for origin message from command center
if (event.flags.haveSetOriginMessage) {
GeocentricCoordinate ecefOrigin;
ecefOrigin.x = Mavlink_newMessage.gpsGeocentricData.x;
ecefOrigin.y = Mavlink_newMessage.gpsGeocentricData.y;
ecefOrigin.z = Mavlink_newMessage.gpsGeocentricData.z;
Navigation_setOrigin(&ecefOrigin);
handleAcknowledgement();
haveOrigin = TRUE;
event.flags.setOriginDone = TRUE;
DBPRINT("Set new origin: X=%.1f, Y=%.1f, Z=%.1f\n",
ecefOrigin.x, ecefOrigin.y, ecefOrigin.z);
}
else {
// Resend request if timer expires
if (Timer_isExpired(TIMER_SETORIGIN)) {
// Resend request origin if timed out
if (resendMessageCount >= RESEND_MESSAGE_LIMIT) {
// Sent too many times
setError(ERROR_NO_ORIGIN);
return;
}
else {
DBPRINTF("Resending origin request.\n");
Mavlink_sendRequestOrigin(NO_ACK); // just want message
Timer_new(TIMER_SETORIGIN, RESEND_MESSAGE_DELAY);
resendMessageCount++;
}
} // timer expired
}
}
int main(){
Board_init();
Board_configure(USE_TIMER);
DELAY(10);
DBPRINT("Starting XBee...\n");
if (Xbee_init(XBEE_UART_ID) != SUCCESS) {
DBPRINT("Failed XBee init.\n");
return FAILURE;
}
DBPRINT("XBee initialized.\n");
DELAY(STARTUP_DELAY);
Timer_new(TIMER_TEST, PRINT_DELAY);
unsigned long int sent = 0;
unsigned long int got = 0;
while(1){
Xbee_runSM();
if (Timer_isExpired(TIMER_TEST)) {
//dbprint("XBee: Sent=%ld, Got=%ld\n", ++sent, got);
Mavlink_sendRequestOrigin();
Timer_new(TIMER_TEST, PRINT_DELAY);
}
if (Mavlink_hasNewMessage()) {
++got;
}
}
return SUCCESS;
}
/**********************************************************************
* Function: doHeartbeatMessage
* @return None.
* @remark Occasionally sends a heartbeat message to the ComPAS.
* @author David Goodman
* @date 2013.05.04
**********************************************************************/
static void doHeartbeatMessage() {
if (Timer_isExpired(TIMER_HEARTBEAT)) {
Mavlink_sendHeartbeat();
Timer_new(TIMER_HEARTBEAT, HEARTBEAT_SEND_DELAY);
}
}
int main(void) {
// Initialize the UART,Timers, and I2C1v
Board_init();
Board_configure(USE_SERIAL | USE_LCD | USE_TIMER);
dbprint("Starting encoders...\n");
I2C_init(ENCODER_I2C_ID, I2C_CLOCK_FREQ);
if (Encoder_init() != SUCCESS) {
dbprint("Failed init. encoder\n");
return FAILURE;
}
Timer_new(TIMER_TEST, PRINT_DELAY );
while (!Timer_isExpired(TIMER_TEST)) {
Encoder_runSM();
}
// Not working?
//Encoder_setZeroPitch();
//Encoder_setZeroYaw();
dbprint("Encoders initialized.\n");
DELAY(STARTUP_DELAY);
Timer_new(TIMER_TEST, PRINT_DELAY );
LCD_setPosition(0,0);
dbprint("Encoders:\n");
while(1){
if (Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(1,0);
dbprint(" P=%.1f,\n Y=%.1f\n",Encoder_getPitch(), Encoder_getYaw());
/*dbprint("Encoders:\n P=%d,\n Y=%d\n",
readDevice(SLAVE_PITCH_READ_ADDRESS,SLAVE_PITCH_WRITE_ADDRESS),
readDevice(SLAVE_YAW_READ_ADDRESS,SLAVE_YAW_WRITE_ADDRESS));*/
/*dbprint("Encoders:\n P=%.1f,\n Y=%.1f\n",
readDevice(SLAVE_PITCH_READ_ADDRESS,SLAVE_PITCH_WRITE_ADDRESS) * DEGREE_PER_NUMBER,
readDevice(SLAVE_YAW_READ_ADDRESS,SLAVE_YAW_WRITE_ADDRESS) * DEGREE_PER_NUMBER);*/
Timer_new(TIMER_TEST, PRINT_DELAY );
}
Encoder_runSM();
}
return (SUCCESS);
}
/**********************************************************************
* Function: doBarometerUpdate
* @return None.
* @remark Sends barometer data such as temperature and altitude if
* the timer expired.
* @author David Goodman
* @date 2013.05.04
**********************************************************************/
static void doBarometerUpdate() {
if (Timer_isExpired(TIMER_BAROMETER_SEND)) {
Mavlink_sendBarometerData(Barometer_getTemperature(),
Barometer_getAltitude());
Timer_new(TIMER_BAROMETER_SEND, BAROMETER_SEND_DELAY);
}
}
/**********************************************************************
* Function: Drive_runSM
* @return None.
* @remark Executes a cycle of the drive system state machine.
* @author David Goodman
* @date 2013.03.27
**********************************************************************/
void Drive_runSM() {
switch (state) {
case STATE_IDLE:
// Do nothing
break;
case STATE_DRIVE:
// Just driving, do nothing
break;
case STATE_TRACK:
if (Timer_isExpired(TIMER_DRIVE)) {
updateRudder();
Timer_new(TIMER_DRIVE, TRACK_UPDATE_DELAY);
}
break;
} // switch
}
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: doGpsCorrectionUpdate
* @return None.
* @remark Receives GPS correction data and applies it to the navigation
* module, or turns it off if no new message were received.
* @author David Goodman
* @date 2013.05.05
**********************************************************************/
static void gpsCorrectionUpdate() {
if (event.flags.haveGeocentricErrorMessage) {
GeocentricCoordinate ecefError;
ecefError.x = Mavlink_newMessage.gpsGeocentricData.x;
ecefError.y = Mavlink_newMessage.gpsGeocentricData.y;
ecefError.z = Mavlink_newMessage.gpsGeocentricData.z;
Navigation_setGeocentricError(&ecefError);
if (!Navigation_isUsingErrorCorrection())
DBPRINT("Error corrections enabled.\n");
Navigation_enableErrorCorrection();
Timer_new(TIMER_GPS_CORRECTION_LOST, GPS_CORRECTION_LOST_DELAY);
}
else if (Timer_isExpired(TIMER_GPS_CORRECTION_LOST)) {
// Disable error corrections
Navigation_disableErrorCorrection();
DBPRINT("Error corrections disabled due to telemetry timeout.\n");
}
}
void Thermal_runSM() {
if (Timer_isExpired(TIMER_THERMAL)) {
#ifdef DEBUG
printf("Reading sensor...\n");
#endif
if(count == 0){
readChipTemp();
calculateChipTemp();
}
count++;
if(count >= 16){
count = 0;
}
readPixelValue();
readCPixelValue();
calculateIRTemp();
Timer_new(TIMER_THERMAL,READ_DELAY);
}
}
int main(void) {
//initializations
Board_init();
Serial_init();
Timer_init();
LCD_init();
Interface_init();
printf("INITIALIZATIONS COMPLETE\n");
LCD_writeString("Interface online.\n");
Timer_new(TIMER_TEST, 100);
while (1) {
if (Timer_isExpired(TIMER_TEST)) {
printf("%X -- C:%X F:%X\n",Interface_isOkPressed(), buttonCount.okay, buttonPressed.flag.okay);
Timer_new(TIMER_TEST, 100);
}
Interface_runSM();
}
}
int main(void) {
// Initialize the UART,Timers, and I2C1
//int row = 0;
Board_init();
Timer_init();
Thermal_init();
Serial_init();
/*Timer_new(TIMER_TEST,100);
while(!Timer_isExpired(TIMER_TEST);*/
#ifdef DEBUG_TEST2
printf("Initialized Thermal module.\n");
#endif
uint8_t row = 0, col = 0;
Timer_new(TIMER_TEST,PRINT_DELAY);
while(1) {
if (Timer_isExpired(TIMER_TEST)) {
#ifdef DEBUG_TEST2
printf("Sending thermal data...\n");
#endif
sendSerial32(0xFFFF1234);
for(col = 0; col < TOTAL_PIXEL_COLS; col++) {
for(row = 0; row < TOTAL_PIXEL_ROWS; row++){
uint8_t pixel = row + (col * TOTAL_PIXEL_ROWS);
sendSerialFloat(finalPixelTempF[pixel]);
}
sendSerial32(END_COL_SEQUENCE);
}
#ifdef DEBUG_TEST2
printf("\nFinished sending thermal data.\n");
#endif
Timer_new(TIMER_TEST,PRINT_DELAY);
} // Timer_isExpired
Thermal_runSM();
} // while
}
BOOL isZeroPressed() {
// Start lock press timer if pressed
if (!readZeroButton()) {
if (zeroTimerStarted)
zeroTimerStarted = FALSE;
//printf("Zero released.\n");
zeroPressed = FALSE;
}
else if (!zeroTimerStarted && readZeroButton()) {
Timer_new(TIMER_BUTTONS, BUTTON_DELAY);
zeroTimerStarted = TRUE;
zeroPressed = FALSE;
//printf("Zero timer started.\n");
}
else if (Timer_isExpired(TIMER_BUTTONS)) {
zeroPressed = TRUE;
//printf("Zero on.\n");
}
return zeroPressed;
}
int main(){
Board_init();
Board_configure(USE_SERIAL | USE_TIMER);
//Board_configure(USE_LCD | USE_TIMER);
DELAY(10);
dbprint("Starting XBee...\n");
if (Xbee_init(XBEE_UART_ID) != SUCCESS) {
dbprint("Failed XBee init.\n");
return FAILURE;
}
dbprint("XBee initialized.\n");
DELAY(STARTUP_DELAY);
LCD_setPosition(0,0);
Timer_new(TIMER_TEST, PRINT_DELAY);
unsigned long int sent = 0;
unsigned long int got = 0;
LCD_clearDisplay();
LCD_setPosition(2,0);
dbprint("Received: %ld", got);
while(1){
Xbee_runSM();
if (Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(0,0);
dbprint("Sent XBee msg.\nSent: %ld\n", ++sent);
Mavlink_sendSaveStation(WANT_ACK);
Timer_new(TIMER_TEST, PRINT_DELAY);
}
if (Mavlink_hasNewMessage()) {
LCD_setPosition(2,0);
dbprint("Received: %ld", ++got);
}
}
return SUCCESS;
}
BOOL isLockPressed() {
// Start lock press timer if pressed
if (!readLockButton()) {
if (lockTimerStarted)
lockTimerStarted = FALSE;
//printf("Lock released.\n");
lockPressed = FALSE;
}
else if (!lockTimerStarted && readLockButton()) {
Timer_new(TIMER_BUTTONS, BUTTON_DELAY);
lockTimerStarted = TRUE;
lockPressed = FALSE;
//printf("Lock timer started.\n");
}
else if (Timer_isExpired(TIMER_BUTTONS)) {
lockPressed = TRUE;
//printf("Lock on.\n");
}
return lockPressed;
}
int main(void) {
//initializations
Board_init();
Board_configure(USE_SERIAL | USE_LCD | USE_TIMER);
Interface_init();
DELAY(5);
dbprint("Interface online.\n");
Timer_new(TIMER_TEST, LIGHT_ON_DELAY);
state = 1;
//cycle and check if buttons are pressed, if so, turn light on for 3 seconds
while(1) {
switch (state) {
case READY_LIGHT:
if (Timer_isExpired(TIMER_TEST)) {
Interface_clearAll();
Interface_readyLightOn();
LCD_setPosition(1,0);
dbprint("Ready light. \n");
Timer_new(TIMER_TEST, LIGHT_ON_DELAY);
state++;
}
break;
case READY_LIGHT_TIMER:
if (Timer_isExpired(TIMER_TEST)) {
Interface_clearAll();
Interface_readyLightOnTimer(LIGHT_ON_TIMER_DELAY);
LCD_setPosition(1,0);
dbprint("Ready light timer.\n");
Timer_new(TIMER_TEST, LIGHT_ON_DELAY);
state++;
}
break;
case WAIT_LIGHT:
if (Timer_isExpired(TIMER_TEST)) {
Interface_clearAll();
Interface_waitLightOn();
LCD_setPosition(0,0);
dbprint("Wait light. \n");
Timer_new(TIMER_TEST, LIGHT_ON_DELAY);
state++;
}
break;
case WAIT_LIGHT_TIMER:
if (Timer_isExpired(TIMER_TEST)) {
Interface_clearAll();
Interface_waitLightOnTimer(LIGHT_ON_TIMER_DELAY);
LCD_setPosition(0,0);
dbprint("Wait light timer. \n");
Timer_new(TIMER_TEST, LIGHT_ON_DELAY);
state++;
}
break;
case ERROR_LIGHT:
if (Timer_isExpired(TIMER_TEST)) {
Interface_clearAll();
Interface_errorLightOn();
LCD_setPosition(0,0);
dbprint("Error light. \n");
Timer_new(TIMER_TEST, LIGHT_ON_DELAY);
state++;
}
break;
case ERROR_LIGHT_TIMER:
if (Timer_isExpired(TIMER_TEST)) {
Interface_clearAll();
Interface_errorLightOnTimer(LIGHT_ON_TIMER_DELAY);
LCD_setPosition(0,0);
dbprint("Error light timer. \n");
Timer_new(TIMER_TEST, LIGHT_ON_DELAY);
state++;
}
break;
case FRONT_CALIB_LIGHT:
if (Timer_isExpired(TIMER_TEST)) {
Interface_clearAll();
CALIBRATE_FRONT_LED = ON;
LCD_setPosition(0,0);
dbprint("Front calib. light. \n");
Timer_new(TIMER_TEST, LIGHT_ON_DELAY);
state++;
}
break;
case BACK_CALIB_LIGHT:
if (Timer_isExpired(TIMER_TEST)) {
Interface_clearAll();
CALIBRATE_BACK_LED = ON;
LCD_setPosition(0,0);
dbprint("Back calib. light. \n");
Timer_new(TIMER_TEST, LIGHT_ON_DELAY);
state = 1;
}
break;
} // switch
Interface_runSM();
}
return SUCCESS;
}
/**********************************************************************
* Function: Interface_showMessage
* @param message_t you want to display
* @return None.
* @remark Message will be printed out to the screen
**********************************************************************/
void Interface_showMessage(message_t msgCode){
if ( !Timer_isExpired(TIMER_LCD_HOLD) && !Timer_isActive(TIMER_LCD_HOLD))
showMessage(msgCode);
else
nextMsgCode = msgCode;
}
void delayMillisecond(int ms) {
Timer_new(TIMER_DELAY, ms);
while (!Timer_isExpired(TIMER_DELAY))
asm("nop");
}
int main(void) {
// Initializations
Board_init();
Board_configure(USE_SERIAL | USE_LCD | USE_TIMER);
I2C_init(I2C_ID, I2C_CLOCK_FREQ);
Magnetometer_init();
Interface_init();
DELAY(5);
if (Accelerometer_init() != SUCCESS) {
Interface_errorLightOn();
dbprint("Failed accel. init.\n");
return FAILURE;
}
dbprint("Ready to calibrate.\n");
Interface_readyLightOn();
DELAY(STARTUP_DELAY);
LCD_setPosition(0,0);
dbprint("Please level scope.\n");
Interface_pitchLightsOn();
state = CALIBRATE_PITCH;
Timer_new(TIMER_TEST2, PRINT_DELAY);
bool holdingCalibrate = FALSE;
while (1) {
switch (state) {
case CALIBRATE_PITCH:
if (Accelerometer_isLevel() && !holdingCalibrate) {
holdingCalibrate = TRUE;
Timer_new(TIMER_TEST, CALIBRATE_HOLD_DELAY);
Interface_readyLightOff();
Interface_waitLightOn();
LCD_setPosition(1,0);
dbprint("Hold level...\n");
}
if (Accelerometer_isLevel() && holdingCalibrate && Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(1,0);
dbprint("Pitch done!\n");
Interface_pitchLightsOff();
Interface_readyLightOn();
Interface_waitLightOff();
holdingCalibrate = FALSE;
DELAY(STARTUP_DELAY);
state = CALIBRATE_YAW;
LCD_setPosition(0,0);
dbprint("Turn scope north.\n \n");
Interface_yawLightsOn();
}
else if (!Accelerometer_isLevel() && holdingCalibrate) {
holdingCalibrate = FALSE;
Timer_stop(TIMER_TEST);
LCD_setPosition(1,0);
dbprint(" \n");
Interface_readyLightOn();
Interface_waitLightOff();
}
break;
case CALIBRATE_YAW:
if (Magnetometer_isNorth() && !holdingCalibrate) {
holdingCalibrate = TRUE;
Timer_new(TIMER_TEST, CALIBRATE_HOLD_DELAY);
Interface_readyLightOff();
Interface_waitLightOn();
LCD_setPosition(1,0);
dbprint("Hold north + level..\n");
}
if (Magnetometer_isNorth() && holdingCalibrate && Timer_isExpired(TIMER_TEST)) {
LCD_setPosition(1,0);
dbprint("Yaw done!\n");
Interface_yawLightsOff();
Interface_readyLightOn();
Interface_waitLightOff();
holdingCalibrate = FALSE;
DELAY(STARTUP_DELAY);
return SUCCESS;
}
else if (!Magnetometer_isNorth() && holdingCalibrate) {
holdingCalibrate = FALSE;
Timer_stop(TIMER_TEST);
LCD_setPosition(1,0);
dbprint(" \n");
Interface_readyLightOn();
Interface_waitLightOff();
}
#ifdef DEBUG_MAGNETOMETER
if (Timer_isExpired(TIMER_TEST2)) {
LCD_setPosition(3,0);
dbprint("Mag: %.2f\n",Magnetometer_getHeading());
Timer_new(TIMER_TEST2, PRINT_DELAY);
}
#endif
break;
}
Magnetometer_runSM();
Accelerometer_runSM();
Interface_runSM();
}
return (SUCCESS);
}
/**********************************************************************
* Function: Interface_runSM
* @param None.
* @return None.
* @remark Must be called every cycle of the Command Center, checks the
* timers and reacts.
**********************************************************************/
void Interface_runSM(){
if (Timer_isExpired(TIMER_LIGHT_HOLD)) {
timerLightOffFunction();
Timer_clear(TIMER_LIGHT_HOLD);
}
if (Timer_isExpired(TIMER_LCD_HOLD)) {
Interface_clearDisplay();
if (nextMsgCode != NO_MESSAGE) {
showMessage(nextMsgCode);
nextMsgCode = NO_MESSAGE;
}
Timer_clear(TIMER_LCD_HOLD);
}
//printf("HERE! ");
if(Timer_isExpired(TIMER_INTERFACE)) {
#if NUMBER_OF_TIMES_TO_CHECK > 1
if(OKAY_BUTTON == PRESSED)
buttonCount.okay += 1;
if(CANCEL_BUTTON == PRESSED)
buttonCount.cancel += 1;
if(STOP_BUTTON == PRESSED)
buttonCount.stop += 1;
if(RESCUE_BUTTON == PRESSED)
buttonCount.rescue += 1;
if(SETSTATION_BUTTON == PRESSED)
buttonCount.setStation += 1;
if (RESET_BUTTON == PRESSED)
buttonCount.reset += 1;
buttonReadCount++;
if(buttonReadCount >= NUMBER_OF_TIMES_TO_CHECK){
buttonReadCount = 0;
// Clear button flag
buttonPressed.bytes[0] = false;
if(buttonCount.okay >= MINIMUM_POSITIVES)
buttonPressed.flag.okay = true;
if(buttonCount.cancel >= MINIMUM_POSITIVES)
buttonPressed.flag.cancel = true;
if(buttonCount.stop >= MINIMUM_POSITIVES)
buttonPressed.flag.stop = true;
if(buttonCount.rescue >= MINIMUM_POSITIVES)
buttonPressed.flag.rescue = true;
if(buttonCount.setStation >= MINIMUM_POSITIVES)
buttonPressed.flag.setStation = true;
if (buttonCount.reset >= MINIMUM_POSITIVES)
buttonPressed.flag.reset = TRUE;
// Reset button counts
buttonCount.okay = 0;
buttonCount.cancel = 0;
buttonCount.stop = 0;
buttonCount.rescue = 0;
buttonCount.setStation = 0;
buttonCount.reset = 0;
}
#else
buttonPressed.flag.okay = (OKAY_BUTTON == PRESSED);
buttonPressed.flag.cancel = (CANCEL_BUTTON == PRESSED);
buttonPressed.flag.stop = (STOP_BUTTON == PRESSED);
buttonPressed.flag.rescue = (RESCUE_BUTTON == PRESSED);
buttonPressed.flag.setStation = (SETSTATION_BUTTON == PRESSED);
buttonPressed.flag.reset = (RESET_BUTTON == PRESSED);
#endif
#ifdef DEBUG_BUTTON_CHECK
Interface_waitLightOnTimer(1000);
printf("Ok=%d, C=%d, S=%d, R=%d, SS=%d, Reset=%d\n",
buttonPressed.flag.okay, buttonPressed.flag.cancel,
buttonPressed.flag.stop, buttonPressed.flag.rescue,
buttonPressed.flag.setStation, buttonPressed.flag.reset);
#endif
Timer_new(TIMER_INTERFACE, WAIT_BETWEEN_CHECKS);
}
// Calibration lights
if (usingPitchLights) {
READY_LED = OFF;
if (Accelerometer_isLevel()) {
CALIBRATE_FRONT_LED = ON;
CALIBRATE_BACK_LED = ON;
READY_LED = ON;
}
else if (Accelerometer_getX() > Accelerometer_getY()) {
CALIBRATE_FRONT_LED = OFF;
CALIBRATE_BACK_LED = ON;
}
else if (Accelerometer_getY() > Accelerometer_getX()) {
CALIBRATE_FRONT_LED = ON;
CALIBRATE_BACK_LED = OFF;
}
}
else if (usingYawLights) {
READY_LED = OFF;
#ifdef USE_MAGNETOMETER
if (Magnetometer_isNorth() && Accelerometer_isLevel()) {
#else
if (Accelerometer_isLevel()) {
#endif
CALIBRATE_FRONT_LED = ON;
CALIBRATE_BACK_LED = ON;
READY_LED = ON;
}
else {
CALIBRATE_FRONT_LED = OFF;
CALIBRATE_BACK_LED = OFF;
}
}
if (Timer_isExpired(TIMER_INTERFACE) || !Timer_isActive(TIMER_INTERFACE))
Timer_new(TIMER_INTERFACE, WAIT_BETWEEN_CHECKS);
}
/**********************************************************************
* Function: Interface_isCancelPressed
* @param None.
* @return TRUE if the cancel button was pressed.
* @remark
**********************************************************************/
bool Interface_isCancelPressed(){
return buttonPressed.flag.cancel;
}
/**********************************************************************
* Function: Interface_isOkPressed
* @param None.
* @return TRUE if the ok button was pressed.
* @remark
**********************************************************************/
bool Interface_isOkPressed(){
return buttonPressed.flag.okay;
}
/**********************************************************************
* Function: Interface_isStopPressed
* @param None.
* @return TRUE if the stop button was pressed.
* @remark
**********************************************************************/
bool Interface_isStopPressed(){
return buttonPressed.flag.stop;
}
/**********************************************************************
* Function: Interface_isRescuePressed
* @param None.
* @return TRUE if the rescue button was pressed.
* @remark
**********************************************************************/
bool Interface_isRescuePressed(){
return buttonPressed.flag.rescue;
}
/**********************************************************************
* Function: Interface_isSetStationPressed
* @param None.
* @return TRUE if the set station button was pressed.
* @remark
**********************************************************************/
bool Interface_isSetStationPressed(){
return buttonPressed.flag.setStation;
}
/**********************************************************************
* Function: Interface_isResetPressed
* @param None.
* @return TRUE if the reset button was pressed.
* @remark
**********************************************************************/
bool Interface_isResetPressed(){
return buttonPressed.flag.reset;
}
/**********************************************************************
* Function: Interface_readyLightOn
* @param None.
* @return None.
* @remark Turns the LED On by
**********************************************************************/
void Interface_readyLightOn(){
READY_LED = ON;
}
/**********************************************************************
* Function: Interface_readyLightOff
* @param None.
* @return None.
* @remark Turns the LED off by
**********************************************************************/
void Interface_readyLightOff(){
READY_LED = OFF;
}
/**********************************************************************
* Function: Interface_waitLightOn
* @param None.
* @return None.
* @remark Turns the LED On by
**********************************************************************/
void Interface_waitLightOn(){
WAIT_LED = ON;
}
/**********************************************************************
* Function: Interface_waitLightOff
* @param None.
* @return None.
* @remark Turns the LED off by
**********************************************************************/
void Interface_waitLightOff(){
WAIT_LED = OFF;
}
/**********************************************************************
* Function: Interface_errorLightOn
* @param None.
* @return None.
* @remark Turns the LED On by
**********************************************************************/
void Interface_errorLightOn(){
ERROR_LED = ON;
}
/**********************************************************************
* Function: Interface_errorLightOff
* @param None.
* @return None.
* @remark Turns the LED off by
**********************************************************************/
void Interface_errorLightOff(){
ERROR_LED = OFF;
}
/**********************************************************************
* Function: Interface_readyLightOnTimer
* @param amount of time in ms that you want the light to remain on
* @return None.
* @remark Turns the LED on for a certain amount of time
**********************************************************************/
void Interface_readyLightOnTimer(uint16_t ms){
Timer_new(TIMER_LIGHT_HOLD, ms);
timerLightOffFunction = &Interface_readyLightOff;
Interface_readyLightOn();
}
int main(){
//Initializations
Board_init();
Serial_init();
Timer_init();
Drive_init();
int spd = 10;
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
enum{
MICRO_FORWARD = 0x01, //State where Microcontroller is driving forward
MICRO_STOP = 0x02, //State where Micro is driving backwards
MICRO_LIMBO = 0x03,
RECIEVER_STATE = 0x04, //Reciever has taken over state
}test_state;
printf("Boat is Initialized\n");
//Initialize the state to begin at forward
test_state = MICRO_FORWARD;
Drive_stop();
Timer_new(TIMER_TEST,RECIEVER_DELAY);
int state_flag = 0;
Override_init();
while(1){
switch(test_state){
case MICRO_FORWARD:
printf("STATE: MICRO_FORWARD\n\n\n");
Drive_forward(spd); //The input param is a pwm duty cycle percentage that gets translated to a RC Servo time pulse
Timer_new(TIMER_TEST2,RECIEVER_DELAY);
test_state = MICRO_STOP;
break;
case MICRO_STOP:
if(Timer_isExpired(TIMER_TEST2)){
printf("STATE: MICRO_STOP\n\n\n:");
Drive_stop();
Timer_new(TIMER_TEST2,RECIEVER_DELAY);
test_state = MICRO_LIMBO;
}
break;
case MICRO_LIMBO:
if(Timer_isExpired(TIMER_TEST2)){
printf("STATE: MICRO_LIMBO\n\n\n");
test_state = MICRO_FORWARD;
}
Drive_stop();
break;
case RECIEVER_STATE:
;
break;
}
//If we got a pulse, control to reciever.
if (OVERRIDE_TRIGGERED == TRUE){
printf("Reciever Control\n\n");
Timer_new(TIMER_TEST, 1000); //Set timer that is greater than the pulsewidth of the CH3 signal(54Hz)
OVERRIDE_TRIGGERED = FALSE; //Re-init to zero so that we know when our pulse is triggered again.
test_state = RECIEVER_STATE; //Set state equal to reciever where we do nothing autonomous
ENABLE_OUT_LAT = RECIEVER; //Give control over to Reciever using the enable line
INTEnable(INT_CN,1);
}
if (Timer_isExpired(TIMER_TEST)){ //Reciever gave up control
printf("Micro Has Control\n\n");
Timer_clear(TIMER_TEST); //Clear timer so that it doesn't keep registering an expired signal
test_state = MICRO_LIMBO; //Set state equal to forward for regular function
OVERRIDE_TRIGGERED = FALSE; //Set Override to false to be sure that we don't trigger falsely
ENABLE_OUT_LAT = MICRO; //Give Control back to microcontroller using enable line
Timer_new(TIMER_TEST2, RECIEVER_DELAY);
}
}
}
int main(){
//Initializations
Board_init();
Serial_init();
Timer_init();
Drive_init();
int spd = 10;
ENABLE_TRIS = INPUT; //Set the direction of the Enable pin to be an input, a switch will be used for now
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
enum{
MICRO_FORWARD = 0x01, //State where Microcontroller is driving forward
MICRO_STOP = 0x02, //State where Micro is driving backwards
MICRO_LIMBO = 0x03,
RECIEVER_STATE = 0x04, //Reciever has taken over state
}test_state;
printf("Boat is Initialized\n");
//Initialize the state to begin at forward
test_state = MICRO_FORWARD;
Drive_stop();
Timer_new(TIMER_TEST,RECIEVER_DELAY);
int state_flag = 0;
while(1){
if(Timer_isExpired(TIMER_TEST)){
if ((ENABLE_BIT == 1)){
test_state = RECIEVER_STATE;
printf("YOU HAVE TRIGGERED THE RECIEVER FOR CONTROL\n\n");
ENABLE_OUT_LAT = RECIEVER; //Set the Enable_out pin that will be routed to the AND gates
state_flag = 1;
}else if (ENABLE_BIT == 0){
printf("MICRO HAS CONTROL\n\n");
ENABLE_OUT_LAT = MICRO; //setting Enable_out that will be routed to AND gates
if(state_flag == 1){
Timer_new(TIMER_TEST2,RECIEVER_DELAY);
test_state = MICRO_FORWARD;
state_flag = 0;
}
}
Timer_new(TIMER_TEST,RECIEVER_DELAY);
}
switch(test_state){
case MICRO_FORWARD:
printf("STATE: MICRO_FORWARD\n\n\n");
Drive_forward(spd); //The input param is a pwm duty cycle percentage that gets translated to a RC Servo time pulse
Timer_new(TIMER_TEST2,RECIEVER_DELAY);
test_state = MICRO_STOP;
break;
case MICRO_STOP:
if(Timer_isExpired(TIMER_TEST2)){
printf("STATE: MICRO_STOP\n\n\n:");
Drive_stop();
Timer_new(TIMER_TEST2,RECIEVER_DELAY);
test_state = MICRO_LIMBO;
}
break;
case MICRO_LIMBO:
if(Timer_isExpired(TIMER_TEST2)){
printf("STATE: MICRO_LIMBO\n\n\n");
test_state = MICRO_FORWARD;
}
Drive_stop();
break;
case RECIEVER_STATE:
;
break;
}
}
}
/**********************************************************************
* Function: Override_isTriggered
* @return TRUE or FALSE if the receiver has come online.
* @remark The receiver will only be considered offline (FALSE), if
* it is turned off for longer and 2 seconds.
* @author David Goodman
* @date 2013.04.01
**********************************************************************/
bool Override_isTriggered() {
return !Timer_isExpired(TIMER_OVERRIDE);
}
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;
}
