![](/media/cache/profiles/2012_sf_avc_095.jpg.50x50_q85.jpg)
2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
main.cpp
- Committer:
- shimniok
- Date:
- 2018-12-17
- Revision:
- 22:4d62bd16f037
- Parent:
- 21:020f4ebbb13a
- Child:
- 23:5e61cf4a8c34
File content as of revision 22:4d62bd16f037:
/* 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 "SimpleShell.h" #include "Config.h" #include "Updater.h" #include "Ublox6.h" Serial pc(USBTX, USBRX); //SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS //FATFileSystem ffs("log", &bd); LocalFileSystem lfs("etc"); Config config; SimpleShell sh; Ublox6 ublox; DigitalOut led1(LED1); DigitalOut led3(LED3); RawSerial s(UART1TX, UART1RX, 38400); EventQueue gpsQueue(32 * EVENTS_EVENT_SIZE); // thread / eventqueue wrapper for gps parse void gps_parse(int c) { if (ublox.parse(c)) { led3 = !led3; } } // ISR for GPS serial, passes off to thread void gps_handler() { while (s.readable()) { int c = s.getc(); //gpsQueue.call(gps_parse, c); gpsQueue.call(&ublox, &Ublox6::parse, c); } } /******** SHELL COMMANDS ********/ void test() { printf("Hello world!\n"); } void read_gps() { 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_gyro() { int g[3]; float dt; Updater *u = Updater::instance(); u->gyro(g, dt); printf("Gyro: %d, %d, %d - dt: %f\n", g[0], g[1], g[2], dt); } void read_enc() { Updater *u = Updater::instance(); printf("Encoder: %d\n", u->encoder()); } void bridge_uart1() { RawSerial s(UART1TX, UART1RX, 38400); while (1) { if (pc.readable()) s.putc(pc.getc()); if (s.readable()) pc.putc(s.getc()); } } void reset() { NVIC_SystemReset(); } /******** MAIN ********/ // main() runs in its own thread in the OS int main() { //bridge_uart1(); printf("Bootup...\n"); fflush(stdout); 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 shell...\n"); sh.attach(test, "test"); sh.attach(read_gyro, "gyro"); sh.attach(read_enc, "enc"); sh.attach(read_gps, "gps"); sh.attach(reset, "reset"); Thread shellThread; shellThread.start(callback(&sh, &SimpleShell::run)); printf("Starting updater...\n"); Updater *u = Updater::instance(); u->setInterval(20); Thread updaterThread(osPriorityRealtime); updaterThread.start(callback(u, &Updater::start)); printf("Starting gps...\n"); Thread gpsThread(osPriorityHigh); gpsThread.start(callback(&gpsQueue, &EventQueue::dispatch_forever)); s.attach(gps_handler); /* FILE *fp; char buf[128]; printf("Initializing the block device... "); fflush(stdout); int err = bd.init(); printf("%s\n", (err ? "Fail :(" : "OK")); printf("Opening sdtest.txt..."); fp = fopen("/log/sdtest.txt", "r"); if(fp) { while (!feof(fp)) { fgets(buf, 127, fp); printf(buf); } fclose(fp); } printf("Opening config.txt..."); fp = fopen("/etc/config.txt", "r"); if(fp) { while (!feof(fp)) { fgets(buf, 127, fp); printf(buf); } fclose(fp); } */ //SystemReport sys_state(500); while (true) { // Blink LED and wait 0.5 seconds led1 = !led1; wait(0.5f); // Following the main thread wait, report on the current system status //sys_state.report_state(); } }