Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

main.cpp@10:9fb3feb38746, 2018-12-10 (annotated)

Committer:
shimniok
Date:
Mon Dec 10 20:12:41 2018 +0000
Revision:
10:9fb3feb38746
Parent:
9:fc3575d2cbbf
Child:
11:8ec858b7c6d1
Implemented periodic gyro and dt update

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 0:7e98bbfd102a 9 #include "stats_report.h"
shimniok 0:7e98bbfd102a 10 #include "SDBlockDevice.h"
shimniok 0:7e98bbfd102a 11 #include "FATFileSystem.h"
shimniok 0:7e98bbfd102a 12 #include "SimpleShell.h"
shimniok 4:de7feb458652 13 #include "Config.h"
shimniok 9:fc3575d2cbbf 14 #include "L3G4200D.h"
shimniok 0:7e98bbfd102a 15
shimniok 0:7e98bbfd102a 16 Serial pc(USBTX, USBRX);
shimniok 0:7e98bbfd102a 17 //SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
shimniok 0:7e98bbfd102a 18 //FATFileSystem ffs("log", &bd);
shimniok 0:7e98bbfd102a 19 LocalFileSystem lfs("etc");
shimniok 4:de7feb458652 20 Config config;
shimniok 0:7e98bbfd102a 21 SimpleShell sh;
shimniok 0:7e98bbfd102a 22
shimniok 0:7e98bbfd102a 23 DigitalOut led1(LED1);
shimniok 10:9fb3feb38746 24 DigitalOut led2(LED2);
shimniok 10:9fb3feb38746 25 Thread thread;
shimniok 0:7e98bbfd102a 26
shimniok 10:9fb3feb38746 27 typedef struct {
shimniok 10:9fb3feb38746 28 int g[3]; // gyro
shimniok 10:9fb3feb38746 29 float dt; // delta time
shimniok 10:9fb3feb38746 30 } sensor_data_t;
shimniok 10:9fb3feb38746 31
shimniok 10:9fb3feb38746 32 sensor_data_t data;
shimniok 0:7e98bbfd102a 33
shimniok 9:fc3575d2cbbf 34 /******** SHELL COMMANDS ********/
shimniok 9:fc3575d2cbbf 35
shimniok 1:7019a60fd585 36 void test() {
shimniok 1:7019a60fd585 37 printf("Hello world!\n");
shimniok 1:7019a60fd585 38 }
shimniok 1:7019a60fd585 39
shimniok 9:fc3575d2cbbf 40 void read_gyro()
shimniok 9:fc3575d2cbbf 41 {
shimniok 10:9fb3feb38746 42 printf("Gyro: %d, %d, %d %f\n", data.g[0], data.g[1], data.g[2], data.dt);
shimniok 9:fc3575d2cbbf 43 }
shimniok 9:fc3575d2cbbf 44
shimniok 10:9fb3feb38746 45 /********** PERIODIC FUNCTIONS ***********/
shimniok 10:9fb3feb38746 46 L3G4200D gyro(p9, p10);
shimniok 10:9fb3feb38746 47 Timer timer;
shimniok 10:9fb3feb38746 48 int thisTime = 0;
shimniok 10:9fb3feb38746 49 int lastTime = -1;
shimniok 9:fc3575d2cbbf 50
shimniok 10:9fb3feb38746 51 void periodic()
shimniok 9:fc3575d2cbbf 52 {
shimniok 10:9fb3feb38746 53 // Compute dt
shimniok 10:9fb3feb38746 54 thisTime = timer.read_us();
shimniok 10:9fb3feb38746 55 data.dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
shimniok 10:9fb3feb38746 56 lastTime = thisTime;
shimniok 10:9fb3feb38746 57
shimniok 10:9fb3feb38746 58 // Read encoders
shimniok 10:9fb3feb38746 59 // Read gyro
shimniok 10:9fb3feb38746 60 gyro.read(data.g);
shimniok 10:9fb3feb38746 61 //gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_];
shimniok 10:9fb3feb38746 62
shimniok 10:9fb3feb38746 63 // Save current data into history fifo to use 1 second from now
shimniok 10:9fb3feb38746 64 //history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
shimniok 10:9fb3feb38746 65 //history[now].gyro = sensors.gyro[_z_]; // current raw gyro data
shimniok 10:9fb3feb38746 66 //history[now].dt = dt; // current dt, to address jitter
shimniok 10:9fb3feb38746 67 //history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro) ); // compute current heading from current gyro
shimniok 10:9fb3feb38746 68 //float r = PI/180 * history[now].hdg;
shimniok 10:9fb3feb38746 69 //history[now].x = history[prev].x + history[now].dist * sin(r); // update current position
shimniok 10:9fb3feb38746 70 //history[now].y = history[prev].y + history[now].dist * cos(r);
shimniok 10:9fb3feb38746 71
shimniok 10:9fb3feb38746 72 led2 = !led2;
shimniok 10:9fb3feb38746 73
shimniok 10:9fb3feb38746 74 return;
shimniok 9:fc3575d2cbbf 75 }
shimniok 9:fc3575d2cbbf 76
shimniok 9:fc3575d2cbbf 77
shimniok 9:fc3575d2cbbf 78 /******** MAIN ********/
shimniok 1:7019a60fd585 79
shimniok 0:7e98bbfd102a 80 // main() runs in its own thread in the OS
shimniok 0:7e98bbfd102a 81 int main()
shimniok 0:7e98bbfd102a 82 {
shimniok 10:9fb3feb38746 83 EventQueue *queue = mbed_highprio_event_queue();
shimniok 10:9fb3feb38746 84
shimniok 10:9fb3feb38746 85 timer.start();
shimniok 10:9fb3feb38746 86
shimniok 0:7e98bbfd102a 87 printf("Bootup...\n");
shimniok 0:7e98bbfd102a 88 fflush(stdout);
shimniok 1:7019a60fd585 89
shimniok 4:de7feb458652 90 printf("Loading config...\n");
shimniok 7:1f2661b840ed 91 config.add("intercept_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 92 config.add("waypoint_threshold", Config::DOUBLE);
shimniok 7:1f2661b840ed 93 config.add("minimum_turning_radius", Config::DOUBLE);
shimniok 7:1f2661b840ed 94 config.add("wheelbase", Config::DOUBLE);
shimniok 7:1f2661b840ed 95 config.add("track_width", Config::DOUBLE);
shimniok 7:1f2661b840ed 96 config.add("tire_circumference", Config::DOUBLE);
shimniok 7:1f2661b840ed 97 config.add("encoder_stripes", Config::INT);
shimniok 7:1f2661b840ed 98 config.add("esc_brake", Config::INT);
shimniok 7:1f2661b840ed 99 config.add("esc_off", Config::INT);
shimniok 7:1f2661b840ed 100 config.add("esc_max", Config::INT);
shimniok 7:1f2661b840ed 101 config.add("turn_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 102 config.add("turn_distance", Config::DOUBLE);
shimniok 7:1f2661b840ed 103 config.add("start_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 104 config.add("cruise_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 105 config.add("speed_kp", Config::DOUBLE);
shimniok 7:1f2661b840ed 106 config.add("speed_ki", Config::DOUBLE);
shimniok 7:1f2661b840ed 107 config.add("speed_kd", Config::DOUBLE);
shimniok 7:1f2661b840ed 108 config.add("steer_center", Config::DOUBLE);
shimniok 7:1f2661b840ed 109 config.add("steer_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 110 config.add("gyro_scale", Config::DOUBLE);
shimniok 7:1f2661b840ed 111 config.add("gps_valid_speed", Config::DOUBLE);
shimniok 7:1f2661b840ed 112
shimniok 4:de7feb458652 113 if (config.load("/etc/2018cfg.txt")) {
shimniok 4:de7feb458652 114 printf("error loading config\n");
shimniok 4:de7feb458652 115 }
shimniok 10:9fb3feb38746 116
shimniok 10:9fb3feb38746 117 // Startup shell
shimniok 1:7019a60fd585 118 sh.attach(test, "test");
shimniok 9:fc3575d2cbbf 119 sh.attach(read_gyro, "gyro");
shimniok 0:7e98bbfd102a 120 thread.start(callback(&sh, &SimpleShell::run));
shimniok 0:7e98bbfd102a 121
shimniok 10:9fb3feb38746 122 //ticker.attach(callback(periodic), 0.010); // call periodic every 10ms
shimniok 10:9fb3feb38746 123
shimniok 10:9fb3feb38746 124 // schedule calls to periodic at 20 Hz
shimniok 10:9fb3feb38746 125 Event<void()> event(queue, periodic);
shimniok 10:9fb3feb38746 126 event.period(50);
shimniok 10:9fb3feb38746 127 event.post();
shimniok 10:9fb3feb38746 128 queue->dispatch_forever();
shimniok 10:9fb3feb38746 129
shimniok 0:7e98bbfd102a 130 /*
shimniok 0:7e98bbfd102a 131 FILE *fp;
shimniok 0:7e98bbfd102a 132 char buf[128];
shimniok 0:7e98bbfd102a 133 printf("Initializing the block device... ");
shimniok 0:7e98bbfd102a 134 fflush(stdout);
shimniok 0:7e98bbfd102a 135 int err = bd.init();
shimniok 0:7e98bbfd102a 136 printf("%s\n", (err ? "Fail :(" : "OK"));
shimniok 0:7e98bbfd102a 137
shimniok 0:7e98bbfd102a 138 printf("Opening sdtest.txt...");
shimniok 0:7e98bbfd102a 139 fp = fopen("/log/sdtest.txt", "r");
shimniok 0:7e98bbfd102a 140 if(fp) {
shimniok 0:7e98bbfd102a 141 while (!feof(fp)) {
shimniok 0:7e98bbfd102a 142 fgets(buf, 127, fp);
shimniok 0:7e98bbfd102a 143 printf(buf);
shimniok 0:7e98bbfd102a 144 }
shimniok 0:7e98bbfd102a 145 fclose(fp);
shimniok 0:7e98bbfd102a 146 }
shimniok 0:7e98bbfd102a 147
shimniok 0:7e98bbfd102a 148 printf("Opening config.txt...");
shimniok 0:7e98bbfd102a 149 fp = fopen("/etc/config.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
shimniok 0:7e98bbfd102a 159 //SystemReport sys_state(500);
shimniok 0:7e98bbfd102a 160
shimniok 0:7e98bbfd102a 161 while (true) {
shimniok 0:7e98bbfd102a 162 // Blink LED and wait 0.5 seconds
shimniok 0:7e98bbfd102a 163 led1 = !led1;
shimniok 0:7e98bbfd102a 164 wait(0.5f);
shimniok 0:7e98bbfd102a 165
shimniok 0:7e98bbfd102a 166 // Following the main thread wait, report on the current system status
shimniok 0:7e98bbfd102a 167 //sys_state.report_state();
shimniok 0:7e98bbfd102a 168 }
shimniok 0:7e98bbfd102a 169 }