File content as of revision 15:35c40765f7c3:

/* mbed Microcontroller Library
 * Copyright (c) 2018 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include <stdio.h>
#include <errno.h>
#include "stats_report.h"
#include "SDBlockDevice.h"
#include "FATFileSystem.h"
#include "SimpleShell.h"
#include "Config.h"
#include "Updater.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;

DigitalOut led1(LED1);

/******** SHELL COMMANDS ********/

void test() {
    printf("Hello world!\n");
}

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 reset()
{
    NVIC_SystemReset();
}

/******** MAIN ********/

// main() runs in its own thread in the OS
int main()
{   
    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(reset, "reset");
    Thread shellThread;
    shellThread.start(callback(&sh, &SimpleShell::run));

    printf("Starting updater...\n");
    Updater *u = Updater::instance();
    u->setInterval(50);
    Thread updaterThread;
    updaterThread.set_priority(osPriorityRealtime);
    updaterThread.start(callback(u, &Updater::start));
    
/*
    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();
    }
}