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@20:1c2067937065, 2018-11-29 (annotated)

Committer:
shimniok
Date:
Thu Nov 29 18:08:05 2018 +0000
Revision:
20:1c2067937065
Parent:
17:08f6ee55abbe
Child:
22:dc54ca6e6eec
fixed main compile errors; updated Display

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