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
main.cpp@17:08f6ee55abbe, 2018-11-29 (annotated)
- Committer:
- shimniok
- Date:
- Thu Nov 29 17:17:52 2018 +0000
- Revision:
- 17:08f6ee55abbe
- Parent:
- 5:e301811727f7
- Child:
- 20:1c2067937065
Removed refs to SerialMux, Filesystem
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 17:08f6ee55abbe | 1 | /** Code for "Data Bus" UGV entry for Sparkfun AVC 2014 |
shimniok | 0:a6a169de725f | 2 | * http://www.bot-thoughts.com/ |
shimniok | 0:a6a169de725f | 3 | */ |
shimniok | 0:a6a169de725f | 4 | |
shimniok | 0:a6a169de725f | 5 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 6 | // INCLUDES |
shimniok | 0:a6a169de725f | 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 8 | |
shimniok | 17:08f6ee55abbe | 9 | #include "mbed.h" |
shimniok | 0:a6a169de725f | 10 | #include <math.h> |
shimniok | 17:08f6ee55abbe | 11 | #include <stdint.h> |
shimniok | 17:08f6ee55abbe | 12 | #include "devices.h" |
shimniok | 0:a6a169de725f | 13 | #include "globals.h" |
shimniok | 0:a6a169de725f | 14 | #include "Config.h" |
shimniok | 0:a6a169de725f | 15 | #include "Buttons.h" |
shimniok | 0:a6a169de725f | 16 | #include "Display.h" |
shimniok | 0:a6a169de725f | 17 | #include "Menu.h" |
shimniok | 0:a6a169de725f | 18 | #include "GPSStatus.h" |
shimniok | 0:a6a169de725f | 19 | #include "logging.h" |
shimniok | 17:08f6ee55abbe | 20 | #include "Telemetry.h" |
shimniok | 17:08f6ee55abbe | 21 | #include "SystemState.h" |
shimniok | 0:a6a169de725f | 22 | #include "shell.h" |
shimniok | 17:08f6ee55abbe | 23 | #include "Steering.h" |
shimniok | 0:a6a169de725f | 24 | #include "Sensors.h" |
shimniok | 0:a6a169de725f | 25 | #include "kalman.h" |
shimniok | 0:a6a169de725f | 26 | #include "Ublox6.h" |
shimniok | 17:08f6ee55abbe | 27 | #include "PinDetect.h" // TODO 4 this should be broken into .h, .cpp |
shimniok | 0:a6a169de725f | 28 | #include "IncrementalEncoder.h" |
shimniok | 0:a6a169de725f | 29 | #include "Steering.h" |
shimniok | 0:a6a169de725f | 30 | #include "Schedule.h" |
shimniok | 0:a6a169de725f | 31 | #include "GeoPosition.h" |
shimniok | 0:a6a169de725f | 32 | #include "Mapping.h" |
shimniok | 0:a6a169de725f | 33 | #include "SimpleFilter.h" |
shimniok | 0:a6a169de725f | 34 | #include "Beep.h" |
shimniok | 0:a6a169de725f | 35 | #include "util.h" |
shimniok | 0:a6a169de725f | 36 | #include "updater.h" |
shimniok | 0:a6a169de725f | 37 | |
shimniok | 0:a6a169de725f | 38 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 39 | // DEFINES |
shimniok | 0:a6a169de725f | 40 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 41 | |
shimniok | 0:a6a169de725f | 42 | #define absf(x) (x *= (x < 0.0) ? -1 : 1) |
shimniok | 0:a6a169de725f | 43 | |
shimniok | 0:a6a169de725f | 44 | #define GPS_MIN_SPEED 2.0 // speed below which we won't trust GPS course |
shimniok | 0:a6a169de725f | 45 | #define GPS_MAX_HDOP 2.0 // HDOP above which we won't trust GPS course/position |
shimniok | 0:a6a169de725f | 46 | |
shimniok | 0:a6a169de725f | 47 | // Driver configuration parameters |
shimniok | 0:a6a169de725f | 48 | #define SONARLEFT_CHAN 0 |
shimniok | 0:a6a169de725f | 49 | #define SONARRIGHT_CHAN 1 |
shimniok | 0:a6a169de725f | 50 | #define IRLEFT_CHAN 2 |
shimniok | 0:a6a169de725f | 51 | #define IRRIGHT_CHAN 3 |
shimniok | 0:a6a169de725f | 52 | #define TEMP_CHAN 4 |
shimniok | 0:a6a169de725f | 53 | #define GYRO_CHAN 5 |
shimniok | 0:a6a169de725f | 54 | |
shimniok | 0:a6a169de725f | 55 | #define INSTRUMENT_CHECK 0 |
shimniok | 0:a6a169de725f | 56 | #define AHRS_VISUALIZATION 1 |
shimniok | 0:a6a169de725f | 57 | #define DISPLAY_PANEL 2 |
shimniok | 0:a6a169de725f | 58 | |
shimniok | 0:a6a169de725f | 59 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 60 | // GLOBAL VARIABLES |
shimniok | 0:a6a169de725f | 61 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 62 | |
shimniok | 0:a6a169de725f | 63 | // OUTPUT |
shimniok | 0:a6a169de725f | 64 | DigitalOut confStatus(LED1); // Config file status LED |
shimniok | 0:a6a169de725f | 65 | DigitalOut logStatus(LED2); // Log file status LED |
shimniok | 0:a6a169de725f | 66 | DigitalOut gpsStatus(LED3); // GPS fix status LED |
shimniok | 17:08f6ee55abbe | 67 | DigitalOut updaterStatus(LED4); // update loop status LED |
shimniok | 17:08f6ee55abbe | 68 | //DigitalOut sonarStart(p18); // Sends signal to start sonar array pings |
shimniok | 0:a6a169de725f | 69 | Display display; // UI display |
shimniok | 17:08f6ee55abbe | 70 | //Beep speaker(p24); // Piezo speaker |
shimniok | 0:a6a169de725f | 71 | |
shimniok | 0:a6a169de725f | 72 | // INPUT |
shimniok | 0:a6a169de725f | 73 | Menu menu; |
shimniok | 0:a6a169de725f | 74 | Buttons keypad; |
shimniok | 0:a6a169de725f | 75 | |
shimniok | 0:a6a169de725f | 76 | // COMM |
shimniok | 0:a6a169de725f | 77 | Serial pc(USBTX, USBRX); // PC usb communications |
shimniok | 17:08f6ee55abbe | 78 | SerialGraphicLCD lcd(LCDTX, LCDRX, SD_FW); // Graphic LCD with summoningdark firmware |
shimniok | 17:08f6ee55abbe | 79 | Serial tel(TELEMTX, TELEMRX); // UART for telemetry |
shimniok | 17:08f6ee55abbe | 80 | Telemetry telem(tel); // Setup telemetry system |
shimniok | 0:a6a169de725f | 81 | |
shimniok | 0:a6a169de725f | 82 | // SENSORS |
shimniok | 0:a6a169de725f | 83 | Sensors sensors; // Abstraction of sensor drivers |
shimniok | 0:a6a169de725f | 84 | //DCM ahrs; // ArduPilot/MatrixPilot AHRS |
shimniok | 0:a6a169de725f | 85 | Serial *dev; // For use with bridge |
shimniok | 0:a6a169de725f | 86 | |
shimniok | 0:a6a169de725f | 87 | // MISC |
shimniok | 0:a6a169de725f | 88 | FILE *camlog; // Camera log |
shimniok | 17:08f6ee55abbe | 89 | Filesystem fs; // Set up filesystems |
shimniok | 17:08f6ee55abbe | 90 | Config config; // Configuration utility |
shimniok | 0:a6a169de725f | 91 | |
shimniok | 0:a6a169de725f | 92 | // Timing |
shimniok | 0:a6a169de725f | 93 | Timer timer; // For main loop scheduling |
shimniok | 0:a6a169de725f | 94 | |
shimniok | 0:a6a169de725f | 95 | // GPS Variables |
shimniok | 0:a6a169de725f | 96 | unsigned long age = 0; // gps fix age |
shimniok | 0:a6a169de725f | 97 | |
shimniok | 0:a6a169de725f | 98 | // schedule for LED warning flasher |
shimniok | 0:a6a169de725f | 99 | Schedule blink; |
shimniok | 0:a6a169de725f | 100 | |
shimniok | 0:a6a169de725f | 101 | // Estimation & Navigation Variables |
shimniok | 0:a6a169de725f | 102 | GeoPosition dr_here; // Estimated position based on estimated heading |
shimniok | 0:a6a169de725f | 103 | Mapping mapper; |
shimniok | 0:a6a169de725f | 104 | |
shimniok | 0:a6a169de725f | 105 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 106 | // FUNCTION DEFINITIONS |
shimniok | 0:a6a169de725f | 107 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 108 | |
shimniok | 0:a6a169de725f | 109 | int autonomousMode(void); |
shimniok | 0:a6a169de725f | 110 | void serialBridge(Serial &gps); |
shimniok | 0:a6a169de725f | 111 | int gyroSwing(void); |
shimniok | 0:a6a169de725f | 112 | int setBacklight(void); |
shimniok | 0:a6a169de725f | 113 | int reverseScreen(void); |
shimniok | 17:08f6ee55abbe | 114 | float irDistance(const unsigned int adc); |
shimniok | 0:a6a169de725f | 115 | extern "C" void mbed_reset(); |
shimniok | 0:a6a169de725f | 116 | |
shimniok | 0:a6a169de725f | 117 | // If we don't close the log file, when we restart, all the written data |
shimniok | 0:a6a169de725f | 118 | // will be lost. So we have to use a button to force mbed to close the |
shimniok | 0:a6a169de725f | 119 | // file and preserve the data. |
shimniok | 0:a6a169de725f | 120 | // |
shimniok | 0:a6a169de725f | 121 | |
shimniok | 0:a6a169de725f | 122 | int dummy(void) |
shimniok | 0:a6a169de725f | 123 | { |
shimniok | 0:a6a169de725f | 124 | return 0; |
shimniok | 0:a6a169de725f | 125 | } |
shimniok | 0:a6a169de725f | 126 | |
shimniok | 0:a6a169de725f | 127 | |
shimniok | 0:a6a169de725f | 128 | int resetMe() |
shimniok | 0:a6a169de725f | 129 | { |
shimniok | 0:a6a169de725f | 130 | mbed_reset(); |
shimniok | 0:a6a169de725f | 131 | |
shimniok | 0:a6a169de725f | 132 | return 0; |
shimniok | 0:a6a169de725f | 133 | } |
shimniok | 0:a6a169de725f | 134 | |
shimniok | 0:a6a169de725f | 135 | |
shimniok | 0:a6a169de725f | 136 | int main() |
shimniok | 0:a6a169de725f | 137 | { |
shimniok | 17:08f6ee55abbe | 138 | //checkit(__FILE__, __LINE__); |
shimniok | 17:08f6ee55abbe | 139 | //xTaskCreate( shell, (const signed char * ) "shell", 128, NULL, (tskIDLE_PRIORITY+3), NULL ); |
shimniok | 17:08f6ee55abbe | 140 | //checkit(__FILE__, __LINE__); |
shimniok | 17:08f6ee55abbe | 141 | //vTaskStartScheduler(); // should never get past this line. |
shimniok | 17:08f6ee55abbe | 142 | //while(1); |
shimniok | 17:08f6ee55abbe | 143 | |
shimniok | 0:a6a169de725f | 144 | // Let's try setting priorities... |
shimniok | 17:08f6ee55abbe | 145 | //NVIC_SetPriority(DMA_IRQn, 0); |
shimniok | 17:08f6ee55abbe | 146 | NVIC_SetPriority(TIMER3_IRQn, 2); // updater running off Ticker, must be highest priority!! |
shimniok | 17:08f6ee55abbe | 147 | NVIC_SetPriority(EINT0_IRQn, 5); // wheel encoders |
shimniok | 17:08f6ee55abbe | 148 | NVIC_SetPriority(EINT1_IRQn, 5); // wheel encoders |
shimniok | 17:08f6ee55abbe | 149 | NVIC_SetPriority(EINT2_IRQn, 5); // wheel encoders |
shimniok | 17:08f6ee55abbe | 150 | NVIC_SetPriority(EINT3_IRQn, 5); // wheel encoders |
shimniok | 17:08f6ee55abbe | 151 | NVIC_SetPriority(SPI_IRQn, 7); // uSD card, logging |
shimniok | 17:08f6ee55abbe | 152 | NVIC_SetPriority(UART0_IRQn, 10); // USB |
shimniok | 2:fbc6e3cf3ed8 | 153 | NVIC_SetPriority(UART1_IRQn, 10); |
shimniok | 2:fbc6e3cf3ed8 | 154 | NVIC_SetPriority(UART2_IRQn, 10); |
shimniok | 2:fbc6e3cf3ed8 | 155 | NVIC_SetPriority(UART3_IRQn, 10); |
shimniok | 2:fbc6e3cf3ed8 | 156 | NVIC_SetPriority(I2C0_IRQn, 10); // sensors? |
shimniok | 2:fbc6e3cf3ed8 | 157 | NVIC_SetPriority(I2C1_IRQn, 10); // sensors? |
shimniok | 2:fbc6e3cf3ed8 | 158 | NVIC_SetPriority(I2C2_IRQn, 10); // sensors? |
shimniok | 2:fbc6e3cf3ed8 | 159 | NVIC_SetPriority(ADC_IRQn, 10); // Voltage/current |
shimniok | 17:08f6ee55abbe | 160 | NVIC_SetPriority(TIMER0_IRQn, 10); // unused(?) |
shimniok | 17:08f6ee55abbe | 161 | NVIC_SetPriority(TIMER1_IRQn, 10); // unused(?) |
shimniok | 17:08f6ee55abbe | 162 | NVIC_SetPriority(TIMER2_IRQn, 10); // unused(?) |
shimniok | 0:a6a169de725f | 163 | |
shimniok | 2:fbc6e3cf3ed8 | 164 | // Something here is jacking up the I2C stuff |
shimniok | 2:fbc6e3cf3ed8 | 165 | // Also when initializing with ESC powered, it causes motor to run which |
shimniok | 2:fbc6e3cf3ed8 | 166 | // totally jacks up everything (noise?) |
shimniok | 2:fbc6e3cf3ed8 | 167 | initSteering(); |
shimniok | 2:fbc6e3cf3ed8 | 168 | initThrottle(); |
shimniok | 0:a6a169de725f | 169 | |
shimniok | 0:a6a169de725f | 170 | display.init(); |
shimniok | 17:08f6ee55abbe | 171 | display.status("Data Bus 2014"); |
shimniok | 17:08f6ee55abbe | 172 | |
shimniok | 17:08f6ee55abbe | 173 | // Send data back to the PC |
shimniok | 17:08f6ee55abbe | 174 | pc.baud(115200); |
shimniok | 17:08f6ee55abbe | 175 | fputs("Data Bus 2014\n", stdout); |
shimniok | 17:08f6ee55abbe | 176 | fflush(stdin); |
shimniok | 17:08f6ee55abbe | 177 | |
shimniok | 17:08f6ee55abbe | 178 | fputs("Initializing...\n", stdout); |
shimniok | 0:a6a169de725f | 179 | display.status("Initializing"); |
shimniok | 0:a6a169de725f | 180 | |
shimniok | 0:a6a169de725f | 181 | // Initialize status LEDs |
shimniok | 17:08f6ee55abbe | 182 | updaterStatus = 0; |
shimniok | 0:a6a169de725f | 183 | gpsStatus = 0; |
shimniok | 0:a6a169de725f | 184 | logStatus = 0; |
shimniok | 0:a6a169de725f | 185 | confStatus = 0; |
shimniok | 0:a6a169de725f | 186 | |
shimniok | 17:08f6ee55abbe | 187 | if (!fifo_init()) { |
shimniok | 17:08f6ee55abbe | 188 | error("\n\n%% Error initializing SystemState fifo %%\n"); |
shimniok | 17:08f6ee55abbe | 189 | } |
shimniok | 0:a6a169de725f | 190 | |
shimniok | 17:08f6ee55abbe | 191 | fputs("Loading configuration...\n", stdout); |
shimniok | 0:a6a169de725f | 192 | display.status("Load config"); |
shimniok | 17:08f6ee55abbe | 193 | if (config.load("/etc/config.txt")) // Load various configurable parameters, e.g., waypoints, declination, etc. |
shimniok | 0:a6a169de725f | 194 | confStatus = 1; |
shimniok | 1:cb84b477886c | 195 | |
shimniok | 17:08f6ee55abbe | 196 | initThrottle(); |
shimniok | 17:08f6ee55abbe | 197 | |
shimniok | 1:cb84b477886c | 198 | //pc.printf("Declination: %.1f\n", config.declination); |
shimniok | 17:08f6ee55abbe | 199 | pc.puts("Speed: escZero="); |
shimniok | 17:08f6ee55abbe | 200 | pc.puts(cvftos(config.escZero, 3)); |
shimniok | 17:08f6ee55abbe | 201 | pc.puts(" escMin="); |
shimniok | 17:08f6ee55abbe | 202 | pc.puts(cvftos(config.escMin, 3)); |
shimniok | 17:08f6ee55abbe | 203 | pc.puts(" escMax="); |
shimniok | 17:08f6ee55abbe | 204 | pc.puts(cvftos(config.escMax, 3)); |
shimniok | 17:08f6ee55abbe | 205 | pc.puts(" top="); |
shimniok | 17:08f6ee55abbe | 206 | pc.puts(cvftos(config.topSpeed, 1)); |
shimniok | 17:08f6ee55abbe | 207 | pc.puts(" turn="); |
shimniok | 17:08f6ee55abbe | 208 | pc.puts(cvftos(config.turnSpeed, 1)); |
shimniok | 17:08f6ee55abbe | 209 | pc.puts(" Kp="); |
shimniok | 17:08f6ee55abbe | 210 | pc.puts(cvftos(config.speedKp, 4)); |
shimniok | 17:08f6ee55abbe | 211 | pc.puts(" Ki="); |
shimniok | 17:08f6ee55abbe | 212 | pc.puts(cvftos(config.speedKi, 4)); |
shimniok | 17:08f6ee55abbe | 213 | pc.puts(" Kd="); |
shimniok | 17:08f6ee55abbe | 214 | pc.puts(cvftos(config.speedKd, 4)); |
shimniok | 17:08f6ee55abbe | 215 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 216 | |
shimniok | 17:08f6ee55abbe | 217 | pc.puts("Steering: steerZero="); |
shimniok | 17:08f6ee55abbe | 218 | pc.puts(cvftos(config.steerZero, 2)); |
shimniok | 17:08f6ee55abbe | 219 | pc.puts(" steerScale="); |
shimniok | 17:08f6ee55abbe | 220 | pc.puts(cvftos(config.steerScale, 1)); |
shimniok | 17:08f6ee55abbe | 221 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 222 | steering.setScale(config.steerScale); |
shimniok | 0:a6a169de725f | 223 | |
shimniok | 0:a6a169de725f | 224 | // Convert lat/lon waypoints to cartesian |
shimniok | 0:a6a169de725f | 225 | mapper.init(config.wptCount, config.wpt); |
shimniok | 17:08f6ee55abbe | 226 | for (unsigned int w = 0; w < MAXWPT && w < config.wptCount; w++) { |
shimniok | 0:a6a169de725f | 227 | mapper.geoToCart(config.wpt[w], &(config.cwpt[w])); |
shimniok | 17:08f6ee55abbe | 228 | pc.puts("Waypoint #"); |
shimniok | 17:08f6ee55abbe | 229 | pc.puts(cvntos(w)); |
shimniok | 17:08f6ee55abbe | 230 | pc.puts(" ("); |
shimniok | 17:08f6ee55abbe | 231 | pc.puts(cvftos(config.cwpt[w].x, 4)); |
shimniok | 17:08f6ee55abbe | 232 | pc.puts(", "); |
shimniok | 17:08f6ee55abbe | 233 | pc.puts(cvftos(config.cwpt[w].y, 4)); |
shimniok | 17:08f6ee55abbe | 234 | pc.puts(") lat: "); |
shimniok | 17:08f6ee55abbe | 235 | pc.puts(cvftos(config.wpt[w].latitude(), 6)); |
shimniok | 17:08f6ee55abbe | 236 | pc.puts(" lon: "); |
shimniok | 17:08f6ee55abbe | 237 | pc.puts(cvftos(config.wpt[w].longitude(), 6)); |
shimniok | 17:08f6ee55abbe | 238 | pc.puts(", topspeed: "); |
shimniok | 17:08f6ee55abbe | 239 | pc.puts(cvftos(config.topSpeed + config.wptTopSpeedAdj[w], 1)); |
shimniok | 17:08f6ee55abbe | 240 | pc.puts(", turnspeed: "); |
shimniok | 17:08f6ee55abbe | 241 | pc.puts(cvftos(config.turnSpeed + config.wptTurnSpeedAdj[w], 1)); |
shimniok | 17:08f6ee55abbe | 242 | pc.puts("\n"); |
shimniok | 0:a6a169de725f | 243 | } |
shimniok | 0:a6a169de725f | 244 | |
shimniok | 1:cb84b477886c | 245 | // TODO 3 print mag and gyro calibrations |
shimniok | 0:a6a169de725f | 246 | |
shimniok | 1:cb84b477886c | 247 | // TODO 3 remove GPS configuration, all config will be in object itself I think |
shimniok | 0:a6a169de725f | 248 | |
shimniok | 17:08f6ee55abbe | 249 | display.status("Vehicle config "); |
shimniok | 17:08f6ee55abbe | 250 | pc.puts("Wheelbase: "); |
shimniok | 17:08f6ee55abbe | 251 | pc.puts(cvftos(config.wheelbase, 3)); |
shimniok | 17:08f6ee55abbe | 252 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 253 | pc.puts("Track Width: "); |
shimniok | 17:08f6ee55abbe | 254 | pc.puts(cvftos(config.track, 3)); |
shimniok | 17:08f6ee55abbe | 255 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 256 | steering.setWheelbase(config.wheelbase); |
shimniok | 17:08f6ee55abbe | 257 | steering.setTrack(config.track); |
shimniok | 17:08f6ee55abbe | 258 | |
shimniok | 17:08f6ee55abbe | 259 | display.status("Encoder config "); |
shimniok | 17:08f6ee55abbe | 260 | pc.puts("Tire Circumference: "); |
shimniok | 17:08f6ee55abbe | 261 | pc.puts(cvftos(config.tireCirc, 5)); |
shimniok | 17:08f6ee55abbe | 262 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 263 | pc.puts("Ticks per revolution: "); |
shimniok | 17:08f6ee55abbe | 264 | pc.puts(cvftos(config.encStripes, 5)); |
shimniok | 17:08f6ee55abbe | 265 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 266 | sensors.configureEncoders(config.tireCirc, config.encStripes); |
shimniok | 17:08f6ee55abbe | 267 | |
shimniok | 0:a6a169de725f | 268 | display.status("Nav configuration "); |
shimniok | 17:08f6ee55abbe | 269 | pc.puts("Intercept distance: "); |
shimniok | 17:08f6ee55abbe | 270 | pc.puts(cvftos(config.intercept, 1)); |
shimniok | 17:08f6ee55abbe | 271 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 272 | steering.setIntercept(config.intercept); |
shimniok | 17:08f6ee55abbe | 273 | pc.puts("Waypoint distance: "); |
shimniok | 17:08f6ee55abbe | 274 | pc.puts(cvftos(config.waypointDist, 1)); |
shimniok | 17:08f6ee55abbe | 275 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 276 | pc.puts("Brake distance: "); |
shimniok | 17:08f6ee55abbe | 277 | pc.puts(cvftos(config.brakeDist, 1)); |
shimniok | 17:08f6ee55abbe | 278 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 279 | pc.puts("Min turn radius: "); |
shimniok | 17:08f6ee55abbe | 280 | pc.puts(cvftos(config.minRadius, 3)); |
shimniok | 17:08f6ee55abbe | 281 | pc.puts("\n"); |
shimniok | 0:a6a169de725f | 282 | |
shimniok | 17:08f6ee55abbe | 283 | display.status("Gyro config "); |
shimniok | 17:08f6ee55abbe | 284 | pc.puts("Gyro scale: "); |
shimniok | 17:08f6ee55abbe | 285 | pc.puts(cvftos(config.gyroScale, 5)); |
shimniok | 17:08f6ee55abbe | 286 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 287 | sensors.setGyroScale(config.gyroScale); |
shimniok | 17:08f6ee55abbe | 288 | |
shimniok | 17:08f6ee55abbe | 289 | display.status("GPS configuration "); |
shimniok | 17:08f6ee55abbe | 290 | pc.puts("GPS valid speed: "); |
shimniok | 17:08f6ee55abbe | 291 | pc.puts(cvftos(config.gpsValidSpeed,1)); |
shimniok | 17:08f6ee55abbe | 292 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 293 | |
shimniok | 17:08f6ee55abbe | 294 | pc.puts("Gyro config "); |
shimniok | 17:08f6ee55abbe | 295 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 296 | pc.puts("Gyro scale: "); |
shimniok | 17:08f6ee55abbe | 297 | pc.puts(cvftos(config.gyroScale, 5)); |
shimniok | 17:08f6ee55abbe | 298 | pc.puts("\n"); |
shimniok | 17:08f6ee55abbe | 299 | sensors.setGyroScale(config.gyroScale); |
shimniok | 17:08f6ee55abbe | 300 | |
shimniok | 17:08f6ee55abbe | 301 | pc.puts("Calculating offsets...\n"); |
shimniok | 0:a6a169de725f | 302 | display.status("Offset calculation "); |
shimniok | 0:a6a169de725f | 303 | wait(0.2); |
shimniok | 1:cb84b477886c | 304 | // TODO 3 Really need to give the gyro more time to settle |
shimniok | 0:a6a169de725f | 305 | sensors.gps.disable(); |
shimniok | 17:08f6ee55abbe | 306 | // TODO 2 sensors.Calculate_Offsets(); |
shimniok | 0:a6a169de725f | 307 | |
shimniok | 17:08f6ee55abbe | 308 | pc.puts("Starting GPS...\n"); |
shimniok | 0:a6a169de725f | 309 | display.status("Start GPS "); // TODO 3: would be nice not to have to pad at this level |
shimniok | 0:a6a169de725f | 310 | wait(0.2); |
shimniok | 0:a6a169de725f | 311 | sensors.gps.setUpdateRate(10); |
shimniok | 17:08f6ee55abbe | 312 | sensors.gps.enable(); |
shimniok | 0:a6a169de725f | 313 | |
shimniok | 17:08f6ee55abbe | 314 | pc.puts("Starting Scheduler...\n"); |
shimniok | 0:a6a169de725f | 315 | display.status("Start scheduler "); |
shimniok | 0:a6a169de725f | 316 | wait(0.2); |
shimniok | 0:a6a169de725f | 317 | // Startup sensor/AHRS ticker; update every UPDATE_PERIOD |
shimniok | 0:a6a169de725f | 318 | restartNav(); |
shimniok | 0:a6a169de725f | 319 | startUpdater(); |
shimniok | 0:a6a169de725f | 320 | |
shimniok | 17:08f6ee55abbe | 321 | pc.puts("Starting keypad...\n"); |
shimniok | 0:a6a169de725f | 322 | |
shimniok | 0:a6a169de725f | 323 | keypad.init(); |
shimniok | 0:a6a169de725f | 324 | |
shimniok | 17:08f6ee55abbe | 325 | pc.puts("Adding menu items...\n"); |
shimniok | 17:08f6ee55abbe | 326 | |
shimniok | 0:a6a169de725f | 327 | // Setup LCD Input Menu |
shimniok | 0:a6a169de725f | 328 | menu.add("Auto mode", &autonomousMode); |
shimniok | 0:a6a169de725f | 329 | menu.add("Gyro Calib", &gyroSwing); |
shimniok | 0:a6a169de725f | 330 | menu.add("Backlight", &setBacklight); |
shimniok | 0:a6a169de725f | 331 | menu.add("Reverse", &reverseScreen); |
shimniok | 0:a6a169de725f | 332 | menu.add("Reset", &resetMe); |
shimniok | 17:08f6ee55abbe | 333 | |
shimniok | 17:08f6ee55abbe | 334 | pc.puts("Starting main timer...\n"); |
shimniok | 17:08f6ee55abbe | 335 | |
shimniok | 0:a6a169de725f | 336 | timer.start(); |
shimniok | 0:a6a169de725f | 337 | timer.reset(); |
shimniok | 0:a6a169de725f | 338 | |
shimniok | 0:a6a169de725f | 339 | int thisUpdate = timer.read_ms(); |
shimniok | 17:08f6ee55abbe | 340 | int nextDisplayUpdate = thisUpdate; |
shimniok | 17:08f6ee55abbe | 341 | int nextWaypointUpdate = thisUpdate; |
shimniok | 17:08f6ee55abbe | 342 | char cmd; |
shimniok | 17:08f6ee55abbe | 343 | bool printMenu = true; |
shimniok | 17:08f6ee55abbe | 344 | bool printLCDMenu = true; |
shimniok | 17:08f6ee55abbe | 345 | |
shimniok | 17:08f6ee55abbe | 346 | pc.puts("Timer done, enter loop...\n"); |
shimniok | 0:a6a169de725f | 347 | |
shimniok | 0:a6a169de725f | 348 | while (1) { |
shimniok | 0:a6a169de725f | 349 | |
shimniok | 17:08f6ee55abbe | 350 | thisUpdate = timer.read_ms(); |
shimniok | 17:08f6ee55abbe | 351 | if (thisUpdate > nextDisplayUpdate) { |
shimniok | 1:cb84b477886c | 352 | // Pulling out current state so we get the most current |
shimniok | 17:08f6ee55abbe | 353 | SystemState *s = fifo_first(); |
shimniok | 17:08f6ee55abbe | 354 | // TODO 3 fix this so gps is already in state |
shimniok | 1:cb84b477886c | 355 | // Now populate in the current GPS data |
shimniok | 17:08f6ee55abbe | 356 | s->gpsHDOP = sensors.gps.hdop(); |
shimniok | 17:08f6ee55abbe | 357 | s->gpsSats = sensors.gps.sat_count(); |
shimniok | 17:08f6ee55abbe | 358 | |
shimniok | 17:08f6ee55abbe | 359 | telem.sendPacket(s); |
shimniok | 0:a6a169de725f | 360 | display.update(s); |
shimniok | 17:08f6ee55abbe | 361 | nextDisplayUpdate = thisUpdate + 200; |
shimniok | 0:a6a169de725f | 362 | } |
shimniok | 17:08f6ee55abbe | 363 | |
shimniok | 17:08f6ee55abbe | 364 | // every so often, send the currently configured waypoints |
shimniok | 17:08f6ee55abbe | 365 | if (thisUpdate > nextWaypointUpdate) { |
shimniok | 17:08f6ee55abbe | 366 | telem.sendPacket(config.cwpt, config.wptCount); |
shimniok | 17:08f6ee55abbe | 367 | nextWaypointUpdate = thisUpdate + 10000; |
shimniok | 17:08f6ee55abbe | 368 | // TODO 2: make this a request/response, Telemetry has to receive packets, decode, etc. |
shimniok | 17:08f6ee55abbe | 369 | } |
shimniok | 17:08f6ee55abbe | 370 | |
shimniok | 0:a6a169de725f | 371 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 372 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 373 | printLCDMenu = true; |
shimniok | 0:a6a169de725f | 374 | switch (keypad.which) { |
shimniok | 0:a6a169de725f | 375 | case NEXT_BUTTON: |
shimniok | 0:a6a169de725f | 376 | menu.next(); |
shimniok | 0:a6a169de725f | 377 | break; |
shimniok | 0:a6a169de725f | 378 | case PREV_BUTTON: |
shimniok | 0:a6a169de725f | 379 | menu.prev(); |
shimniok | 0:a6a169de725f | 380 | break; |
shimniok | 0:a6a169de725f | 381 | case SELECT_BUTTON: |
shimniok | 0:a6a169de725f | 382 | display.select(menu.getItemName()); |
shimniok | 0:a6a169de725f | 383 | menu.select(); |
shimniok | 0:a6a169de725f | 384 | printMenu = true; |
shimniok | 0:a6a169de725f | 385 | break; |
shimniok | 0:a6a169de725f | 386 | default: |
shimniok | 0:a6a169de725f | 387 | printLCDMenu = false; |
shimniok | 0:a6a169de725f | 388 | break; |
shimniok | 0:a6a169de725f | 389 | }//switch |
shimniok | 0:a6a169de725f | 390 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 391 | }// if (keypad.pressed) |
shimniok | 0:a6a169de725f | 392 | |
shimniok | 0:a6a169de725f | 393 | |
shimniok | 0:a6a169de725f | 394 | if (printLCDMenu) { |
shimniok | 0:a6a169de725f | 395 | display.menu( menu.getItemName() ); |
shimniok | 0:a6a169de725f | 396 | display.status("Ready."); |
shimniok | 0:a6a169de725f | 397 | display.redraw(); |
shimniok | 0:a6a169de725f | 398 | printLCDMenu = false; |
shimniok | 0:a6a169de725f | 399 | } |
shimniok | 0:a6a169de725f | 400 | |
shimniok | 17:08f6ee55abbe | 401 | // TODO 3 move to UI area |
shimniok | 0:a6a169de725f | 402 | if (printMenu) { |
shimniok | 17:08f6ee55abbe | 403 | fputs("\n==============\nData Bus Menu\n==============\n", stdout); |
shimniok | 17:08f6ee55abbe | 404 | fputs("0) Autonomous mode\n", stdout); |
shimniok | 17:08f6ee55abbe | 405 | fputs("1) Bridge serial to GPS\n", stdout); |
shimniok | 17:08f6ee55abbe | 406 | fputs("2) Gyro calibrate\n", stdout); |
shimniok | 17:08f6ee55abbe | 407 | fputs("3) Shell\n", stdout); |
shimniok | 17:08f6ee55abbe | 408 | fputs("R) Reset\n", stdout); |
shimniok | 17:08f6ee55abbe | 409 | fputs("\nSelect from the above: ", stdout); |
shimniok | 17:08f6ee55abbe | 410 | fflush(stdout); |
shimniok | 0:a6a169de725f | 411 | printMenu = false; |
shimniok | 0:a6a169de725f | 412 | } |
shimniok | 0:a6a169de725f | 413 | |
shimniok | 0:a6a169de725f | 414 | if (pc.readable()) { |
shimniok | 17:08f6ee55abbe | 415 | cmd = fgetc(stdin); |
shimniok | 17:08f6ee55abbe | 416 | fputc(cmd, stdout); |
shimniok | 17:08f6ee55abbe | 417 | fputc('\n', stdout); |
shimniok | 0:a6a169de725f | 418 | printMenu = true; |
shimniok | 0:a6a169de725f | 419 | printLCDMenu = true; |
shimniok | 0:a6a169de725f | 420 | |
shimniok | 0:a6a169de725f | 421 | switch (cmd) { |
shimniok | 0:a6a169de725f | 422 | case 'R' : |
shimniok | 0:a6a169de725f | 423 | resetMe(); |
shimniok | 0:a6a169de725f | 424 | break; |
shimniok | 0:a6a169de725f | 425 | case '0' : |
shimniok | 0:a6a169de725f | 426 | display.select(menu.getItemName(0)); |
shimniok | 0:a6a169de725f | 427 | autonomousMode(); |
shimniok | 0:a6a169de725f | 428 | break; |
shimniok | 0:a6a169de725f | 429 | case '1' : |
shimniok | 0:a6a169de725f | 430 | display.select("Serial bridge"); |
shimniok | 0:a6a169de725f | 431 | display.status("Standby."); |
shimniok | 0:a6a169de725f | 432 | sensors.gps.enableVerbose(); |
shimniok | 0:a6a169de725f | 433 | serialBridge( *(sensors.gps.getSerial()) ); |
shimniok | 0:a6a169de725f | 434 | sensors.gps.disableVerbose(); |
shimniok | 0:a6a169de725f | 435 | break; |
shimniok | 0:a6a169de725f | 436 | case '2' : |
shimniok | 17:08f6ee55abbe | 437 | display.select("Gyro Calib"); |
shimniok | 0:a6a169de725f | 438 | display.select(menu.getItemName(2)); |
shimniok | 0:a6a169de725f | 439 | gyroSwing(); |
shimniok | 0:a6a169de725f | 440 | break; |
shimniok | 17:08f6ee55abbe | 441 | case '3' : |
shimniok | 0:a6a169de725f | 442 | display.select("Shell"); |
shimniok | 0:a6a169de725f | 443 | display.status("Standby."); |
shimniok | 17:08f6ee55abbe | 444 | shell(0); |
shimniok | 0:a6a169de725f | 445 | break; |
shimniok | 0:a6a169de725f | 446 | default : |
shimniok | 0:a6a169de725f | 447 | break; |
shimniok | 0:a6a169de725f | 448 | } // switch |
shimniok | 0:a6a169de725f | 449 | |
shimniok | 0:a6a169de725f | 450 | } // if (pc.readable()) |
shimniok | 0:a6a169de725f | 451 | |
shimniok | 17:08f6ee55abbe | 452 | wait(0.1); |
shimniok | 17:08f6ee55abbe | 453 | |
shimniok | 0:a6a169de725f | 454 | } // while |
shimniok | 0:a6a169de725f | 455 | |
shimniok | 0:a6a169de725f | 456 | } |
shimniok | 0:a6a169de725f | 457 | |
shimniok | 0:a6a169de725f | 458 | |
shimniok | 0:a6a169de725f | 459 | |
shimniok | 0:a6a169de725f | 460 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 461 | // OPERATIONAL MODE FUNCTIONS |
shimniok | 0:a6a169de725f | 462 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 463 | |
shimniok | 0:a6a169de725f | 464 | int autonomousMode() |
shimniok | 0:a6a169de725f | 465 | { |
shimniok | 17:08f6ee55abbe | 466 | bool goGoGo = false; // signal to start moving |
shimniok | 17:08f6ee55abbe | 467 | bool navDone; // signal that we're done navigating |
shimniok | 17:08f6ee55abbe | 468 | int nextTelemUpdate; // keeps track of teletry update periods |
shimniok | 17:08f6ee55abbe | 469 | //extern int tSensor, tGPS, tAHRS, tLog; |
shimniok | 0:a6a169de725f | 470 | |
shimniok | 0:a6a169de725f | 471 | sensors.gps.reset_available(); |
shimniok | 0:a6a169de725f | 472 | |
shimniok | 0:a6a169de725f | 473 | // TODO: 3 move to main? |
shimniok | 0:a6a169de725f | 474 | // Navigation |
shimniok | 0:a6a169de725f | 475 | |
shimniok | 0:a6a169de725f | 476 | goGoGo = false; |
shimniok | 0:a6a169de725f | 477 | navDone = false; |
shimniok | 0:a6a169de725f | 478 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 479 | //bool started = false; // flag to indicate robot has exceeded min speed. |
shimniok | 0:a6a169de725f | 480 | |
shimniok | 17:08f6ee55abbe | 481 | if (initLogfile()) logStatus = 1; // Open the log file in sprintf format string style; numbers go in %d |
shimniok | 0:a6a169de725f | 482 | wait(0.2); |
shimniok | 0:a6a169de725f | 483 | |
shimniok | 0:a6a169de725f | 484 | sensors.gps.disableVerbose(); |
shimniok | 0:a6a169de725f | 485 | sensors.gps.enable(); |
shimniok | 0:a6a169de725f | 486 | //gps2.enable(); |
shimniok | 0:a6a169de725f | 487 | |
shimniok | 17:08f6ee55abbe | 488 | fputs("Press select button to start.\n", stdout); |
shimniok | 0:a6a169de725f | 489 | display.status("Select starts."); |
shimniok | 0:a6a169de725f | 490 | wait(1.0); |
shimniok | 0:a6a169de725f | 491 | |
shimniok | 0:a6a169de725f | 492 | timer.reset(); |
shimniok | 0:a6a169de725f | 493 | timer.start(); |
shimniok | 17:08f6ee55abbe | 494 | nextTelemUpdate = timer.read_ms(); |
shimniok | 0:a6a169de725f | 495 | wait(0.1); |
shimniok | 0:a6a169de725f | 496 | |
shimniok | 0:a6a169de725f | 497 | // Tell the navigation / position estimation stuff to reset to starting waypoint |
shimniok | 1:cb84b477886c | 498 | // Disable 05/27/2013 to try and fix initial heading estimate |
shimniok | 1:cb84b477886c | 499 | //restartNav(); |
shimniok | 17:08f6ee55abbe | 500 | |
shimniok | 0:a6a169de725f | 501 | // Main loop |
shimniok | 0:a6a169de725f | 502 | // |
shimniok | 0:a6a169de725f | 503 | while(navDone == false) { |
shimniok | 0:a6a169de725f | 504 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 505 | // USER INPUT |
shimniok | 0:a6a169de725f | 506 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 507 | |
shimniok | 0:a6a169de725f | 508 | // Button state machine |
shimniok | 0:a6a169de725f | 509 | // if we've not started going, button starts us |
shimniok | 0:a6a169de725f | 510 | // if we have started going, button stops us |
shimniok | 0:a6a169de725f | 511 | // but only if we've released it first |
shimniok | 0:a6a169de725f | 512 | // |
shimniok | 0:a6a169de725f | 513 | // set throttle only if goGoGo set |
shimniok | 0:a6a169de725f | 514 | if (goGoGo) { |
shimniok | 17:08f6ee55abbe | 515 | // TODO: 2 Add additional condition of travel for N meters before the HALT button is armed |
shimniok | 0:a6a169de725f | 516 | |
shimniok | 0:a6a169de725f | 517 | if (keypad.pressed == true) { // && started |
shimniok | 17:08f6ee55abbe | 518 | fputs(">>>>>>>>>>>>>>>>>>>>>>> HALT\n", stdout); |
shimniok | 0:a6a169de725f | 519 | display.status("HALT."); |
shimniok | 0:a6a169de725f | 520 | navDone = true; |
shimniok | 0:a6a169de725f | 521 | goGoGo = false; |
shimniok | 0:a6a169de725f | 522 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 523 | endRun(); |
shimniok | 0:a6a169de725f | 524 | } |
shimniok | 0:a6a169de725f | 525 | } else { |
shimniok | 0:a6a169de725f | 526 | if (keypad.pressed == true) { |
shimniok | 17:08f6ee55abbe | 527 | fputs(">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n", stdout); |
shimniok | 0:a6a169de725f | 528 | display.status("GO GO GO!"); |
shimniok | 0:a6a169de725f | 529 | goGoGo = true; |
shimniok | 0:a6a169de725f | 530 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 531 | beginRun(); |
shimniok | 0:a6a169de725f | 532 | } |
shimniok | 0:a6a169de725f | 533 | } |
shimniok | 0:a6a169de725f | 534 | |
shimniok | 0:a6a169de725f | 535 | // Are we at the last waypoint? |
shimniok | 0:a6a169de725f | 536 | // |
shimniok | 17:08f6ee55abbe | 537 | if (fifo_first()->nextWaypoint == config.wptCount) { |
shimniok | 17:08f6ee55abbe | 538 | fputs("Arrived at final destination.\n", stdout); |
shimniok | 1:cb84b477886c | 539 | display.status("Arrived at end."); |
shimniok | 0:a6a169de725f | 540 | navDone = true; |
shimniok | 0:a6a169de725f | 541 | endRun(); |
shimniok | 0:a6a169de725f | 542 | } |
shimniok | 0:a6a169de725f | 543 | |
shimniok | 0:a6a169de725f | 544 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 17:08f6ee55abbe | 545 | // TELEMETRY |
shimniok | 17:08f6ee55abbe | 546 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 17:08f6ee55abbe | 547 | if (timer.read_ms() > nextTelemUpdate) { |
shimniok | 17:08f6ee55abbe | 548 | SystemState *s = fifo_first(); |
shimniok | 17:08f6ee55abbe | 549 | telem.sendPacket(s); // TODO 4 run this out of timer interrupt |
shimniok | 17:08f6ee55abbe | 550 | nextTelemUpdate += 200; // TODO 3 increase update speed |
shimniok | 17:08f6ee55abbe | 551 | } |
shimniok | 17:08f6ee55abbe | 552 | |
shimniok | 17:08f6ee55abbe | 553 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 554 | // LOGGING |
shimniok | 0:a6a169de725f | 555 | ////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 556 | // sensor reads are happening in the schedHandler(); |
shimniok | 1:cb84b477886c | 557 | // Are there more items to come out of the log fifo? |
shimniok | 0:a6a169de725f | 558 | // Since this could take anywhere from a few hundred usec to |
shimniok | 0:a6a169de725f | 559 | // 150ms, we run it opportunistically and use a buffer. That way |
shimniok | 0:a6a169de725f | 560 | // the sensor updates, calculation, and control can continue to happen |
shimniok | 17:08f6ee55abbe | 561 | if (fifo_available()) { |
shimniok | 0:a6a169de725f | 562 | logStatus = !logStatus; // log indicator LED |
shimniok | 17:08f6ee55abbe | 563 | logData( fifo_pull() ); // log state data to file |
shimniok | 0:a6a169de725f | 564 | logStatus = !logStatus; // log indicator LED |
shimniok | 0:a6a169de725f | 565 | } |
shimniok | 0:a6a169de725f | 566 | |
shimniok | 0:a6a169de725f | 567 | } // while |
shimniok | 0:a6a169de725f | 568 | closeLogfile(); |
shimniok | 1:cb84b477886c | 569 | wait(2.0); |
shimniok | 0:a6a169de725f | 570 | logStatus = 0; |
shimniok | 1:cb84b477886c | 571 | display.status("Completed. Saved."); |
shimniok | 1:cb84b477886c | 572 | wait(2.0); |
shimniok | 0:a6a169de725f | 573 | |
shimniok | 17:08f6ee55abbe | 574 | updaterStatus = 0; |
shimniok | 0:a6a169de725f | 575 | gpsStatus = 0; |
shimniok | 0:a6a169de725f | 576 | //confStatus = 0; |
shimniok | 0:a6a169de725f | 577 | //flasher = 0; |
shimniok | 0:a6a169de725f | 578 | |
shimniok | 0:a6a169de725f | 579 | sensors.gps.disableVerbose(); |
shimniok | 0:a6a169de725f | 580 | |
shimniok | 0:a6a169de725f | 581 | return 0; |
shimniok | 0:a6a169de725f | 582 | } // autonomousMode |
shimniok | 0:a6a169de725f | 583 | |
shimniok | 0:a6a169de725f | 584 | |
shimniok | 0:a6a169de725f | 585 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 586 | // UTILITY FUNCTIONS |
shimniok | 0:a6a169de725f | 587 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 588 | |
shimniok | 0:a6a169de725f | 589 | // Gather gyro data using turntable equipped with dual channel |
shimniok | 0:a6a169de725f | 590 | // encoder. Use onboard wheel encoder system. Left channel |
shimniok | 0:a6a169de725f | 591 | // is the index (0 degree) mark, while the right channel |
shimniok | 0:a6a169de725f | 592 | // is the incremental encoder. Can then compare gyro integrated |
shimniok | 0:a6a169de725f | 593 | // heading with machine-reported heading |
shimniok | 0:a6a169de725f | 594 | // |
shimniok | 0:a6a169de725f | 595 | int gyroSwing() |
shimniok | 0:a6a169de725f | 596 | { |
shimniok | 0:a6a169de725f | 597 | FILE *fp; |
shimniok | 17:08f6ee55abbe | 598 | int now; |
shimniok | 17:08f6ee55abbe | 599 | int next; |
shimniok | 17:08f6ee55abbe | 600 | int g[3]; |
shimniok | 17:08f6ee55abbe | 601 | int leftTotal=0; |
shimniok | 17:08f6ee55abbe | 602 | int rightTotal=0; |
shimniok | 0:a6a169de725f | 603 | |
shimniok | 0:a6a169de725f | 604 | // Timing is pretty critical so just in case, disable serial processing from GPS |
shimniok | 0:a6a169de725f | 605 | sensors.gps.disable(); |
shimniok | 17:08f6ee55abbe | 606 | stopUpdater(); |
shimniok | 0:a6a169de725f | 607 | |
shimniok | 17:08f6ee55abbe | 608 | fputs("Starting gyro swing...\n", stdout); |
shimniok | 0:a6a169de725f | 609 | display.status("Starting..."); |
shimniok | 17:08f6ee55abbe | 610 | // fp = openlog("gy"); |
shimniok | 17:08f6ee55abbe | 611 | fp = stdout; |
shimniok | 0:a6a169de725f | 612 | |
shimniok | 17:08f6ee55abbe | 613 | display.status("Rotate clockwise."); |
shimniok | 17:08f6ee55abbe | 614 | fputs("Begin clockwise rotation, varying rpm\n", stdout); |
shimniok | 17:08f6ee55abbe | 615 | wait(1); |
shimniok | 17:08f6ee55abbe | 616 | |
shimniok | 17:08f6ee55abbe | 617 | display.status("Select exits."); |
shimniok | 17:08f6ee55abbe | 618 | fputs("Press select to exit\n", stdout); |
shimniok | 17:08f6ee55abbe | 619 | wait(1); |
shimniok | 17:08f6ee55abbe | 620 | |
shimniok | 0:a6a169de725f | 621 | |
shimniok | 0:a6a169de725f | 622 | timer.reset(); |
shimniok | 0:a6a169de725f | 623 | timer.start(); |
shimniok | 0:a6a169de725f | 624 | |
shimniok | 17:08f6ee55abbe | 625 | next = now = timer.read_ms(); |
shimniok | 17:08f6ee55abbe | 626 | |
shimniok | 0:a6a169de725f | 627 | sensors._right.read(); // easiest way to reset the heading counter |
shimniok | 17:08f6ee55abbe | 628 | sensors._left.read(); |
shimniok | 0:a6a169de725f | 629 | |
shimniok | 0:a6a169de725f | 630 | while (1) { |
shimniok | 17:08f6ee55abbe | 631 | now = timer.read_ms(); |
shimniok | 17:08f6ee55abbe | 632 | |
shimniok | 0:a6a169de725f | 633 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 634 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 635 | break; |
shimniok | 0:a6a169de725f | 636 | } |
shimniok | 0:a6a169de725f | 637 | |
shimniok | 17:08f6ee55abbe | 638 | if (now >= next) { |
shimniok | 17:08f6ee55abbe | 639 | leftTotal += sensors._left.read(); |
shimniok | 17:08f6ee55abbe | 640 | rightTotal += sensors._right.read(); |
shimniok | 17:08f6ee55abbe | 641 | sensors._gyro.read(g); |
shimniok | 17:08f6ee55abbe | 642 | fputs(cvitos(now), fp); |
shimniok | 17:08f6ee55abbe | 643 | fputs(" ", fp); |
shimniok | 17:08f6ee55abbe | 644 | fputs(cvntos(leftTotal), fp); |
shimniok | 17:08f6ee55abbe | 645 | fputs(" ", fp); |
shimniok | 17:08f6ee55abbe | 646 | fputs(cvntos(rightTotal), fp); |
shimniok | 17:08f6ee55abbe | 647 | fputs(" ", fp); |
shimniok | 17:08f6ee55abbe | 648 | fputs(cvitos(g[_z_]), fp); |
shimniok | 17:08f6ee55abbe | 649 | fputs("\n", fp); |
shimniok | 17:08f6ee55abbe | 650 | next = now + 50; |
shimniok | 17:08f6ee55abbe | 651 | } |
shimniok | 0:a6a169de725f | 652 | } |
shimniok | 17:08f6ee55abbe | 653 | if (fp && fp != stdout) { |
shimniok | 0:a6a169de725f | 654 | fclose(fp); |
shimniok | 0:a6a169de725f | 655 | display.status("Done. Saved."); |
shimniok | 17:08f6ee55abbe | 656 | fputs("Data collection complete.\n", stdout); |
shimniok | 0:a6a169de725f | 657 | wait(2); |
shimniok | 0:a6a169de725f | 658 | } |
shimniok | 17:08f6ee55abbe | 659 | |
shimniok | 17:08f6ee55abbe | 660 | sensors.gps.enable(); |
shimniok | 17:08f6ee55abbe | 661 | restartNav(); |
shimniok | 17:08f6ee55abbe | 662 | startUpdater(); |
shimniok | 0:a6a169de725f | 663 | |
shimniok | 0:a6a169de725f | 664 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 665 | |
shimniok | 0:a6a169de725f | 666 | return 0; |
shimniok | 0:a6a169de725f | 667 | } |
shimniok | 0:a6a169de725f | 668 | |
shimniok | 0:a6a169de725f | 669 | |
shimniok | 0:a6a169de725f | 670 | |
shimniok | 0:a6a169de725f | 671 | void bridgeRecv() |
shimniok | 0:a6a169de725f | 672 | { |
shimniok | 17:08f6ee55abbe | 673 | #if 0 |
shimniok | 0:a6a169de725f | 674 | while (dev && dev->readable()) { |
shimniok | 0:a6a169de725f | 675 | pc.putc(dev->getc()); |
shimniok | 0:a6a169de725f | 676 | } |
shimniok | 17:08f6ee55abbe | 677 | #endif |
shimniok | 0:a6a169de725f | 678 | } |
shimniok | 0:a6a169de725f | 679 | |
shimniok | 17:08f6ee55abbe | 680 | |
shimniok | 0:a6a169de725f | 681 | void serialBridge(Serial &serial) |
shimniok | 0:a6a169de725f | 682 | { |
shimniok | 17:08f6ee55abbe | 683 | #if 0 |
shimniok | 0:a6a169de725f | 684 | char x; |
shimniok | 0:a6a169de725f | 685 | int count = 0; |
shimniok | 0:a6a169de725f | 686 | bool done=false; |
shimniok | 0:a6a169de725f | 687 | |
shimniok | 17:08f6ee55abbe | 688 | fputs("\nEntering serial bridge in 2 seconds, +++ to escape\n\n", stdout); |
shimniok | 0:a6a169de725f | 689 | sensors.gps.enableVerbose(); |
shimniok | 0:a6a169de725f | 690 | wait(2.0); |
shimniok | 0:a6a169de725f | 691 | //dev = &gps; |
shimniok | 2:fbc6e3cf3ed8 | 692 | sensors.gps.disable(); |
shimniok | 17:08f6ee55abbe | 693 | serial.baud(38400); |
shimniok | 0:a6a169de725f | 694 | while (!done) { |
shimniok | 0:a6a169de725f | 695 | if (pc.readable()) { |
shimniok | 0:a6a169de725f | 696 | x = pc.getc(); |
shimniok | 0:a6a169de725f | 697 | serial.putc(x); |
shimniok | 0:a6a169de725f | 698 | // escape sequence |
shimniok | 0:a6a169de725f | 699 | if (x == '+') { |
shimniok | 0:a6a169de725f | 700 | if (++count >= 3) done=true; |
shimniok | 0:a6a169de725f | 701 | } else { |
shimniok | 0:a6a169de725f | 702 | count = 0; |
shimniok | 0:a6a169de725f | 703 | } |
shimniok | 0:a6a169de725f | 704 | } |
shimniok | 0:a6a169de725f | 705 | if (serial.readable()) { |
shimniok | 17:08f6ee55abbe | 706 | fputc(serial.getc(), stdout); |
shimniok | 0:a6a169de725f | 707 | } |
shimniok | 0:a6a169de725f | 708 | } |
shimniok | 17:08f6ee55abbe | 709 | #endif |
shimniok | 0:a6a169de725f | 710 | } |
shimniok | 0:a6a169de725f | 711 | |
shimniok | 0:a6a169de725f | 712 | |
shimniok | 0:a6a169de725f | 713 | int setBacklight(void) { |
shimniok | 0:a6a169de725f | 714 | Menu bmenu; |
shimniok | 0:a6a169de725f | 715 | bool done = false; |
shimniok | 0:a6a169de725f | 716 | bool printUpdate = false; |
shimniok | 0:a6a169de725f | 717 | static int backlight=100; |
shimniok | 0:a6a169de725f | 718 | |
shimniok | 0:a6a169de725f | 719 | display.select(">> Backlight"); |
shimniok | 0:a6a169de725f | 720 | |
shimniok | 0:a6a169de725f | 721 | while (!done) { |
shimniok | 0:a6a169de725f | 722 | if (keypad.pressed) { |
shimniok | 0:a6a169de725f | 723 | keypad.pressed = false; |
shimniok | 0:a6a169de725f | 724 | printUpdate = true; |
shimniok | 0:a6a169de725f | 725 | switch (keypad.which) { |
shimniok | 0:a6a169de725f | 726 | case NEXT_BUTTON: |
shimniok | 0:a6a169de725f | 727 | backlight+=5; |
shimniok | 0:a6a169de725f | 728 | if (backlight > 100) backlight = 100; |
shimniok | 0:a6a169de725f | 729 | lcd.backlight(backlight); |
shimniok | 0:a6a169de725f | 730 | break; |
shimniok | 0:a6a169de725f | 731 | case PREV_BUTTON: |
shimniok | 0:a6a169de725f | 732 | backlight-=5; |
shimniok | 0:a6a169de725f | 733 | if (backlight < 0) backlight = 0; |
shimniok | 0:a6a169de725f | 734 | lcd.backlight(backlight); |
shimniok | 0:a6a169de725f | 735 | break; |
shimniok | 0:a6a169de725f | 736 | case SELECT_BUTTON: |
shimniok | 0:a6a169de725f | 737 | done = true; |
shimniok | 0:a6a169de725f | 738 | break; |
shimniok | 0:a6a169de725f | 739 | } |
shimniok | 0:a6a169de725f | 740 | } |
shimniok | 0:a6a169de725f | 741 | if (printUpdate) { |
shimniok | 0:a6a169de725f | 742 | printUpdate = false; |
shimniok | 0:a6a169de725f | 743 | lcd.pos(0,1); |
shimniok | 17:08f6ee55abbe | 744 | // TODO 3 lcd.printf("%3d%%%-16s", backlight, ""); |
shimniok | 0:a6a169de725f | 745 | } |
shimniok | 0:a6a169de725f | 746 | } |
shimniok | 0:a6a169de725f | 747 | |
shimniok | 0:a6a169de725f | 748 | return 0; |
shimniok | 0:a6a169de725f | 749 | } |
shimniok | 0:a6a169de725f | 750 | |
shimniok | 0:a6a169de725f | 751 | int reverseScreen(void) { |
shimniok | 0:a6a169de725f | 752 | lcd.reverseMode(); |
shimniok | 0:a6a169de725f | 753 | |
shimniok | 0:a6a169de725f | 754 | return 0; |
shimniok | 0:a6a169de725f | 755 | } |
shimniok | 0:a6a169de725f | 756 | |
shimniok | 0:a6a169de725f | 757 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 758 | // ADC CONVERSION FUNCTIONS |
shimniok | 0:a6a169de725f | 759 | /////////////////////////////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:a6a169de725f | 760 | |
shimniok | 0:a6a169de725f | 761 | // returns distance in m for Sharp GP2YOA710K0F |
shimniok | 0:a6a169de725f | 762 | // to get m and b, I wrote down volt vs. dist by eyeballin the |
shimniok | 0:a6a169de725f | 763 | // datasheet chart plot. Then used Excel to do linear regression |
shimniok | 0:a6a169de725f | 764 | // |
shimniok | 17:08f6ee55abbe | 765 | float irDistance(const unsigned int adc) |
shimniok | 0:a6a169de725f | 766 | { |
shimniok | 0:a6a169de725f | 767 | float b = 1.0934; // Intercept from Excel |
shimniok | 0:a6a169de725f | 768 | float m = 1.4088; // Slope from Excel |
shimniok | 0:a6a169de725f | 769 | |
shimniok | 0:a6a169de725f | 770 | return m / (((float) adc) * 4.95/4096 - b); |
shimniok | 0:a6a169de725f | 771 | } |