Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

main.cpp@18:3f8a8f6e3cc1, 2018-12-14 (annotated)

Committer:
shimniok
Date:
Fri Dec 14 00:36:06 2018 +0000
Revision:
18:3f8a8f6e3cc1
Parent:
16:eb28d0f64a9b
Child:
19:0d1728091519
add cmd to read gps; fixed gps flag bug, changed gps interface

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:7e98bbfd102a 1 /* mbed Microcontroller Library
shimniok 0:7e98bbfd102a 2 * Copyright (c) 2018 ARM Limited
shimniok 0:7e98bbfd102a 3 * SPDX-License-Identifier: Apache-2.0
shimniok 0:7e98bbfd102a 4 */
shimniok 0:7e98bbfd102a 5
shimniok 0:7e98bbfd102a 6 #include "mbed.h"
shimniok 0:7e98bbfd102a 7 #include <stdio.h>
shimniok 0:7e98bbfd102a 8 #include <errno.h>
shimniok 16:eb28d0f64a9b 9 #include "pinouts.h"
shimniok 0:7e98bbfd102a 10 #include "stats_report.h"
shimniok 0:7e98bbfd102a 11 #include "SDBlockDevice.h"
shimniok 0:7e98bbfd102a 12 #include "FATFileSystem.h"
shimniok 0:7e98bbfd102a 13 #include "SimpleShell.h"
shimniok 4:de7feb458652 14 #include "Config.h"
shimniok 11:8ec858b7c6d1 15 #include "Updater.h"
shimniok 16:eb28d0f64a9b 16 #include "Ublox6.h"
shimniok 0:7e98bbfd102a 17
shimniok 0:7e98bbfd102a 18 Serial pc(USBTX, USBRX);
shimniok 0:7e98bbfd102a 19 //SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
shimniok 0:7e98bbfd102a 20 //FATFileSystem ffs("log", &bd);
shimniok 0:7e98bbfd102a 21 LocalFileSystem lfs("etc");
shimniok 4:de7feb458652 22 Config config;
shimniok 0:7e98bbfd102a 23 SimpleShell sh;
shimniok 18:3f8a8f6e3cc1 24 Ublox6 ublox;
shimniok 0:7e98bbfd102a 25
shimniok 0:7e98bbfd102a 26 DigitalOut led1(LED1);
shimniok 16:eb28d0f64a9b 27 DigitalOut led3(LED3);
shimniok 16:eb28d0f64a9b 28
shimniok 16:eb28d0f64a9b 29 void do_gps() {
shimniok 16:eb28d0f64a9b 30 RawSerial s(UART1TX, UART1RX, 38400);
shimniok 16:eb28d0f64a9b 31 while (1) {
shimniok 16:eb28d0f64a9b 32 int c = s.getc();
shimniok 16:eb28d0f64a9b 33 //int c = 'a';
shimniok 16:eb28d0f64a9b 34 ublox.parse(c);
shimniok 16:eb28d0f64a9b 35 led3 = !led3;
shimniok 16:eb28d0f64a9b 36 }
shimniok 16:eb28d0f64a9b 37 }
shimniok 16:eb28d0f64a9b 38
shimniok 0:7e98bbfd102a 39
shimniok 9:fc3575d2cbbf 40 /******** SHELL COMMANDS ********/
shimniok 9:fc3575d2cbbf 41
shimniok 1:7019a60fd585 42 void test() {
shimniok 1:7019a60fd585 43 printf("Hello world!\n");
shimniok 1:7019a60fd585 44 }
shimniok 1:7019a60fd585 45
shimniok 18:3f8a8f6e3cc1 46 void read_gps()
shimniok 18:3f8a8f6e3cc1 47 {
shimniok 18:3f8a8f6e3cc1 48 double lat=0;
shimniok 18:3f8a8f6e3cc1 49 double lon=0;
shimniok 18:3f8a8f6e3cc1 50 float course=0;
shimniok 18:3f8a8f6e3cc1 51 float speed=0;
shimniok 18:3f8a8f6e3cc1 52 float hdop=0.0;
shimniok 18:3f8a8f6e3cc1 53 int svcount=0;
shimniok 18:3f8a8f6e3cc1 54
shimniok 18:3f8a8f6e3cc1 55 ublox.read(lat, lon, course, speed, hdop, svcount);
shimniok 18:3f8a8f6e3cc1 56 printf("%3.7f %3.7f\n", lat, lon);
shimniok 18:3f8a8f6e3cc1 57 printf("hdg=%03.1f deg spd=%3.1f m/s\n", course, speed);
shimniok 18:3f8a8f6e3cc1 58 printf("%d %f\n", svcount, hdop);
shimniok 18:3f8a8f6e3cc1 59 }
shimniok 18:3f8a8f6e3cc1 60
shimniok 9:fc3575d2cbbf 61 void read_gyro()
shimniok 9:fc3575d2cbbf 62 {
shimniok 12:3cd91e150d9c 63 int g[3];
shimniok 13:5566df1250f1 64 float dt;
shimniok 12:3cd91e150d9c 65
shimniok 11:8ec858b7c6d1 66 Updater *u = Updater::instance();
shimniok 11:8ec858b7c6d1 67
shimniok 13:5566df1250f1 68 u->gyro(g, dt);
shimniok 11:8ec858b7c6d1 69
shimniok 13:5566df1250f1 70 printf("Gyro: %d, %d, %d - dt: %f\n", g[0], g[1], g[2], dt);
shimniok 12:3cd91e150d9c 71 }
shimniok 12:3cd91e150d9c 72
shimniok 14:1dd83e626153 73 void read_enc()
shimniok 14:1dd83e626153 74 {
shimniok 14:1dd83e626153 75 Updater *u = Updater::instance();
shimniok 14:1dd83e626153 76
shimniok 14:1dd83e626153 77 printf("Encoder: %d\n", u->encoder());
shimniok 14:1dd83e626153 78 }
shimniok 14:1dd83e626153 79
shimniok 12:3cd91e150d9c 80 void reset()
shimniok 12:3cd91e150d9c 81 {
shimniok 12:3cd91e150d9c 82 NVIC_SystemReset();
shimniok 9:fc3575d2cbbf 83 }
shimniok 9:fc3575d2cbbf 84
shimniok 9:fc3575d2cbbf 85 /******** MAIN ********/
shimniok 1:7019a60fd585 86
shimniok 0:7e98bbfd102a 87 // main() runs in its own thread in the OS
shimniok 0:7e98bbfd102a 88 int main()
shimniok 11:8ec858b7c6d1 89 {
shimniok 0:7e98bbfd102a 90 printf("Bootup...\n");
shimniok 0:7e98bbfd102a 91 fflush(stdout);
shimniok 1:7019a60fd585 92
shimniok 4:de7feb458652 93 printf("Loading config...\n");
shimniok 7:1f2661b840ed 94 config.add("intercept_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 95 config.add("waypoint_threshold", Config::DOUBLE);
shimniok 7:1f2661b840ed 96 config.add("minimum_turning_radius", Config::DOUBLE);
shimniok 7:1f2661b840ed 97 config.add("wheelbase", Config::DOUBLE);
shimniok 7:1f2661b840ed 98 config.add("track_width", Config::DOUBLE);
shimniok 7:1f2661b840ed 99 config.add("tire_circumference", Config::DOUBLE);
shimniok 7:1f2661b840ed 100 config.add("encoder_stripes", Config::INT);
shimniok 7:1f2661b840ed 101 config.add("esc_brake", Config::INT);
shimniok 7:1f2661b840ed 102 config.add("esc_off", Config::INT);
shimniok 7:1f2661b840ed 103 config.add("esc_max", Config::INT);
shimniok 7:1f2661b840ed 104 config.add("turn_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 105 config.add("turn_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 106 config.add("start_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 107 config.add("cruise_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 108 config.add("speed_kp", Config::DOUBLE);
shimniok 7:1f2661b840ed 109 config.add("speed_ki", Config::DOUBLE);
shimniok 7:1f2661b840ed 110 config.add("speed_kd", Config::DOUBLE);
shimniok 7:1f2661b840ed 111 config.add("steer_center", Config::DOUBLE);
shimniok 7:1f2661b840ed 112 config.add("steer_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 113 config.add("gyro_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 114 config.add("gps_valid_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 115
shimniok 4:de7feb458652 116 if (config.load("/etc/2018cfg.txt")) {
shimniok 4:de7feb458652 117 printf("error loading config\n");
shimniok 4:de7feb458652 118 }
shimniok 10:9fb3feb38746 119
shimniok 13:5566df1250f1 120 printf("Starting shell...\n");
shimniok 1:7019a60fd585 121 sh.attach(test, "test");
shimniok 9:fc3575d2cbbf 122 sh.attach(read_gyro, "gyro");
shimniok 14:1dd83e626153 123 sh.attach(read_enc, "enc");
shimniok 18:3f8a8f6e3cc1 124 sh.attach(read_gps, "gps");
shimniok 12:3cd91e150d9c 125 sh.attach(reset, "reset");
shimniok 15:35c40765f7c3 126 Thread shellThread;
shimniok 15:35c40765f7c3 127 shellThread.start(callback(&sh, &SimpleShell::run));
shimniok 0:7e98bbfd102a 128
shimniok 13:5566df1250f1 129 printf("Starting updater...\n");
shimniok 11:8ec858b7c6d1 130 Updater *u = Updater::instance();
shimniok 15:35c40765f7c3 131 u->setInterval(50);
shimniok 15:35c40765f7c3 132 Thread updaterThread;
shimniok 15:35c40765f7c3 133 updaterThread.set_priority(osPriorityRealtime);
shimniok 15:35c40765f7c3 134 updaterThread.start(callback(u, &Updater::start));
shimniok 16:eb28d0f64a9b 135
shimniok 16:eb28d0f64a9b 136 printf("Starting gps...\n");
shimniok 16:eb28d0f64a9b 137 Thread gpsThread;
shimniok 16:eb28d0f64a9b 138 gpsThread.start(&do_gps);
shimniok 10:9fb3feb38746 139
shimniok 0:7e98bbfd102a 140 /*
shimniok 0:7e98bbfd102a 141 FILE *fp;
shimniok 0:7e98bbfd102a 142 char buf[128];
shimniok 0:7e98bbfd102a 143 printf("Initializing the block device... ");
shimniok 0:7e98bbfd102a 144 fflush(stdout);
shimniok 0:7e98bbfd102a 145 int err = bd.init();
shimniok 0:7e98bbfd102a 146 printf("%s\n", (err ? "Fail :(" : "OK"));
shimniok 0:7e98bbfd102a 147
shimniok 0:7e98bbfd102a 148 printf("Opening sdtest.txt...");
shimniok 0:7e98bbfd102a 149 fp = fopen("/log/sdtest.txt", "r");
shimniok 0:7e98bbfd102a 150 if(fp) {
shimniok 0:7e98bbfd102a 151 while (!feof(fp)) {
shimniok 0:7e98bbfd102a 152 fgets(buf, 127, fp);
shimniok 0:7e98bbfd102a 153 printf(buf);
shimniok 0:7e98bbfd102a 154 }
shimniok 0:7e98bbfd102a 155 fclose(fp);
shimniok 0:7e98bbfd102a 156 }
shimniok 0:7e98bbfd102a 157
shimniok 0:7e98bbfd102a 158 printf("Opening config.txt...");
shimniok 0:7e98bbfd102a 159 fp = fopen("/etc/config.txt", "r");
shimniok 0:7e98bbfd102a 160 if(fp) {
shimniok 0:7e98bbfd102a 161 while (!feof(fp)) {
shimniok 0:7e98bbfd102a 162 fgets(buf, 127, fp);
shimniok 0:7e98bbfd102a 163 printf(buf);
shimniok 0:7e98bbfd102a 164 }
shimniok 0:7e98bbfd102a 165 fclose(fp);
shimniok 0:7e98bbfd102a 166 }
shimniok 0:7e98bbfd102a 167 */
shimniok 0:7e98bbfd102a 168
shimniok 0:7e98bbfd102a 169 //SystemReport sys_state(500);
shimniok 0:7e98bbfd102a 170
shimniok 0:7e98bbfd102a 171 while (true) {
shimniok 0:7e98bbfd102a 172 // Blink LED and wait 0.5 seconds
shimniok 0:7e98bbfd102a 173 led1 = !led1;
shimniok 0:7e98bbfd102a 174 wait(0.5f);
shimniok 0:7e98bbfd102a 175
shimniok 0:7e98bbfd102a 176 // Following the main thread wait, report on the current system status
shimniok 0:7e98bbfd102a 177 //sys_state.report_state();
shimniok 0:7e98bbfd102a 178 }
shimniok 0:7e98bbfd102a 179 }