2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
main.cpp
- Committer:
- shimniok
- Date:
- 2018-12-31
- Revision:
- 35:c42e7e29c3bd
- Parent:
- 33:74c514aea0a1
- Child:
- 37:b8259500dbd3
File content as of revision 35:c42e7e29c3bd:
/* mbed Microcontroller Library * Copyright (c) 2018 ARM Limited * SPDX-License-Identifier: Apache-2.0 */ #include "mbed.h" #include <stdio.h> #include <errno.h> #include "pinouts.h" #include "stats_report.h" #include "SDBlockDevice.h" #include "FATFileSystem.h" #include "Servo.h" #include "SimpleShell.h" #include "Config.h" #include "Updater.h" #include "Ublox6.h" #include "Logger.h" #include "SystemState.h" /////////////////////////////////////////////////////////////////////////////// // Config Config config; /////////////////////////////////////////////////////////////////////////////// // Devices DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); LocalFileSystem lfs("etc"); SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS FATFileSystem ffs("log", &bd); Serial pc(USBTX, USBRX); RawSerial s(UART1TX, UART1RX, 38400); InterruptIn lbutton(LBUTTON, PullUp); // button interrupts InterruptIn cbutton(CBUTTON, PullUp); InterruptIn rbutton(RBUTTON, PullUp); Servo steer(STEERING); Servo esc(THROTTLE); /////////////////////////////////////////////////////////////////////////////// // Idle hook void idler() { while(1) { led1 = !led1; wait(0.1); } } /////////////////////////////////////////////////////////////////////////////// // Logging EventQueue logQueue(16 * EVENTS_EVENT_SIZE); Logger logger; /////////////////////////////////////////////////////////////////////////////// // Updater Updater *u = Updater::instance(); EventQueue updaterQueue(8 * EVENTS_EVENT_SIZE); void updater_callback() { SensorData d; float dt; led1 = !led1; d.timestamp = Kernel::get_ms_count(); d.encoder = u->encoder(); u->imu(d.gyro, d.accel, d.mag, dt); logQueue.call(&logger, &Logger::log_sensors, d); } /////////////////////////////////////////////////////////////////////////////// // Buttons //enum button_mask { LEFT = 0x01, CENTER = 0x02, RIGHT = 0x04 }; EventQueue buttonQueue(16 * EVENTS_EVENT_SIZE); const int BUTTON_DEBOUNCE_SAMPLES = 20; const int BUTTON_DEBOUNCE_THRESH = 8; const int BUTTON_SAMPLE_MS = 0.005; void button_event() { int samples = 0; // disable subsequent interrupts lbutton.fall(NULL); // sample repeatedly to debounce for (int i=0; i < BUTTON_DEBOUNCE_SAMPLES; i++) { if (lbutton == 0) samples++; wait(BUTTON_SAMPLE_MS); } if (samples > BUTTON_DEBOUNCE_THRESH) { if (logger.enabled()) { logQueue.call(&logger, &Logger::stop); led3 = 0; } else { logQueue.call(&logger, &Logger::start); led3 = 1; } } lbutton.fall(buttonQueue.event(button_event)); } /////////////////////////////////////////////////////////////////////////////// // GPS Ublox6 ublox; EventQueue gpsQueue(8 * EVENTS_EVENT_SIZE); // Callback for gps parse data ready void gps_callback() { GpsData d; led2 = !led2; ublox.read(d.latitude, d.longitude, d.course, d.speed, d.hdop, d.svcount); d.timestamp = Kernel::get_ms_count(); logQueue.call(&logger, &Logger::log_gps, d); } // ISR for GPS serial, passes off to thread void gps_handler() { while (s.readable()) { int c = s.getc(); gpsQueue.call(&ublox, &Ublox6::parse, c); } } /////////////////////////////////////////////////////////////////////////////// // Shell SimpleShell sh("/log"); void test(int argc, char **argv) { //printf("Hello world!\n"); char file[32]; for (int i=30; i < 40; i++) { sprintf(file, "/log/%04d.csv", i); printf("removing <%s>\n", file); int stat = remove(file); printf("return: %d\n", stat); } } void stats(int argc, char **argv) { SystemReport r(200); r.report_state(); return; } void read_gps(int argc, char **argv) { double lat=0; double lon=0; float course=0; float speed=0; float hdop=0.0; int svcount=0; ublox.read(lat, lon, course, speed, hdop, svcount); printf("%3.7f %3.7f\n", lat, lon); printf("hdg=%03.1f deg spd=%3.1f m/s\n", course, speed); printf("%d %f\n", svcount, hdop); } void read_imu(int argc, char **argv) { int g[3]; int a[3]; int m[3]; float dt; Updater *u = Updater::instance(); u->imu(g, a, m, dt); printf(" Gyro: %d, %d, %d\n", g[0], g[1], g[2]); printf(" Accel: %d, %d, %d\n", a[0], a[1], a[2]); printf(" Mag: %d, %d, %d\n", m[0], m[1], m[2]); printf(" dt: %f\n", dt); } void read_enc(int argc, char **argv) { Updater *u = Updater::instance(); printf("Encoder: %d\n", u->encoder()); } void log(int argc, char **argv) { char *usage = "usage: log [start|stop]"; if (argc == 1) { printf("logging "); if (logger.enabled()) printf("enabled"); else printf("disabled"); printf("\n"); } else if (argc == 2) { if (!strcmp("start", argv[1])) { logger.start(); } else if (!strcmp("stop", argv[1])) { logger.stop(); } else { puts(usage); } } else { puts(usage); } } void bridge_uart1(int argc, char **argv) { RawSerial s(UART1TX, UART1RX, 38400); while (1) { if (pc.readable()) s.putc(pc.getc()); if (s.readable()) pc.putc(s.getc()); } } void reset(int argc, char **argv) { NVIC_SystemReset(); } /////////////////////////////////////////////////////////////////////////////// // MAIN // main() runs in its own thread in the OS int main() { //bridge_uart1(); //Kernel::attach_idle_hook(idler); printf("Bootup...\n"); fflush(stdout); steer = 0.50; esc = 0.00; printf("Loading config...\n"); config.add("intercept_distance", Config::DOUBLE); config.add("waypoint_threshold", Config::DOUBLE); config.add("minimum_turning_radius", Config::DOUBLE); config.add("wheelbase", Config::DOUBLE); config.add("track_width", Config::DOUBLE); config.add("tire_circumference", Config::DOUBLE); config.add("encoder_stripes", Config::INT); config.add("esc_brake", Config::INT); config.add("esc_off", Config::INT); config.add("esc_max", Config::INT); config.add("turn_speed", Config::DOUBLE); config.add("turn_distance", Config::DOUBLE); config.add("start_speed", Config::DOUBLE); config.add("cruise_speed", Config::DOUBLE); config.add("speed_kp", Config::DOUBLE); config.add("speed_ki", Config::DOUBLE); config.add("speed_kd", Config::DOUBLE); config.add("steer_center", Config::DOUBLE); config.add("steer_scale", Config::DOUBLE); config.add("gyro_scale", Config::DOUBLE); config.add("gps_valid_speed", Config::DOUBLE); if (config.load("/etc/2018cfg.txt")) { printf("error loading config\n"); } printf("Starting buttons...\n"); lbutton.fall(buttonQueue.event(button_event)); //cbutton.rise(buttonQueue.event(button_event)); //rbutton.rise(buttonQueue.event(button_event)); Thread buttonThread(osPriorityNormal, 256, 0, "buttons"); buttonThread.start(callback(&buttonQueue, &EventQueue::dispatch_forever)); printf("Starting updater...\n"); u->attach(updater_callback); Thread updaterThread(osPriorityRealtime, 512, 0, "updater"); updaterQueue.call_every(20, u, &Updater::update); updaterThread.start(callback(&updaterQueue, &EventQueue::dispatch_forever)); printf("Starting gps...\n"); Thread gpsThread(osPriorityHigh, 2048, 0, "gps"); gpsThread.start(callback(&gpsQueue, &EventQueue::dispatch_forever)); ublox.subscribe(gps_callback); s.attach(gps_handler); printf("Starting logging...\n"); Thread logThread(osPriorityNormal, 2048, 0, "log"); logThread.start(callback(&logQueue, &EventQueue::dispatch_forever)); printf("Starting shell...\n"); sh.command(test, "test"); sh.command(read_imu, "imu"); sh.command(read_enc, "enc"); sh.command(read_gps, "gps"); sh.command(reset, "reset"); sh.command(stats, "stats"); sh.command(log, "log"); sh.run(); while (true) { // Blink LED and wait 0.5 seconds led1 = !led1; wait(0.5f); } }