Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Diff: main.cpp
- Revision:
- 22:dc54ca6e6eec
- Parent:
- 20:1c2067937065
diff -r 894ba6f8d67b -r dc54ca6e6eec main.cpp --- a/main.cpp Thu Nov 29 18:34:22 2018 +0000 +++ b/main.cpp Thu Nov 29 19:45:49 2018 +0000 @@ -1,770 +0,0 @@ -/** Code for "Data Bus" UGV entry for Sparkfun AVC 2014 - * http://www.bot-thoughts.com/ - */ - -/////////////////////////////////////////////////////////////////////////////////////////////////////// -// INCLUDES -/////////////////////////////////////////////////////////////////////////////////////////////////////// - -#include "mbed.h" -#include <math.h> -#include <stdint.h> -#include "devices.h" -#include "globals.h" -#include "Config.h" -#include "Buttons.h" -#include "Display.h" -#include "Menu.h" -#include "GPSStatus.h" -#include "logging.h" -#include "Telemetry.h" -#include "SystemState.h" -#include "shell.h" -#include "Steering.h" -#include "Sensors.h" -#include "kalman.h" -#include "Ublox6.h" -#include "PinDetect.h" // TODO 4 this should be broken into .h, .cpp -#include "IncrementalEncoder.h" -#include "Steering.h" -#include "Schedule.h" -#include "GeoPosition.h" -#include "Mapping.h" -#include "SimpleFilter.h" -#include "Beep.h" -#include "util.h" -#include "updater.h" - -/////////////////////////////////////////////////////////////////////////////////////////////////////// -// DEFINES -/////////////////////////////////////////////////////////////////////////////////////////////////////// - -#define absf(x) (x *= (x < 0.0) ? -1 : 1) - -#define GPS_MIN_SPEED 2.0 // speed below which we won't trust GPS course -#define GPS_MAX_HDOP 2.0 // HDOP above which we won't trust GPS course/position - -// Driver configuration parameters -#define SONARLEFT_CHAN 0 -#define SONARRIGHT_CHAN 1 -#define IRLEFT_CHAN 2 -#define IRRIGHT_CHAN 3 -#define TEMP_CHAN 4 -#define GYRO_CHAN 5 - -#define INSTRUMENT_CHECK 0 -#define AHRS_VISUALIZATION 1 -#define DISPLAY_PANEL 2 - -/////////////////////////////////////////////////////////////////////////////////////////////////////// -// GLOBAL VARIABLES -/////////////////////////////////////////////////////////////////////////////////////////////////////// - -// OUTPUT -DigitalOut confStatus(LED1); // Config file status LED -DigitalOut logStatus(LED2); // Log file status LED -DigitalOut gpsStatus(LED3); // GPS fix status LED -DigitalOut updaterStatus(LED4); // update loop status LED -//DigitalOut sonarStart(p18); // Sends signal to start sonar array pings -Display display; // UI display -//Beep speaker(p24); // Piezo speaker - -// INPUT -Menu menu; -Buttons keypad; - -// COMM -Serial pc(USBTX, USBRX); // PC usb communications -SerialGraphicLCD lcd(LCDTX, LCDRX, SD_FW); // Graphic LCD with summoningdark firmware -Serial tel(TELEMTX, TELEMRX); // UART for telemetry -Telemetry telem(tel); // Setup telemetry system - -// SENSORS -Sensors sensors; // Abstraction of sensor drivers -//DCM ahrs; // ArduPilot/MatrixPilot AHRS -Serial *dev; // For use with bridge - -// MISC -FILE *camlog; // Camera log -Config config; // Configuration utility - -// Timing -Timer timer; // For main loop scheduling - -// GPS Variables -unsigned long age = 0; // gps fix age - -// schedule for LED warning flasher -Schedule blink; - -// Estimation & Navigation Variables -GeoPosition dr_here; // Estimated position based on estimated heading -Mapping mapper; - -/////////////////////////////////////////////////////////////////////////////////////////////////////// -// FUNCTION DEFINITIONS -/////////////////////////////////////////////////////////////////////////////////////////////////////// - -int autonomousMode(void); -void serialBridge(Serial &gps); -int gyroSwing(void); -int setBacklight(void); -int reverseScreen(void); -float irDistance(const unsigned int adc); -extern "C" void mbed_reset(); - -// If we don't close the log file, when we restart, all the written data -// will be lost. So we have to use a button to force mbed to close the -// file and preserve the data. -// - -int dummy(void) -{ - return 0; -} - - -int resetMe() -{ - mbed_reset(); - - return 0; -} - - -int main() -{ - //checkit(__FILE__, __LINE__); - //xTaskCreate( shell, (const signed char * ) "shell", 128, NULL, (tskIDLE_PRIORITY+3), NULL ); - //checkit(__FILE__, __LINE__); - //vTaskStartScheduler(); // should never get past this line. - //while(1); - - // Let's try setting priorities... - //NVIC_SetPriority(DMA_IRQn, 0); - NVIC_SetPriority(TIMER3_IRQn, 2); // updater running off Ticker, must be highest priority!! - NVIC_SetPriority(EINT0_IRQn, 5); // wheel encoders - NVIC_SetPriority(EINT1_IRQn, 5); // wheel encoders - NVIC_SetPriority(EINT2_IRQn, 5); // wheel encoders - NVIC_SetPriority(EINT3_IRQn, 5); // wheel encoders - NVIC_SetPriority(SPI_IRQn, 7); // uSD card, logging - NVIC_SetPriority(UART0_IRQn, 10); // USB - NVIC_SetPriority(UART1_IRQn, 10); - NVIC_SetPriority(UART2_IRQn, 10); - NVIC_SetPriority(UART3_IRQn, 10); - NVIC_SetPriority(I2C0_IRQn, 10); // sensors? - NVIC_SetPriority(I2C1_IRQn, 10); // sensors? - NVIC_SetPriority(I2C2_IRQn, 10); // sensors? - NVIC_SetPriority(ADC_IRQn, 10); // Voltage/current - NVIC_SetPriority(TIMER0_IRQn, 10); // unused(?) - NVIC_SetPriority(TIMER1_IRQn, 10); // unused(?) - NVIC_SetPriority(TIMER2_IRQn, 10); // unused(?) - - // Something here is jacking up the I2C stuff - // Also when initializing with ESC powered, it causes motor to run which - // totally jacks up everything (noise?) - initSteering(); - initThrottle(); - - display.init(); - display.status("Data Bus 2014"); - - // Send data back to the PC - pc.baud(115200); - fputs("Data Bus 2014\n", stdout); - fflush(stdin); - - fputs("Initializing...\n", stdout); - display.status("Initializing"); - - // Initialize status LEDs - updaterStatus = 0; - gpsStatus = 0; - logStatus = 0; - confStatus = 0; - - if (!fifo_init()) { - error("\n\n%% Error initializing SystemState fifo %%\n"); - } - - fputs("Loading configuration...\n", stdout); - display.status("Load config"); - if (config.load("/etc/config.txt")) // Load various configurable parameters, e.g., waypoints, declination, etc. - confStatus = 1; - - initThrottle(); - - //pc.printf("Declination: %.1f\n", config.declination); - pc.puts("Speed: escZero="); - pc.puts(cvftos(config.escZero, 3)); - pc.puts(" escMin="); - pc.puts(cvftos(config.escMin, 3)); - pc.puts(" escMax="); - pc.puts(cvftos(config.escMax, 3)); - pc.puts(" top="); - pc.puts(cvftos(config.topSpeed, 1)); - pc.puts(" turn="); - pc.puts(cvftos(config.turnSpeed, 1)); - pc.puts(" Kp="); - pc.puts(cvftos(config.speedKp, 4)); - pc.puts(" Ki="); - pc.puts(cvftos(config.speedKi, 4)); - pc.puts(" Kd="); - pc.puts(cvftos(config.speedKd, 4)); - pc.puts("\n"); - - pc.puts("Steering: steerZero="); - pc.puts(cvftos(config.steerZero, 2)); - pc.puts(" steerScale="); - pc.puts(cvftos(config.steerScale, 1)); - pc.puts("\n"); - steering.setScale(config.steerScale); - - // Convert lat/lon waypoints to cartesian - mapper.init(config.wptCount, config.wpt); - for (unsigned int w = 0; w < MAXWPT && w < config.wptCount; w++) { - mapper.geoToCart(config.wpt[w], &(config.cwpt[w])); - pc.puts("Waypoint #"); - pc.puts(cvntos(w)); - pc.puts(" ("); - pc.puts(cvftos(config.cwpt[w].x, 4)); - pc.puts(", "); - pc.puts(cvftos(config.cwpt[w].y, 4)); - pc.puts(") lat: "); - pc.puts(cvftos(config.wpt[w].latitude(), 6)); - pc.puts(" lon: "); - pc.puts(cvftos(config.wpt[w].longitude(), 6)); - pc.puts(", topspeed: "); - pc.puts(cvftos(config.topSpeed + config.wptTopSpeedAdj[w], 1)); - pc.puts(", turnspeed: "); - pc.puts(cvftos(config.turnSpeed + config.wptTurnSpeedAdj[w], 1)); - pc.puts("\n"); - } - - // TODO 3 print mag and gyro calibrations - - // TODO 3 remove GPS configuration, all config will be in object itself I think - - display.status("Vehicle config "); - pc.puts("Wheelbase: "); - pc.puts(cvftos(config.wheelbase, 3)); - pc.puts("\n"); - pc.puts("Track Width: "); - pc.puts(cvftos(config.track, 3)); - pc.puts("\n"); - steering.setWheelbase(config.wheelbase); - steering.setTrack(config.track); - - display.status("Encoder config "); - pc.puts("Tire Circumference: "); - pc.puts(cvftos(config.tireCirc, 5)); - pc.puts("\n"); - pc.puts("Ticks per revolution: "); - pc.puts(cvftos(config.encStripes, 5)); - pc.puts("\n"); - sensors.configureEncoders(config.tireCirc, config.encStripes); - - display.status("Nav configuration "); - pc.puts("Intercept distance: "); - pc.puts(cvftos(config.intercept, 1)); - pc.puts("\n"); - steering.setIntercept(config.intercept); - pc.puts("Waypoint distance: "); - pc.puts(cvftos(config.waypointDist, 1)); - pc.puts("\n"); - pc.puts("Brake distance: "); - pc.puts(cvftos(config.brakeDist, 1)); - pc.puts("\n"); - pc.puts("Min turn radius: "); - pc.puts(cvftos(config.minRadius, 3)); - pc.puts("\n"); - - display.status("Gyro config "); - pc.puts("Gyro scale: "); - pc.puts(cvftos(config.gyroScale, 5)); - pc.puts("\n"); - sensors.setGyroScale(config.gyroScale); - - display.status("GPS configuration "); - pc.puts("GPS valid speed: "); - pc.puts(cvftos(config.gpsValidSpeed,1)); - pc.puts("\n"); - - pc.puts("Gyro config "); - pc.puts("\n"); - pc.puts("Gyro scale: "); - pc.puts(cvftos(config.gyroScale, 5)); - pc.puts("\n"); - sensors.setGyroScale(config.gyroScale); - - pc.puts("Calculating offsets...\n"); - display.status("Offset calculation "); - wait(0.2); - // TODO 3 Really need to give the gyro more time to settle - sensors.gps.disable(); - // TODO 2 sensors.Calculate_Offsets(); - - pc.puts("Starting GPS...\n"); - display.status("Start GPS "); // TODO 3: would be nice not to have to pad at this level - wait(0.2); - sensors.gps.setUpdateRate(10); - sensors.gps.enable(); - - pc.puts("Starting Scheduler...\n"); - display.status("Start scheduler "); - wait(0.2); - // Startup sensor/AHRS ticker; update every UPDATE_PERIOD - restartNav(); - startUpdater(); - - pc.puts("Starting keypad...\n"); - - keypad.init(); - - pc.puts("Adding menu items...\n"); - - // Setup LCD Input Menu - menu.add("Auto mode", &autonomousMode); - menu.add("Gyro Calib", &gyroSwing); - menu.add("Backlight", &setBacklight); - menu.add("Reverse", &reverseScreen); - menu.add("Reset", &resetMe); - - pc.puts("Starting main timer...\n"); - - timer.start(); - timer.reset(); - - int thisUpdate = timer.read_ms(); - int nextDisplayUpdate = thisUpdate; - int nextWaypointUpdate = thisUpdate; - char cmd; - bool printMenu = true; - bool printLCDMenu = true; - - pc.puts("Timer done, enter loop...\n"); - - while (1) { - - thisUpdate = timer.read_ms(); - if (thisUpdate > nextDisplayUpdate) { - // Pulling out current state so we get the most current - SystemState *s = fifo_first(); - // TODO 3 fix this so gps is already in state - // Now populate in the current GPS data - s->gpsHDOP = sensors.gps.hdop(); - s->gpsSats = sensors.gps.sat_count(); - - telem.sendPacket(s); - display.update(s); - nextDisplayUpdate = thisUpdate + 200; - } - - // every so often, send the currently configured waypoints - if (thisUpdate > nextWaypointUpdate) { - telem.sendPacket(config.cwpt, config.wptCount); - nextWaypointUpdate = thisUpdate + 10000; - // TODO 2: make this a request/response, Telemetry has to receive packets, decode, etc. - } - - if (keypad.pressed) { - keypad.pressed = false; - printLCDMenu = true; - switch (keypad.which) { - case NEXT_BUTTON: - menu.next(); - break; - case PREV_BUTTON: - menu.prev(); - break; - case SELECT_BUTTON: - display.select(menu.getItemName()); - menu.select(); - printMenu = true; - break; - default: - printLCDMenu = false; - break; - }//switch - keypad.pressed = false; - }// if (keypad.pressed) - - - if (printLCDMenu) { - display.menu( menu.getItemName() ); - display.status("Ready."); - display.redraw(); - printLCDMenu = false; - } - - // TODO 3 move to UI area - if (printMenu) { - fputs("\n==============\nData Bus Menu\n==============\n", stdout); - fputs("0) Autonomous mode\n", stdout); - fputs("1) Bridge serial to GPS\n", stdout); - fputs("2) Gyro calibrate\n", stdout); - fputs("3) Shell\n", stdout); - fputs("R) Reset\n", stdout); - fputs("\nSelect from the above: ", stdout); - fflush(stdout); - printMenu = false; - } - - if (pc.readable()) { - cmd = fgetc(stdin); - fputc(cmd, stdout); - fputc('\n', stdout); - printMenu = true; - printLCDMenu = true; - - switch (cmd) { - case 'R' : - resetMe(); - break; - case '0' : - display.select(menu.getItemName(0)); - autonomousMode(); - break; - case '1' : - display.select("Serial bridge"); - display.status("Standby."); - sensors.gps.enableVerbose(); - serialBridge( *(sensors.gps.getSerial()) ); - sensors.gps.disableVerbose(); - break; - case '2' : - display.select("Gyro Calib"); - display.select(menu.getItemName(2)); - gyroSwing(); - break; - case '3' : - display.select("Shell"); - display.status("Standby."); - shell(0); - break; - default : - break; - } // switch - - } // if (pc.readable()) - - wait(0.1); - - } // while - -} - - - -/////////////////////////////////////////////////////////////////////////////////////////////////////// -// OPERATIONAL MODE FUNCTIONS -/////////////////////////////////////////////////////////////////////////////////////////////////////// - -int autonomousMode() -{ - bool goGoGo = false; // signal to start moving - bool navDone; // signal that we're done navigating - int nextTelemUpdate; // keeps track of teletry update periods - //extern int tSensor, tGPS, tAHRS, tLog; - - sensors.gps.reset_available(); - - // TODO: 3 move to main? - // Navigation - - goGoGo = false; - navDone = false; - keypad.pressed = false; - //bool started = false; // flag to indicate robot has exceeded min speed. - - if (initLogfile()) logStatus = 1; // Open the log file in sprintf format string style; numbers go in %d - wait(0.2); - - sensors.gps.disableVerbose(); - sensors.gps.enable(); - //gps2.enable(); - - fputs("Press select button to start.\n", stdout); - display.status("Select starts."); - wait(1.0); - - timer.reset(); - timer.start(); - nextTelemUpdate = timer.read_ms(); - wait(0.1); - - // Tell the navigation / position estimation stuff to reset to starting waypoint - // Disable 05/27/2013 to try and fix initial heading estimate - //restartNav(); - - // Main loop - // - while(navDone == false) { - ////////////////////////////////////////////////////////////////////////////// - // USER INPUT - ////////////////////////////////////////////////////////////////////////////// - - // Button state machine - // if we've not started going, button starts us - // if we have started going, button stops us - // but only if we've released it first - // - // set throttle only if goGoGo set - if (goGoGo) { - // TODO: 2 Add additional condition of travel for N meters before the HALT button is armed - - if (keypad.pressed == true) { // && started - fputs(">>>>>>>>>>>>>>>>>>>>>>> HALT\n", stdout); - display.status("HALT."); - navDone = true; - goGoGo = false; - keypad.pressed = false; - endRun(); - } - } else { - if (keypad.pressed == true) { - fputs(">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n", stdout); - display.status("GO GO GO!"); - goGoGo = true; - keypad.pressed = false; - beginRun(); - } - } - - // Are we at the last waypoint? - // - if (fifo_first()->nextWaypoint == config.wptCount) { - fputs("Arrived at final destination.\n", stdout); - display.status("Arrived at end."); - navDone = true; - endRun(); - } - - ////////////////////////////////////////////////////////////////////////////// - // TELEMETRY - ////////////////////////////////////////////////////////////////////////////// - if (timer.read_ms() > nextTelemUpdate) { - SystemState *s = fifo_first(); - telem.sendPacket(s); // TODO 4 run this out of timer interrupt - nextTelemUpdate += 200; // TODO 3 increase update speed - } - - ////////////////////////////////////////////////////////////////////////////// - // LOGGING - ////////////////////////////////////////////////////////////////////////////// - // sensor reads are happening in the schedHandler(); - // Are there more items to come out of the log fifo? - // Since this could take anywhere from a few hundred usec to - // 150ms, we run it opportunistically and use a buffer. That way - // the sensor updates, calculation, and control can continue to happen - if (fifo_available()) { - logStatus = !logStatus; // log indicator LED - logData( fifo_pull() ); // log state data to file - logStatus = !logStatus; // log indicator LED - } - - } // while - closeLogfile(); - wait(2.0); - logStatus = 0; - display.status("Completed. Saved."); - wait(2.0); - - updaterStatus = 0; - gpsStatus = 0; - //confStatus = 0; - //flasher = 0; - - sensors.gps.disableVerbose(); - - return 0; -} // autonomousMode - - -/////////////////////////////////////////////////////////////////////////////////////////////////////// -// UTILITY FUNCTIONS -/////////////////////////////////////////////////////////////////////////////////////////////////////// - -// Gather gyro data using turntable equipped with dual channel -// encoder. Use onboard wheel encoder system. Left channel -// is the index (0 degree) mark, while the right channel -// is the incremental encoder. Can then compare gyro integrated -// heading with machine-reported heading -// -int gyroSwing() -{ - FILE *fp; - int now; - int next; - int g[3]; - int leftTotal=0; - int rightTotal=0; - - // Timing is pretty critical so just in case, disable serial processing from GPS - sensors.gps.disable(); - stopUpdater(); - - fputs("Starting gyro swing...\n", stdout); - display.status("Starting..."); -// fp = openlog("gy"); - fp = stdout; - - display.status("Rotate clockwise."); - fputs("Begin clockwise rotation, varying rpm\n", stdout); - wait(1); - - display.status("Select exits."); - fputs("Press select to exit\n", stdout); - wait(1); - - - timer.reset(); - timer.start(); - - next = now = timer.read_ms(); - - sensors._right.read(); // easiest way to reset the heading counter - sensors._left.read(); - - while (1) { - now = timer.read_ms(); - - if (keypad.pressed) { - keypad.pressed = false; - break; - } - - if (now >= next) { - leftTotal += sensors._left.read(); - rightTotal += sensors._right.read(); - sensors._gyro.read(g); - fputs(cvitos(now), fp); - fputs(" ", fp); - fputs(cvntos(leftTotal), fp); - fputs(" ", fp); - fputs(cvntos(rightTotal), fp); - fputs(" ", fp); - fputs(cvitos(g[_z_]), fp); - fputs("\n", fp); - next = now + 50; - } - } - if (fp && fp != stdout) { - fclose(fp); - display.status("Done. Saved."); - fputs("Data collection complete.\n", stdout); - wait(2); - } - - sensors.gps.enable(); - restartNav(); - startUpdater(); - - keypad.pressed = false; - - return 0; -} - - - -void bridgeRecv() -{ -#if 0 - while (dev && dev->readable()) { - pc.putc(dev->getc()); - } -#endif -} - - -void serialBridge(Serial &serial) -{ -#if 0 - char x; - int count = 0; - bool done=false; - - fputs("\nEntering serial bridge in 2 seconds, +++ to escape\n\n", stdout); - sensors.gps.enableVerbose(); - wait(2.0); - //dev = &gps; - sensors.gps.disable(); - serial.baud(38400); - while (!done) { - if (pc.readable()) { - x = pc.getc(); - serial.putc(x); - // escape sequence - if (x == '+') { - if (++count >= 3) done=true; - } else { - count = 0; - } - } - if (serial.readable()) { - fputc(serial.getc(), stdout); - } - } -#endif -} - - -int setBacklight(void) { - Menu bmenu; - bool done = false; - bool printUpdate = false; - static int backlight=100; - - display.select(">> Backlight"); - - while (!done) { - if (keypad.pressed) { - keypad.pressed = false; - printUpdate = true; - switch (keypad.which) { - case NEXT_BUTTON: - backlight+=5; - if (backlight > 100) backlight = 100; - lcd.backlight(backlight); - break; - case PREV_BUTTON: - backlight-=5; - if (backlight < 0) backlight = 0; - lcd.backlight(backlight); - break; - case SELECT_BUTTON: - done = true; - break; - } - } - if (printUpdate) { - printUpdate = false; - lcd.pos(0,1); - // TODO 3 lcd.printf("%3d%%%-16s", backlight, ""); - } - } - - return 0; -} - -int reverseScreen(void) { - lcd.reverseMode(); - - return 0; -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////// -// ADC CONVERSION FUNCTIONS -/////////////////////////////////////////////////////////////////////////////////////////////////////// - -// returns distance in m for Sharp GP2YOA710K0F -// to get m and b, I wrote down volt vs. dist by eyeballin the -// datasheet chart plot. Then used Excel to do linear regression -// -float irDistance(const unsigned int adc) -{ - float b = 1.0934; // Intercept from Excel - float m = 1.4088; // Slope from Excel - - return m / (((float) adc) * 4.95/4096 - b); -}