Michael Shimniok / Mbed 2 deprecated DataBus

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?

UserRevisionLine numberNew 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 }