/*! \fn void task0(void)
\brief Toggle LED #0
*/
void task0(void)
{
LED_blink(LED0);
if (addTraceProtected("thread0 flips LED") != TRACE_OK)
{
stop_cpu;
}
}
int main (int argc, char** argv)
{
int i;
wiringPiSetup () ;
init_LED(LED1_PIN);
init_LED(LED2_PIN);
init_LED(LED3_PIN);
init_LED(LED4_PIN);
init_LED(LED5_PIN);
init_LED(LED6_PIN);
init_button(BUTTON_PIN);
init_beep(BEEP_PIN);
if(strcmp(argv[1], "LED")==0)
{
int led = atoi(argv[2]);
int on = atoi(argv[3]);
int off= atoi(argv[4]);
switch(led)
{
case 1:
LED_blink(LED1_PIN, on, off);
break;
case 2:
LED_blink(LED2_PIN, on, off);
break;
case 3:
LED_blink(LED3_PIN, on, off);
break;
case 4:
LED_blink(LED4_PIN, on, off);
break;
case 5:
LED_blink(LED5_PIN, on, off);
break;
case 6:
LED_blink(LED6_PIN, on, off);
break;
}
}
else if(strcmp(argv[1], "BEEP")==0)
{
int freq = atoi(argv[2]);
Beep_Tone(BEEP_PIN, freq);
}
return 0 ;
}
void stagnation_recovery(double distance_sensors_value[8], int DIST_THRESHOLD)
{
if (align_counter < 2) // Align
{
printf("Realigning\n");
align_counter = align_counter + 1;
realign(distance_sensors_value);
}
else if(align_counter > 0)// Reposition
{
printf("Finding new spot\n");
LED_blink();
find_new_spot(distance_sensors_value, DIST_THRESHOLD);
}
}
int stagnation_recovery(double distance_sensors_value[8], int DIST_THRESHOLD)
{
int stagnation = get_stagnation_state();
if(stagnation){
if (align_counter < 2) // Align
{
align_counter = align_counter + 1;
realign(distance_sensors_value);
}
else if(align_counter > 0)// Reposition
{
LED_blink();
find_new_spot(distance_sensors_value, DIST_THRESHOLD);
}
}
return stagnation;
}
void
loop( void )
{
bool gpsDateTimeValid;
bool gpsTransitDataValid;
bool obdSpeedKphValid;
/* Update GPS & OBD data drivers */
GPS_update();
OBD_update();
/* Wait for valid date & time */
if ( millis() - g_timer_sample_count_ms >= g_timer_sample_rate_ms ) {
/* Blink status LED for 100ms */
LED_blink( LED_ID_STATUS, 100 );
/* Get GPS date & time */
gpsDateTimeValid = GPS_getUtcDateTime( &g_sampledDateTime );
/* Increase sample rate if GPS date and time is locked */
g_timer_sample_rate_ms = ( gpsDateTimeValid )
? TIMER_GPS_LOCKED_SAMPLE_RATE_MS
: TIMER_GPS_NOT_LOCKED_SAMPLE_RATE_MS;
/* Get GPS transit data */
gpsTransitDataValid = GPS_getTransitData( &g_sampledTransitData );
/* Get OBD vehicle speed KPH */
obdSpeedKphValid = OBD_getVehicleSpeed( &g_sampledVehicleSpeedKph );
/* Output sampled data to serial port */
Serial_printf(
/* 0 1 2 3 4 5 6 7 8 9 A B C D E */
"%lu,%u,%04u-%02u-%02uT%02u:%02u:%02uZ00:00,%u,%f,%f,%f,%f,%u,%f\n",
GPS_getNumOfSatellites(), /* 0 */
gpsDateTimeValid, /* 1 */
g_sampledDateTime.year, /* 2 */
g_sampledDateTime.month, /* 3 */
g_sampledDateTime.day, /* 4 */
g_sampledDateTime.hour, /* 5 */
g_sampledDateTime.minute, /* 6 */
g_sampledDateTime.second, /* 7 */
gpsTransitDataValid, /* 8 */
g_sampledTransitData.lat, /* 9 */
g_sampledTransitData.lng, /* A */
g_sampledTransitData.speedMph, /* B */
g_sampledTransitData.course, /* C */
obdSpeedKphValid, /* D */
g_sampledVehicleSpeedKph /* E */
);
g_timer_sample_count_ms = millis();
}
/* Reset the WDT */
WDT_reset();
}
