Michael Shimniok
Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Revision 22:dc54ca6e6eec, committed 2018-11-29
- Comitter:
- shimniok
- Date:
- Thu Nov 29 19:45:49 2018 +0000
- Parent:
- 21:894ba6f8d67b
- Child:
- 23:a34af501ea89
- Commit message:
- Converted updater to class, fixed calls in main; changed include order of globals.h to fix extern pc variable.
Changed in this revision
--- a/Config/Config.cpp Thu Nov 29 18:34:22 2018 +0000
+++ b/Config/Config.cpp Thu Nov 29 19:45:49 2018 +0000
@@ -1,198 +0,0 @@
-#define MAXBUF 64
-
-#include "Config.h"
-#include "SDFileSystem.h"
-#include "GeoPosition.h"
-#include "globals.h"
-#include "util.h"
-
-// Identifiers for each of the parameters
-#define CURB "curb"
-#define WAYPOINT "wpt"
-#define GPS "gps"
-#define GYRO "gyro"
-#define MAGNETOMETER "mag"
-#define ACCELEROMETER "accel"
-#define DECLINATION "decl"
-#define NAVIGATION "nav"
-#define STEER "steer"
-#define SPEED "speed"
-#define VEHICLE "veh"
-#define ENCODER "enc"
-
-
-Config::Config():
- loaded(false),
- intercept(0),
- waypointDist(0),
- brakeDist(0),
- wptCount(0),
- escMin(1300),
- escZero(1300),
- escMax(1300),
- topSpeed(0),
- turnSpeed(0),
- startSpeed(0),
- minRadius(0),
- speedKp(0),
- speedKi(0),
- speedKd(0),
- steerZero(0),
- steerScale(0),
- curbThreshold(0),
- curbGain(0),
- gyroScale(0),
- // Data Bus Original Settings
- wheelbase(0.280),
- track(0.290),
- tireCirc(0.321537),
- encStripes(32)
-{
- for (int i=0; i < MAX_WPT; i++) {
- wptTopSpeedAdj[i] = 0;
- wptTurnSpeedAdj[i] = 0;
- }
- for (int i=0; i < 3; i++) {
- magOffset[i] = 0;
- magScale[i] = 0;
- }
-}
-
-// load configuration from filesystem
-bool Config::load(const char *filename)
-{
- FILE *fp;
- char buf[MAXBUF]; // buffer to read in data
- char tmp[MAXBUF]; // temp buffer
- char *p;
- double lat, lon;
- float wTopSpeed, wTurnSpeed;
- bool declFound = false;
- bool confStatus = false;
-
- pc.printf("opening config file...\n");
-
- fp = fopen(filename, "r");
- if (fp == 0) {
- pc.puts("Could not open ");
- pc.puts(filename);
- pc.puts(" \n");
- } else {
- wptCount = 0;
- while (!feof(fp)) {
- fgets(buf, MAXBUF-1, fp);
- p = split(tmp, buf, MAXBUF, ','); // split off the first field
-
- if (!strcmp(tmp, CURB)) {
-
- p = split(tmp, p, MAXBUF, ','); // threshold distance for curb avoid
- curbThreshold = cvstof(tmp);
- p = split(tmp, p, MAXBUF, ','); // steering gain for curb avoid
- curbGain = cvstof(tmp);
-
- } else if (!strcmp(tmp, WAYPOINT)) {
-
- p = split(tmp, p, MAXBUF, ','); // split off the latitude to tmp
- lat = cvstof(tmp);
- p = split(tmp, p, MAXBUF, ','); // split off the longitude to tmp
- lon = cvstof(tmp);
- p = split(tmp, p, MAXBUF, ','); // split off the waypoint top speed
- wTopSpeed = cvstof(tmp);
- p = split(tmp, p, MAXBUF, ','); // split off the waypoint turn speed
- wTurnSpeed = cvstof(tmp);
- if (wptCount < MAXWPT) {
- wpt[wptCount].set(lat, lon); // set position
- wptTopSpeedAdj[wptCount] = wTopSpeed; // set top speed adjust
- wptTurnSpeedAdj[wptCount] = wTurnSpeed; // set turn speed adjust
- wptCount++;
- }
-
- } else if (!strcmp(tmp, NAVIGATION)) {
-
- p = split(tmp, p, MAXBUF, ',');
- intercept = (float) cvstof(tmp); // intercept distance for steering algorithm
- p = split(tmp, p, MAXBUF, ',');
- waypointDist = (float) cvstof(tmp); // distance before waypoint switch
- p = split(tmp, p, MAXBUF, ',');
- brakeDist = (float) cvstof(tmp); // distance at which braking starts
- p = split(tmp, p, MAXBUF, ',');
- minRadius = (float) cvstof(tmp); // minimum turning radius
-
- } else if (!strcmp(tmp, STEER)) {
-
- p = split(tmp, p, MAXBUF, ',');
- steerZero = cvstof(tmp); // servo center setting
- p = split(tmp, p, MAXBUF, ',');
- steerScale = cvstof(tmp); // steering angle conversion factor
-
- } else if (!strcmp(tmp, SPEED)) {
-
- p = split(tmp, p, MAXBUF, ',');
- escMin = cvstof(tmp); // minimum esc (brake) setting
- p = split(tmp, p, MAXBUF, ',');
- escZero = cvstof(tmp); // esc center/zero setting
- p = split(tmp, p, MAXBUF, ',');
- escMax = cvstof(tmp); // maximum esc setting
- p = split(tmp, p, MAXBUF, ',');
- topSpeed = cvstof(tmp); // desired top speed
- p = split(tmp, p, MAXBUF, ',');
- turnSpeed = cvstof(tmp); // speed to use when turning
- p = split(tmp, p, MAXBUF, ',');
- startSpeed = cvstof(tmp); // speed to use at start
- p = split(tmp, p, MAXBUF, ',');
- speedKp = cvstof(tmp); // speed PID: proportional gain
- p = split(tmp, p, MAXBUF, ',');
- speedKi = cvstof(tmp); // speed PID: integral gain
- p = split(tmp, p, MAXBUF, ',');
- speedKd = cvstof(tmp); // speed PID: derivative gain
-
- } else if (!strcmp(tmp, VEHICLE)) {
- p = split(tmp, p, MAXBUF, ',');
- wheelbase = cvstof(tmp); // vehicle wheelbase (front to rear axle)
- p = split(tmp, p, MAXBUF, ',');
- track = cvstof(tmp); // vehicle track width (left to right)
- } else if (!strcmp(tmp, ENCODER)) {
- p = split(tmp, p, MAXBUF, ',');
- tireCirc = cvstof(tmp); // tire circumference
- p = split(tmp, p, MAXBUF, ',');
- encStripes = atoi(tmp); // ticks per revolution
- } else if (!strcmp(tmp, GYRO)) {
- p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp
- gyroScale = cvstof(tmp); // gyro scaling factor to deg/sec
- } else if (!strcmp(tmp, GPS)) {
- p = split(tmp, p, MAXBUF, ',');
- gpsValidSpeed = cvstof(tmp);
- } //if-else
- /* else if (!strcmp(tmp, DECLINATION)) {
- p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp
- declination = (float) cvstof(tmp);
- declFound = true;
- } else if (!strcmp(tmp, MAGNETOMETER)) {
- for (int i=0; i < 3; i++) {
- p = split(tmp, p, MAXBUF, ',');
- magOffset[i] = (float) cvstof(tmp);
- pc.printf("magOffset[%d]: %.2f\n", i, magOffset[i]);
- }
- for (int i=0; i < 3; i++) {
- p = split(tmp, p, MAXBUF, ',');
- magScale[i] = (float) cvstof(tmp);
- pc.printf("magScale[%d]: %.2f\n", i, magScale[i]);
- }
- }
- */
- } // while
-
- // Did we get the values we were looking for?
- if (wptCount > 0 && declFound) {
- confStatus = true;
- }
-
- } // if fp
-
- if (fp != 0)
- fclose(fp);
-
- loaded = confStatus;
-
- return confStatus;
-}
\ No newline at end of file
--- a/UI/shell.cpp Thu Nov 29 18:34:22 2018 +0000
+++ b/UI/shell.cpp Thu Nov 29 19:45:49 2018 +0000
@@ -1,527 +0,0 @@
-#include <stdio.h>
-#include <string.h>
-#include "mbed.h"
-#include "globals.h"
-#include "updater.h"
-#include "print.h"
-#include "DirHandle.h"
-#include "SDFileSystem.h"
-#include "util.h"
-#include "Buttons.h"
-
-#define MAXBUF 128
-#define MAXCMDARR 21
-
-char cwd[MAXBUF];
-char buf[MAXBUF];
-char path[MAXBUF];
-int status;
-bool _debug=false;
-bool done=false;
-
-typedef struct {
- const char *cmd;
- int (*f)(char *arg0);
- const char *desc;
-} cmd;
-
-extern int autonomousMode();
-extern int resetMe();
-extern int gyroSwing();
-
-extern "C" {
-
-void shell(void *args);
-void docmd(char *cmdline);
-void termInput(char *cmd);
-void resolveDirectory(char *newpath, char *path);
-void splitName(const char *path, char *dirname, char *basename);
-int dols(char *arg);
-int docd(char *path);
-int dopwd(char *s);
-int dotouch(char *path);
-int dorm(char *path);
-int domkdir(char *path);
-int dohead(char *path);
-int docat(char *path);
-int dosend(char *path);
-int doprintfree(char *path);
-int doexit(char *s);
-int dodebug(char *s);
-int dohelp(char *s);
-int dogyroswing(char *s);
-int doreset(char *s);
-int doautonomous(char *s);
-int dospeed(char *arg);
-int dosteer(char *arg);
-int dotimes(char *arg);
-
-// TODO 3 multiple arguments
-
-const cmd command[MAXCMDARR] = {
- { "help", dohelp, "print this help" },
- { "ls", dols, "list files" },
- { "cd", docd, "change directory" },
- { "pwd", dopwd, "print working directory" },
- { "touch", dotouch, "create, update file" },
- { "mkdir", domkdir, "make directory" },
- { "head", dohead, "output first part of file" },
- { "cat", docat, "output file" },
- { "send", dosend, "send file to terminal" },
- { "rm", dorm, "remove file" },
- { "debug", dodebug, "toggle debug mode" },
- { "gyro", dogyroswing, "gyro swing" },
- { "auto", doautonomous, "run autonomous mode" },
- { "reset", doreset, "reset the MCU" },
- { "free", doprintfree, "heap bytes available" },
- { "speed", dospeed, "set speed servo output" },
- { "steer", dosteer, "set steering servo output" },
- { "time", dotimes, "display update timing stats" },
-// { "exit", doexit, "exit shell" },
- { 0, 0, 0 }
-};
-
-void shell(void *args) {
- char cmdline[MAXBUF];
-
- pc.baud(115200);
-
- strcpy(cwd, "/log");
-
- fputs("Type help for assistance.\n", stdout);
-
- status=0;
- done=false;
- while (!done) {
- termInput(cmdline);
-
- // interrupt operation if keypad button is pressed
- if (keypad.pressed) {
- keypad.pressed = false;
- break;
- }
-
- docmd(cmdline);
- }
- fputs("exiting shell\n", stdout);
-
- return;
-}
-
-
-/** docmd
- * Run a command by looking it up the command requested on the shell command line in the
- * command array. If it's found, run the associated function. If not, print an error.
- */
-void docmd(char *cmdline) {
- char *arg;
- char cmd[MAXBUF];
- bool found = false;
-
- arg = split(cmd, cmdline, MAXBUF, ' ');
- if (strlen(cmd) > 0) {
- //if (debug) fprintf(stdout, "cmdline:<%s> cmd:<%s> arg:<%s>\n", cmdline, cmd, arg);
-
- for (int i=0; command[i].cmd; i++) {
- if (!strcmp(cmd, command[i].cmd)) {
- found = true;
- command[i].f(arg);
- }
- }
-
- if (!found) {
- fputs(cmd, stdout);
- fputs(": command not found\n", stdout);
- }
- }
-
- return;
-}
-
-
-/** termInput
- * read input from the terminal
- */
-void termInput(char *cmd) {
- int i=0;
- char c;
- bool done = false;
-
- memset(cmd, 0, MAXBUF);
- fputc('(', stdout);
- fputs(cwd, stdout);
- fputs(")# ", stdout);
- do {
- cmd[i] = 0;
- c = fgetc(stdin);
- if (c == '\r') { // if return is hit, we're done, don't add \r to cmd
- done = true;
- } else if (i < MAXBUF-1) {
- if (c == 0x7f || c == '\b') { // backspace or delete
- if (i > 0) { // if we're at the beginning, do nothing
- i--;
- fputs("\b \b", stdout);
-
- }
- } else {
- fputc(c, stdout);
- cmd[i++] = c;
- }
- }
- } while (!done);
- fputc('\n', stdout);
-
-
-}
-
-/** resolveDirectory
- * resolve the directory path provided, given the cwd
- */
-void resolveDirectory(char *newpath, char *path) {
- char dirname[32], basename[16];
-
- /** absolute path */
- if (path[0] == '/') {
- strcpy(newpath, path);
- }
- /** relative path */
- else {
- strcpy(newpath, cwd);
- if (path[0] != 0) {
- if (newpath[strlen(newpath)-1] != '/')
- strcat(newpath, "/");
- strcat(newpath, path);
- }
- /** Resolve .. references */
- splitName(newpath, dirname, basename);
- if (!strcmp(basename, "..")) {
- splitName(dirname, newpath, basename);
- }
- }
-}
-
-/** splitCmd
- * copy t to s until space is reached
- * return location of delimiter+1 in t
- * if space not found, return t pointing to end of string
- * if s or t null, return null
- */
-char *splitCmd(char *s, char *t, int max)
-{
- int i = 0;
-
- if (s == 0 || t == 0)
- return 0;
-
- while (*t != 0 && i < max) {
- *s++ = *t++;
- i++;
- if (*t == ' ') {
- t++;
- break;
- }
- }
- *s = 0;
-
- return t;
-}
-
-/** splitName
- * split the path into a dirname and a
- */
-void splitName(const char *path, char *dirname, char *basename) {
- int sep;
-
- sep = 0;
- //if (debug) fprintf(stdout, "%d\n", strlen(path));
- for (int i=strlen(path)-1; i >= 0; i--) {
- //if (debug) fprintf(stdout, "- %c\n", path[i]);
- sep = i;
- if (path[i] == '/') break;
- }
- for (int j=0; j < sep; j++) {
- //if (debug) fprintf(stdout, "> %c\n", path[j]);
- dirname[j] = path[j];
- dirname[j+1] = 0;
- }
- for (unsigned int k=sep+1; k != strlen(path); k++) {
- //if (debug) fprintf(stdout, "* %c\n", path[k]);
- basename[k-(sep+1)] = path[k];
- basename[k-sep] = 0;
- }
- //if (_debug) fprintf(stdout, "d:<%s> b:<%s>\n", dirname, basename);
-
-
-}
-
-/** ls
- * lists files in the current working directory, 4 columns
- */
-int dols(char *arg) {
- //if (_debug) fprintf(stdout, "%s\n", cwd);
- DIR *d;
- struct dirent *p;
-
- resolveDirectory(path, arg);
-
- int count=0;
- if ((d = opendir(path)) != NULL) {
- while ((p = readdir(d)) != NULL) {
- int pad = 20 - strlen(p->d_name);
- while (pad-- > 0) fputc(' ', stdout);
- fputs(p->d_name, stdout);
- if (count++ >= 3) {
- count = 0;
- fputc('\n', stdout);
- }
- }
- fputc('\n', stdout);
- if (count < 3)
- fputc('\n', stdout);
- closedir(d);
- status = 0;
- } else {
- fputs(path, stdout);
- fputs(": No such directory\n", stdout);
- status = 1;
- }
-
- return status;
-}
-
-/** cd
- * changes current working directory
- */
-int docd(char *arg) {
-
- resolveDirectory(path, arg);
- strcpy(cwd, path);
-
- return 0;
-}
-
-/** pwd
- * print current working directory
- */
-int dopwd(char *arg) {
- fputs(cwd, stdout);
- fputc('\n', stdout);
- //fprintf(stdout, "%s\n", cwd);
-
- return 0;
-}
-
-/** touch
- * create an empty file
- */
-int dotouch(char *arg) {
- FILE *fp;
-
- resolveDirectory(path, arg);
- if ((fp = fopen(path, "w")) != NULL) {
- fclose(fp);
- status = 0;
- } else {
- fputs(path, stdout);
- fputs(": No such file\n", stdout);
- status = 1;
- }
-
- return status;
-}
-
-int dorm(char *arg) {
- resolveDirectory(path, arg);
- return remove(path);
-}
-
-int domkdir(char *arg) {
- resolveDirectory(path, arg);
- return mkdir(path, S_IRWXU|S_IRWXG|S_IRWXO);
-}
-
-/** head
- * print the first 10 lines of a file
- */
-int dohead(char *arg) {
- FILE *fp;
- char line = 0;
-
- resolveDirectory(path, arg);
- if ((fp = fopen(path, "r")) != NULL) {
- while (!feof(fp) && line++ < 10) {
- fgets(buf, 128, fp);
- fputs(buf, stdout);
- }
- fclose(fp);
- status = 0;
- } else {
- fputs(path, stdout);
- fputs(": No such file\n", stdout);
- status = 1;
- }
-
- return status;
-}
-
-/** cat
- * display the content of a file
- */
-int docat(char *arg) {
- FILE *fp;
-
- resolveDirectory(path, arg);
- if ((fp = fopen(path, "r")) != NULL) {
- while (fgets(buf, 127, fp)) {
- fputs(buf, stdout);
- }
- fclose(fp);
- status = 0;
- } else {
- fputs(path, stdout);
- fputs(": No such file\n", stdout);
- status = 1;
- }
-
- return status;
-}
-
-/** send
- * Simple serial file transfer protocol
- * Initiates escape sequence: ^A^B, sends filename, ^C, and then file
- * contents followed by ^D
- */
-int dosend(char *arg) {
- FILE *fp;
- char dirname[32], basename[16];
-
- resolveDirectory(path, arg);
- if ((fp = fopen(path, "r")) != NULL) {
- splitName(path, dirname, basename);
- fputc(0x01, stdout);
- fputc(0x02, stdout);
- fputs(basename, stdout);
- fputc(0x03, stdout);
- while (!feof(fp)) {
- if (fgets(buf, 127, fp) != NULL)
- fputs(buf, stdout);
- }
- fclose(fp);
- fputc(0x04, stdout);
- status = 0;
- } else {
- fputs(path, stdout);
- fputs(": No such file\n", stdout);
- status = 1;
- }
-
- return status;
-}
-
-int doprintfree(char *arg) {
- //printInt(stdout, xPortGetFreeHeapSize());
- fputs(" bytes free.\n", stdout);
-
- return 0;
-}
-
-/** doexit
- * set a flag to exit the shell
- */
-int doexit(char *arg) {
- done = true;
-
- return 0;
-}
-
-/** dodebug
- * toggle the _debug state variable
- */
-int dodebug(char *arg) {
- _debug = !_debug;
-
- return 0;
-}
-
-/** dohelp
- * print the list of commands and descriptions
- */
-int dohelp(char *arg) {
- for (int i=0; command[i].cmd; i++) {
- int pad = 10 - strlen(command[i].cmd);
- while (pad--) fputc(' ', stdout);
- fputs(command[i].cmd, stdout);
- fputs(": ", stdout);
- fputs(command[i].desc, stdout);
- fputc('\n', stdout);
- }
-
- return 0;
-}
-
-/** dogyroswing
- * perform gyro swing, call external function
- */
-int dogyroswing(char *arg) {
- gyroSwing();
-
- return 0;
-}
-
-/** doreset
- * reset the processor
- */
-int doreset(char *arg) {
- resetMe();
- return 0; // won't ever reach this line...
-}
-
-/** doautonomous
- * call external doAutonomous mode to perform an autonomous run
- */
-int doautonomous(char *arg) {
- autonomousMode();
-
- return 0;
-}
-
-
-int dospeed(char *arg) {
- // TODO 3 dospeed()
-// int v = atoi(arg);
-// if (config.escMin < v && v < config.escMax {
-// fputs("speed=", stdout);
-// setThrottle(v);
-// printFloat(stdout, getThrottle(), 4);
-// fputc('\n', stdout);
-// }
- return 0;
-}
-
-int dosteer(char *arg) {
- float v = cvstof(arg);
- fputs("angle=", stdout);
- printFloat(stdout, v, 4);
- fputs(" servo=", stdout);
- // TODO fix setSteering call
- //setSteering(v);
- // TODO 4 printFloat(stdout, getSteering(), 4);
- fputc('\n', stdout);
- return 0;
-}
-
-int dotimes(char *arg) {
- int i;
- for (i = 1; i < 8; i++) {
- printInt(stdout, i);
- fputc(':', stdout);
- // TODO 4 printInt(stdout, getUpdateTime(i)-getUpdateTime(i-1));
- fputc('\n', stdout);
- }
- fputs("total:", stdout);
- // TODO 4 printInt(stdout, getUpdateTime(7)-getUpdateTime(0));
- fputc('\n', stdout);
- return 0;
-}
-
-} // extern C
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Updater.h Thu Nov 29 19:45:49 2018 +0000
@@ -0,0 +1,45 @@
+#ifndef __SCHEDULER_H
+#define __SCHEDULER_H
+
+/** Updater is the main real time sensor update, estimation, and control routine that is
+ * called at 100Hz by a timer interrupt
+ */
+class Updater {
+public:
+
+ /** attach the update routine to Ticker interrupt */
+ void start(void);
+
+ /** detach the update routine from Ticker interrupt */
+ void stop(void);
+
+ /** Tells the updater to re-initialize the navigation state */
+ void restartNav(void);
+
+ /** Indicates to the updater that the vehicle should begin its run */
+ void beginRun(void);
+
+ /** Indicates to the updater that the vehicle should abort its run */
+ void endRun(void);
+
+ /** Returns the elapsed time taken by the updater routine on its most recent run */
+ int getUpdateTime(void);
+
+ /** Sets the desired speed of the rover */
+ void setSpeed(float speed);
+
+ /** The function that is called at 100Hz. It reads sensors, performs estimation, and controls the robot */
+ void update(void);
+
+ /** initialize throttle to center position */
+ void initThrottle(void);
+
+ /** initialize steering to center position */
+ void initSteering(void);
+
+ /** Sets the steering angle */
+ void setSteering(float steerAngle);
+
+}
+
+#endif
--- a/main.cpp Thu Nov 29 18:34:22 2018 +0000
+++ b/main.cpp Thu Nov 29 19:45:49 2018 +0000
@@ -1,770 +0,0 @@
-/** Code for "Data Bus" UGV entry for Sparkfun AVC 2014
- * http://www.bot-thoughts.com/
- */
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-// INCLUDES
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-
-#include "mbed.h"
-#include <math.h>
-#include <stdint.h>
-#include "devices.h"
-#include "globals.h"
-#include "Config.h"
-#include "Buttons.h"
-#include "Display.h"
-#include "Menu.h"
-#include "GPSStatus.h"
-#include "logging.h"
-#include "Telemetry.h"
-#include "SystemState.h"
-#include "shell.h"
-#include "Steering.h"
-#include "Sensors.h"
-#include "kalman.h"
-#include "Ublox6.h"
-#include "PinDetect.h" // TODO 4 this should be broken into .h, .cpp
-#include "IncrementalEncoder.h"
-#include "Steering.h"
-#include "Schedule.h"
-#include "GeoPosition.h"
-#include "Mapping.h"
-#include "SimpleFilter.h"
-#include "Beep.h"
-#include "util.h"
-#include "updater.h"
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-// DEFINES
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-
-#define absf(x) (x *= (x < 0.0) ? -1 : 1)
-
-#define GPS_MIN_SPEED 2.0 // speed below which we won't trust GPS course
-#define GPS_MAX_HDOP 2.0 // HDOP above which we won't trust GPS course/position
-
-// Driver configuration parameters
-#define SONARLEFT_CHAN 0
-#define SONARRIGHT_CHAN 1
-#define IRLEFT_CHAN 2
-#define IRRIGHT_CHAN 3
-#define TEMP_CHAN 4
-#define GYRO_CHAN 5
-
-#define INSTRUMENT_CHECK 0
-#define AHRS_VISUALIZATION 1
-#define DISPLAY_PANEL 2
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-// GLOBAL VARIABLES
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// OUTPUT
-DigitalOut confStatus(LED1); // Config file status LED
-DigitalOut logStatus(LED2); // Log file status LED
-DigitalOut gpsStatus(LED3); // GPS fix status LED
-DigitalOut updaterStatus(LED4); // update loop status LED
-//DigitalOut sonarStart(p18); // Sends signal to start sonar array pings
-Display display; // UI display
-//Beep speaker(p24); // Piezo speaker
-
-// INPUT
-Menu menu;
-Buttons keypad;
-
-// COMM
-Serial pc(USBTX, USBRX); // PC usb communications
-SerialGraphicLCD lcd(LCDTX, LCDRX, SD_FW); // Graphic LCD with summoningdark firmware
-Serial tel(TELEMTX, TELEMRX); // UART for telemetry
-Telemetry telem(tel); // Setup telemetry system
-
-// SENSORS
-Sensors sensors; // Abstraction of sensor drivers
-//DCM ahrs; // ArduPilot/MatrixPilot AHRS
-Serial *dev; // For use with bridge
-
-// MISC
-FILE *camlog; // Camera log
-Config config; // Configuration utility
-
-// Timing
-Timer timer; // For main loop scheduling
-
-// GPS Variables
-unsigned long age = 0; // gps fix age
-
-// schedule for LED warning flasher
-Schedule blink;
-
-// Estimation & Navigation Variables
-GeoPosition dr_here; // Estimated position based on estimated heading
-Mapping mapper;
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-// FUNCTION DEFINITIONS
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-
-int autonomousMode(void);
-void serialBridge(Serial &gps);
-int gyroSwing(void);
-int setBacklight(void);
-int reverseScreen(void);
-float irDistance(const unsigned int adc);
-extern "C" void mbed_reset();
-
-// If we don't close the log file, when we restart, all the written data
-// will be lost. So we have to use a button to force mbed to close the
-// file and preserve the data.
-//
-
-int dummy(void)
-{
- return 0;
-}
-
-
-int resetMe()
-{
- mbed_reset();
-
- return 0;
-}
-
-
-int main()
-{
- //checkit(__FILE__, __LINE__);
- //xTaskCreate( shell, (const signed char * ) "shell", 128, NULL, (tskIDLE_PRIORITY+3), NULL );
- //checkit(__FILE__, __LINE__);
- //vTaskStartScheduler(); // should never get past this line.
- //while(1);
-
- // Let's try setting priorities...
- //NVIC_SetPriority(DMA_IRQn, 0);
- NVIC_SetPriority(TIMER3_IRQn, 2); // updater running off Ticker, must be highest priority!!
- NVIC_SetPriority(EINT0_IRQn, 5); // wheel encoders
- NVIC_SetPriority(EINT1_IRQn, 5); // wheel encoders
- NVIC_SetPriority(EINT2_IRQn, 5); // wheel encoders
- NVIC_SetPriority(EINT3_IRQn, 5); // wheel encoders
- NVIC_SetPriority(SPI_IRQn, 7); // uSD card, logging
- NVIC_SetPriority(UART0_IRQn, 10); // USB
- NVIC_SetPriority(UART1_IRQn, 10);
- NVIC_SetPriority(UART2_IRQn, 10);
- NVIC_SetPriority(UART3_IRQn, 10);
- NVIC_SetPriority(I2C0_IRQn, 10); // sensors?
- NVIC_SetPriority(I2C1_IRQn, 10); // sensors?
- NVIC_SetPriority(I2C2_IRQn, 10); // sensors?
- NVIC_SetPriority(ADC_IRQn, 10); // Voltage/current
- NVIC_SetPriority(TIMER0_IRQn, 10); // unused(?)
- NVIC_SetPriority(TIMER1_IRQn, 10); // unused(?)
- NVIC_SetPriority(TIMER2_IRQn, 10); // unused(?)
-
- // Something here is jacking up the I2C stuff
- // Also when initializing with ESC powered, it causes motor to run which
- // totally jacks up everything (noise?)
- initSteering();
- initThrottle();
-
- display.init();
- display.status("Data Bus 2014");
-
- // Send data back to the PC
- pc.baud(115200);
- fputs("Data Bus 2014\n", stdout);
- fflush(stdin);
-
- fputs("Initializing...\n", stdout);
- display.status("Initializing");
-
- // Initialize status LEDs
- updaterStatus = 0;
- gpsStatus = 0;
- logStatus = 0;
- confStatus = 0;
-
- if (!fifo_init()) {
- error("\n\n%% Error initializing SystemState fifo %%\n");
- }
-
- fputs("Loading configuration...\n", stdout);
- display.status("Load config");
- if (config.load("/etc/config.txt")) // Load various configurable parameters, e.g., waypoints, declination, etc.
- confStatus = 1;
-
- initThrottle();
-
- //pc.printf("Declination: %.1f\n", config.declination);
- pc.puts("Speed: escZero=");
- pc.puts(cvftos(config.escZero, 3));
- pc.puts(" escMin=");
- pc.puts(cvftos(config.escMin, 3));
- pc.puts(" escMax=");
- pc.puts(cvftos(config.escMax, 3));
- pc.puts(" top=");
- pc.puts(cvftos(config.topSpeed, 1));
- pc.puts(" turn=");
- pc.puts(cvftos(config.turnSpeed, 1));
- pc.puts(" Kp=");
- pc.puts(cvftos(config.speedKp, 4));
- pc.puts(" Ki=");
- pc.puts(cvftos(config.speedKi, 4));
- pc.puts(" Kd=");
- pc.puts(cvftos(config.speedKd, 4));
- pc.puts("\n");
-
- pc.puts("Steering: steerZero=");
- pc.puts(cvftos(config.steerZero, 2));
- pc.puts(" steerScale=");
- pc.puts(cvftos(config.steerScale, 1));
- pc.puts("\n");
- steering.setScale(config.steerScale);
-
- // Convert lat/lon waypoints to cartesian
- mapper.init(config.wptCount, config.wpt);
- for (unsigned int w = 0; w < MAXWPT && w < config.wptCount; w++) {
- mapper.geoToCart(config.wpt[w], &(config.cwpt[w]));
- pc.puts("Waypoint #");
- pc.puts(cvntos(w));
- pc.puts(" (");
- pc.puts(cvftos(config.cwpt[w].x, 4));
- pc.puts(", ");
- pc.puts(cvftos(config.cwpt[w].y, 4));
- pc.puts(") lat: ");
- pc.puts(cvftos(config.wpt[w].latitude(), 6));
- pc.puts(" lon: ");
- pc.puts(cvftos(config.wpt[w].longitude(), 6));
- pc.puts(", topspeed: ");
- pc.puts(cvftos(config.topSpeed + config.wptTopSpeedAdj[w], 1));
- pc.puts(", turnspeed: ");
- pc.puts(cvftos(config.turnSpeed + config.wptTurnSpeedAdj[w], 1));
- pc.puts("\n");
- }
-
- // TODO 3 print mag and gyro calibrations
-
- // TODO 3 remove GPS configuration, all config will be in object itself I think
-
- display.status("Vehicle config ");
- pc.puts("Wheelbase: ");
- pc.puts(cvftos(config.wheelbase, 3));
- pc.puts("\n");
- pc.puts("Track Width: ");
- pc.puts(cvftos(config.track, 3));
- pc.puts("\n");
- steering.setWheelbase(config.wheelbase);
- steering.setTrack(config.track);
-
- display.status("Encoder config ");
- pc.puts("Tire Circumference: ");
- pc.puts(cvftos(config.tireCirc, 5));
- pc.puts("\n");
- pc.puts("Ticks per revolution: ");
- pc.puts(cvftos(config.encStripes, 5));
- pc.puts("\n");
- sensors.configureEncoders(config.tireCirc, config.encStripes);
-
- display.status("Nav configuration ");
- pc.puts("Intercept distance: ");
- pc.puts(cvftos(config.intercept, 1));
- pc.puts("\n");
- steering.setIntercept(config.intercept);
- pc.puts("Waypoint distance: ");
- pc.puts(cvftos(config.waypointDist, 1));
- pc.puts("\n");
- pc.puts("Brake distance: ");
- pc.puts(cvftos(config.brakeDist, 1));
- pc.puts("\n");
- pc.puts("Min turn radius: ");
- pc.puts(cvftos(config.minRadius, 3));
- pc.puts("\n");
-
- display.status("Gyro config ");
- pc.puts("Gyro scale: ");
- pc.puts(cvftos(config.gyroScale, 5));
- pc.puts("\n");
- sensors.setGyroScale(config.gyroScale);
-
- display.status("GPS configuration ");
- pc.puts("GPS valid speed: ");
- pc.puts(cvftos(config.gpsValidSpeed,1));
- pc.puts("\n");
-
- pc.puts("Gyro config ");
- pc.puts("\n");
- pc.puts("Gyro scale: ");
- pc.puts(cvftos(config.gyroScale, 5));
- pc.puts("\n");
- sensors.setGyroScale(config.gyroScale);
-
- pc.puts("Calculating offsets...\n");
- display.status("Offset calculation ");
- wait(0.2);
- // TODO 3 Really need to give the gyro more time to settle
- sensors.gps.disable();
- // TODO 2 sensors.Calculate_Offsets();
-
- pc.puts("Starting GPS...\n");
- display.status("Start GPS "); // TODO 3: would be nice not to have to pad at this level
- wait(0.2);
- sensors.gps.setUpdateRate(10);
- sensors.gps.enable();
-
- pc.puts("Starting Scheduler...\n");
- display.status("Start scheduler ");
- wait(0.2);
- // Startup sensor/AHRS ticker; update every UPDATE_PERIOD
- restartNav();
- startUpdater();
-
- pc.puts("Starting keypad...\n");
-
- keypad.init();
-
- pc.puts("Adding menu items...\n");
-
- // Setup LCD Input Menu
- menu.add("Auto mode", &autonomousMode);
- menu.add("Gyro Calib", &gyroSwing);
- menu.add("Backlight", &setBacklight);
- menu.add("Reverse", &reverseScreen);
- menu.add("Reset", &resetMe);
-
- pc.puts("Starting main timer...\n");
-
- timer.start();
- timer.reset();
-
- int thisUpdate = timer.read_ms();
- int nextDisplayUpdate = thisUpdate;
- int nextWaypointUpdate = thisUpdate;
- char cmd;
- bool printMenu = true;
- bool printLCDMenu = true;
-
- pc.puts("Timer done, enter loop...\n");
-
- while (1) {
-
- thisUpdate = timer.read_ms();
- if (thisUpdate > nextDisplayUpdate) {
- // Pulling out current state so we get the most current
- SystemState *s = fifo_first();
- // TODO 3 fix this so gps is already in state
- // Now populate in the current GPS data
- s->gpsHDOP = sensors.gps.hdop();
- s->gpsSats = sensors.gps.sat_count();
-
- telem.sendPacket(s);
- display.update(s);
- nextDisplayUpdate = thisUpdate + 200;
- }
-
- // every so often, send the currently configured waypoints
- if (thisUpdate > nextWaypointUpdate) {
- telem.sendPacket(config.cwpt, config.wptCount);
- nextWaypointUpdate = thisUpdate + 10000;
- // TODO 2: make this a request/response, Telemetry has to receive packets, decode, etc.
- }
-
- if (keypad.pressed) {
- keypad.pressed = false;
- printLCDMenu = true;
- switch (keypad.which) {
- case NEXT_BUTTON:
- menu.next();
- break;
- case PREV_BUTTON:
- menu.prev();
- break;
- case SELECT_BUTTON:
- display.select(menu.getItemName());
- menu.select();
- printMenu = true;
- break;
- default:
- printLCDMenu = false;
- break;
- }//switch
- keypad.pressed = false;
- }// if (keypad.pressed)
-
-
- if (printLCDMenu) {
- display.menu( menu.getItemName() );
- display.status("Ready.");
- display.redraw();
- printLCDMenu = false;
- }
-
- // TODO 3 move to UI area
- if (printMenu) {
- fputs("\n==============\nData Bus Menu\n==============\n", stdout);
- fputs("0) Autonomous mode\n", stdout);
- fputs("1) Bridge serial to GPS\n", stdout);
- fputs("2) Gyro calibrate\n", stdout);
- fputs("3) Shell\n", stdout);
- fputs("R) Reset\n", stdout);
- fputs("\nSelect from the above: ", stdout);
- fflush(stdout);
- printMenu = false;
- }
-
- if (pc.readable()) {
- cmd = fgetc(stdin);
- fputc(cmd, stdout);
- fputc('\n', stdout);
- printMenu = true;
- printLCDMenu = true;
-
- switch (cmd) {
- case 'R' :
- resetMe();
- break;
- case '0' :
- display.select(menu.getItemName(0));
- autonomousMode();
- break;
- case '1' :
- display.select("Serial bridge");
- display.status("Standby.");
- sensors.gps.enableVerbose();
- serialBridge( *(sensors.gps.getSerial()) );
- sensors.gps.disableVerbose();
- break;
- case '2' :
- display.select("Gyro Calib");
- display.select(menu.getItemName(2));
- gyroSwing();
- break;
- case '3' :
- display.select("Shell");
- display.status("Standby.");
- shell(0);
- break;
- default :
- break;
- } // switch
-
- } // if (pc.readable())
-
- wait(0.1);
-
- } // while
-
-}
-
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-// OPERATIONAL MODE FUNCTIONS
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-
-int autonomousMode()
-{
- bool goGoGo = false; // signal to start moving
- bool navDone; // signal that we're done navigating
- int nextTelemUpdate; // keeps track of teletry update periods
- //extern int tSensor, tGPS, tAHRS, tLog;
-
- sensors.gps.reset_available();
-
- // TODO: 3 move to main?
- // Navigation
-
- goGoGo = false;
- navDone = false;
- keypad.pressed = false;
- //bool started = false; // flag to indicate robot has exceeded min speed.
-
- if (initLogfile()) logStatus = 1; // Open the log file in sprintf format string style; numbers go in %d
- wait(0.2);
-
- sensors.gps.disableVerbose();
- sensors.gps.enable();
- //gps2.enable();
-
- fputs("Press select button to start.\n", stdout);
- display.status("Select starts.");
- wait(1.0);
-
- timer.reset();
- timer.start();
- nextTelemUpdate = timer.read_ms();
- wait(0.1);
-
- // Tell the navigation / position estimation stuff to reset to starting waypoint
- // Disable 05/27/2013 to try and fix initial heading estimate
- //restartNav();
-
- // Main loop
- //
- while(navDone == false) {
- //////////////////////////////////////////////////////////////////////////////
- // USER INPUT
- //////////////////////////////////////////////////////////////////////////////
-
- // Button state machine
- // if we've not started going, button starts us
- // if we have started going, button stops us
- // but only if we've released it first
- //
- // set throttle only if goGoGo set
- if (goGoGo) {
- // TODO: 2 Add additional condition of travel for N meters before the HALT button is armed
-
- if (keypad.pressed == true) { // && started
- fputs(">>>>>>>>>>>>>>>>>>>>>>> HALT\n", stdout);
- display.status("HALT.");
- navDone = true;
- goGoGo = false;
- keypad.pressed = false;
- endRun();
- }
- } else {
- if (keypad.pressed == true) {
- fputs(">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n", stdout);
- display.status("GO GO GO!");
- goGoGo = true;
- keypad.pressed = false;
- beginRun();
- }
- }
-
- // Are we at the last waypoint?
- //
- if (fifo_first()->nextWaypoint == config.wptCount) {
- fputs("Arrived at final destination.\n", stdout);
- display.status("Arrived at end.");
- navDone = true;
- endRun();
- }
-
- //////////////////////////////////////////////////////////////////////////////
- // TELEMETRY
- //////////////////////////////////////////////////////////////////////////////
- if (timer.read_ms() > nextTelemUpdate) {
- SystemState *s = fifo_first();
- telem.sendPacket(s); // TODO 4 run this out of timer interrupt
- nextTelemUpdate += 200; // TODO 3 increase update speed
- }
-
- //////////////////////////////////////////////////////////////////////////////
- // LOGGING
- //////////////////////////////////////////////////////////////////////////////
- // sensor reads are happening in the schedHandler();
- // Are there more items to come out of the log fifo?
- // Since this could take anywhere from a few hundred usec to
- // 150ms, we run it opportunistically and use a buffer. That way
- // the sensor updates, calculation, and control can continue to happen
- if (fifo_available()) {
- logStatus = !logStatus; // log indicator LED
- logData( fifo_pull() ); // log state data to file
- logStatus = !logStatus; // log indicator LED
- }
-
- } // while
- closeLogfile();
- wait(2.0);
- logStatus = 0;
- display.status("Completed. Saved.");
- wait(2.0);
-
- updaterStatus = 0;
- gpsStatus = 0;
- //confStatus = 0;
- //flasher = 0;
-
- sensors.gps.disableVerbose();
-
- return 0;
-} // autonomousMode
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-// UTILITY FUNCTIONS
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// Gather gyro data using turntable equipped with dual channel
-// encoder. Use onboard wheel encoder system. Left channel
-// is the index (0 degree) mark, while the right channel
-// is the incremental encoder. Can then compare gyro integrated
-// heading with machine-reported heading
-//
-int gyroSwing()
-{
- FILE *fp;
- int now;
- int next;
- int g[3];
- int leftTotal=0;
- int rightTotal=0;
-
- // Timing is pretty critical so just in case, disable serial processing from GPS
- sensors.gps.disable();
- stopUpdater();
-
- fputs("Starting gyro swing...\n", stdout);
- display.status("Starting...");
-// fp = openlog("gy");
- fp = stdout;
-
- display.status("Rotate clockwise.");
- fputs("Begin clockwise rotation, varying rpm\n", stdout);
- wait(1);
-
- display.status("Select exits.");
- fputs("Press select to exit\n", stdout);
- wait(1);
-
-
- timer.reset();
- timer.start();
-
- next = now = timer.read_ms();
-
- sensors._right.read(); // easiest way to reset the heading counter
- sensors._left.read();
-
- while (1) {
- now = timer.read_ms();
-
- if (keypad.pressed) {
- keypad.pressed = false;
- break;
- }
-
- if (now >= next) {
- leftTotal += sensors._left.read();
- rightTotal += sensors._right.read();
- sensors._gyro.read(g);
- fputs(cvitos(now), fp);
- fputs(" ", fp);
- fputs(cvntos(leftTotal), fp);
- fputs(" ", fp);
- fputs(cvntos(rightTotal), fp);
- fputs(" ", fp);
- fputs(cvitos(g[_z_]), fp);
- fputs("\n", fp);
- next = now + 50;
- }
- }
- if (fp && fp != stdout) {
- fclose(fp);
- display.status("Done. Saved.");
- fputs("Data collection complete.\n", stdout);
- wait(2);
- }
-
- sensors.gps.enable();
- restartNav();
- startUpdater();
-
- keypad.pressed = false;
-
- return 0;
-}
-
-
-
-void bridgeRecv()
-{
-#if 0
- while (dev && dev->readable()) {
- pc.putc(dev->getc());
- }
-#endif
-}
-
-
-void serialBridge(Serial &serial)
-{
-#if 0
- char x;
- int count = 0;
- bool done=false;
-
- fputs("\nEntering serial bridge in 2 seconds, +++ to escape\n\n", stdout);
- sensors.gps.enableVerbose();
- wait(2.0);
- //dev = &gps;
- sensors.gps.disable();
- serial.baud(38400);
- while (!done) {
- if (pc.readable()) {
- x = pc.getc();
- serial.putc(x);
- // escape sequence
- if (x == '+') {
- if (++count >= 3) done=true;
- } else {
- count = 0;
- }
- }
- if (serial.readable()) {
- fputc(serial.getc(), stdout);
- }
- }
-#endif
-}
-
-
-int setBacklight(void) {
- Menu bmenu;
- bool done = false;
- bool printUpdate = false;
- static int backlight=100;
-
- display.select(">> Backlight");
-
- while (!done) {
- if (keypad.pressed) {
- keypad.pressed = false;
- printUpdate = true;
- switch (keypad.which) {
- case NEXT_BUTTON:
- backlight+=5;
- if (backlight > 100) backlight = 100;
- lcd.backlight(backlight);
- break;
- case PREV_BUTTON:
- backlight-=5;
- if (backlight < 0) backlight = 0;
- lcd.backlight(backlight);
- break;
- case SELECT_BUTTON:
- done = true;
- break;
- }
- }
- if (printUpdate) {
- printUpdate = false;
- lcd.pos(0,1);
- // TODO 3 lcd.printf("%3d%%%-16s", backlight, "");
- }
- }
-
- return 0;
-}
-
-int reverseScreen(void) {
- lcd.reverseMode();
-
- return 0;
-}
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-// ADC CONVERSION FUNCTIONS
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// returns distance in m for Sharp GP2YOA710K0F
-// to get m and b, I wrote down volt vs. dist by eyeballin the
-// datasheet chart plot. Then used Excel to do linear regression
-//
-float irDistance(const unsigned int adc)
-{
- float b = 1.0934; // Intercept from Excel
- float m = 1.4088; // Slope from Excel
-
- return m / (((float) adc) * 4.95/4096 - b);
-}
--- a/updater.cpp Thu Nov 29 18:34:22 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,577 +0,0 @@
-#include "mbed.h"
-#include "util.h"
-#include "globals.h"
-#include "updater.h"
-#include "Config.h"
-#include "Actuators.h"
-#include "Sensors.h"
-#include "SystemState.h"
-#include "Venus638flpx.h"
-//#include "Ublox6.h"
-#include "Steering.h"
-#include "Servo.h"
-#include "Mapping.h"
-#include "CartPosition.h"
-#include "GeoPosition.h"
-#include "kalman.h"
-
-#define UPDATE_PERIOD 0.010 // update period in s
-
-#define _x_ 0
-#define _y_ 1
-#define _z_ 2
-
-// The below is for main loop at 50ms = 20hz operation
-//#define CTRL_SKIP 2 // 100ms, 10hz, control update
-//#define MAG_SKIP 1 // 50ms, 20hz, magnetometer update
-//#define LOG_SKIP 1 // 50ms, 20hz, log entry entered into fifo
-
-// The following is for main loop at 10ms = 100hz
-#define HDG_LAG 40 // Number of update steps that the GPS report is behind realtime
-#define CTRL_SKIP 5 // 50ms (20hz), control update
-#define MAG_SKIP 2 // 20ms (50hz), magnetometer update
-#define LOG_SKIP 2 // 20ms (50hz), log entry entered into fifo
-//#define LOG_SKIP 5 // 50ms (20hz), log entry entered into fifo
-
-Ticker sched; // scheduler for interrupt driven routines
-
-int control_count=CTRL_SKIP;
-int update_count=MAG_SKIP; // call Update_mag() every update_count calls to schedHandler()
-int log_count=0; // buffer a new status entry for logging every log_count calls to schedHandler
-int tReal; // calculate real elapsed time
-int bufCount=0;
-
-extern DigitalOut gpsStatus;
-
-// TODO: 3 better encapsulation, please
-extern Sensors sensors;
-extern SystemState state[SSBUF];
-extern unsigned char inState;
-extern unsigned char outState;
-extern bool ssBufOverrun;
-extern Mapping mapper;
-extern Steering steerCalc; // steering calculator
-extern Timer timer;
-extern DigitalOut ahrsStatus; // AHRS status LED
-
-// Navigation
-extern Config config;
-int nextWaypoint = 0; // next waypoint destination
-int lastWaypoint = 1;
-double bearing; // bearing to next waypoint
-double distance; // distance to next waypoint
-float steerAngle; // steering angle
-
-// Throttle PID
-float speedDt=0; // dt for the speed PID
-float integral=0; // error integral for speed PID
-float lastError=0; // previous error, used for calculating derivative
-bool go=false; // initiate throttle (or not)
-float desiredSpeed; // speed set point
-float nowSpeed;
-
-// Pose Estimation
-bool initNav = true; // initialize navigation estimates
-bool doLog = false; // determines when to start and stop entering log data
-float initialHeading=-999; // initial heading
-CartPosition cartHere; // position estimate, cartesian
-GeoPosition here; // position estimate, lat/lon
-int timeZero=0;
-int lastTime=-1; // used to calculate dt for KF
-int thisTime; // used to calculate dt for KF
-float dt; // dt for the KF
-float estLagHeading = 0; // lagged heading estimate
-//GeoPosition lagHere; // lagged position estimate; use here as the current position estimate
-float errHeading; // error between gyro hdg estimate and gps hdg estimate
-float gyroBias=0; // exponentially weighted moving average of gyro error
-float Ag = (2.0/(1000.0+1.0)); // Gyro bias filter alpha, gyro; # of 10ms steps
-float Kbias = 0.995;
-float filtErrRate = 0;
-float biasErrAngle = 0;
-
-#define MAXHIST 128 // must be multiple of 0x08
-#define inc(x) (x) = ((x)+1)&(MAXHIST-1)
-struct history_rec {
- float x; // x coordinate
- float y; // y coordinate
- float hdg; // heading
- float dist; // distance
- float gyro; // heading rate
- //float ghdg; // uncorrected gyro heading
- float dt; // delta time
-} history[MAXHIST]; // fifo for sensor data, position, heading, dt
-
-int hCount; // history counter; one > HDG_LAG, we can go back in time to reference gyro history
-int now = 0; // fifo input index, latest entry
-int prev = 0; // previous fifo iput index, next to latest entry
-int lag = 0; // fifo output index, entry from 1 second ago (HDG_LAG entries prior)
-int lagPrev = 0; // previous fifo output index, 101 entries prior
-
-/** attach update to Ticker */
-void startUpdater()
-{
- // Initialize logging buffer
- // Needs to happen after we've reset the millisecond timer and after
- // the schedHandler() fires off at least once more with the new time
- sched.attach(&update, UPDATE_PERIOD);
-}
-
-/** set flag to initialize navigation at next schedHandler() call */
-void restartNav()
-{
- go = false;
- inState = outState = 0;
- initNav = true;
- return;
-}
-
-/** instruct the controller to start running */
-void beginRun()
-{
- go = true;
- inState = outState = 0;
- timeZero = thisTime; // initialize
- bufCount = 0;
- return;
-}
-
-/** instruct the controller that we're done with the run */
-void endRun()
-{
- go = false;
- initNav = true;
- return;
-}
-
-/** get elasped time in update loop */
-int getUpdateTime()
-{
- return tReal;
-}
-
-/** Set the desired speed for the controller to attain */
-void setSpeed(float speed)
-{
- if (desiredSpeed != speed) {
- desiredSpeed = speed;
- integral = 0; // reset the error integral
- }
- return;
-}
-
-/** update() runs the data collection, estimation, steering control, and throttle control */
-void update()
-{
- tReal = timer.read_us();
- bool useGps=false;
-
- // TODO 1 do we need to set up the dt stuff after initialization?
-
- ahrsStatus = 0;
- thisTime = timer.read_ms();
- dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
- lastTime = thisTime;
-
- // Add up dt to speedDt
- // We're adding up distance over several update() calls so have to keep track of total time
- speedDt += dt;
-
- // Log Data Timestamp
- int timestamp = timer.read_ms();
-
- //////////////////////////////////////////////////////////////////////////////
- // NAVIGATION INIT
- //////////////////////////////////////////////////////////////////////////////
- // initNav is set with call to restartNav() when the "go" button is pressed. Up
- // to 10ms later this function is called and the code below will initialize the
- // dead reckoning position and estimated position variables
- //
- if (initNav == true) {
- initNav = false;
- here.set(config.wpt[0]);
- nextWaypoint = 1; // Point to the next waypoint; 0th wpt is the starting point
- lastWaypoint = 0;
-
- // TODO 1 need to start off, briefly, at a slow speed
-
- // Initialize lag estimates
- //lagHere.set( here );
- hCount = 2; // lag entry and first now entry are two entries
- now = 0;
- // initialize what will become lag data in 1 second from now
- history[now].dt = 0;
- history[now].dist = 0;
- // initial position is waypoint 0
- history[now].x = config.cwpt[0]._x;
- history[now].y = config.cwpt[0]._y;
- cartHere.set(history[now].x, history[now].y);
- // initialize heading to bearing between waypoint 0 and 1
- //history[now].hdg = here.bearingTo(config.wpt[nextWaypoint]);
- initialHeading = history[now].hdg = cartHere.bearingTo(config.cwpt[nextWaypoint]);
- //history[now].ghdg = history[now].hdg;
- // Initialize Kalman Filter
- headingKalmanInit(initialHeading);
- // point next fifo input to slot 1, slot 0 occupied/initialized, now
- lag = 0;
- lagPrev = 0;
- prev = now; // point to the most recently entered data
- now = 1; // new input slot
- }
-
-
- //////////////////////////////////////////////////////////////////////////////
- // SENSOR UPDATES
- //////////////////////////////////////////////////////////////////////////////
-
- // TODO 3 This really should run infrequently
- sensors.Read_Power();
-
- sensors.Read_Encoders();
- // really need to do some filtering on the speed
- //nowSpeed = 0.8*nowSpeed + 0.2*sensors.encSpeed;
- nowSpeed = sensors.encSpeed;
-
- sensors.Read_Gyro();
-
- sensors.Read_Rangers();
-
- sensors.Read_Accel();
-
- //sensors.Read_Camera();
-
- //////////////////////////////////////////////////////////////////////////////
- // Obtain GPS data
- //////////////////////////////////////////////////////////////////////////////
-
- // synchronize when RMC and GGA sentences received w/ AHRS
- // Do this in schedHandler?? GPS data is coming in not entirely in sync
- // with the logging info
- if (sensors.gps.available()) {
- // update system status struct for logging
- gpsStatus = !gpsStatus;
- state[inState].gpsLatitude = sensors.gps.latitude();
- state[inState].gpsLongitude = sensors.gps.longitude();
- state[inState].gpsHDOP = sensors.gps.hdop();
- state[inState].gpsCourse_deg = sensors.gps.heading_deg();
- state[inState].gpsSpeed_mps = sensors.gps.speed_mps(); // if need to convert from mph to mps, use *0.44704
- state[inState].gpsSats = sensors.gps.sat_count();
-
- // May 26, 2013, moved the useGps setting in here, so that we'd only use the GPS heading in the
- // Kalman filter when it has just been received. Before this I had a bug where it was using the
- // last known GPS data at every call to this function, meaning the more stale the GPS data, the more
- // it would likely throw off the GPS/gyro error term. Hopefully this will be a tad more acccurate.
- // Only an issue when heading is changing, I think.
-
- // GPS heading is unavailable from this particular GPS at speeds < 0.5 mph
- // Also, best to only use GPS if we've got at least 4 sats active -- really should be like 5 or 6
- // Finally, it takes 3-5 secs of runtime for the gps heading to converge.
- useGps = (state[inState].gpsSats > 4 &&
- state[inState].lrEncSpeed > 1.0 &&
- state[inState].rrEncSpeed > 1.0 &&
- (thisTime-timeZero) > 3000); // gps hdg converges by 3-5 sec.
- }
-
- DigitalOut useGpsStat(LED1);
- useGpsStat = useGps;
-
- //////////////////////////////////////////////////////////////////////////////
- // HEADING AND POSITION UPDATE
- //////////////////////////////////////////////////////////////////////////////
-
- // TODO: 2 Position filtering
- // position will be updated based on heading error from heading estimate
- // TODO: 2 Distance/speed filtering
- // this might be useful, but not sure it's worth the effort
-
- // So the big pain in the ass is that the GPS data coming in represents the
- // state of the system ~1s ago. Yes, a full second of lag despite running
- // at 10hz (or whatever). So if we try and fuse a lagged gps heading with a
- // relatively current gyro heading rate, the gyro is ignored and the heading
- // estimate lags reality
- //
- // In real life testing, the robot steering control was highly unstable with
- // too much gain (typical of any negative feedback system with a very high
- // phase shift and too much feedback). It'd drive around in circles trying to
- // hunt for the next waypoint. Dropping the gain restored stability but the
- // steering overshot significantly, because of the lag. It sort of worked but
- // it was ugly. So here's my lame attempt to fix all this.
- //
- // We'll find out how much error there is between gps heading and the integrated
- // gyro heading from a second ago.
-
- // Save current data into history fifo to use 1 second from now
- history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
- history[now].gyro = sensors.gyro[_z_]; // current raw gyro data
- history[now].dt = dt; // current dt, to address jitter
- history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro - gyroBias) ); // compute current heading from current gyro
- float r = PI/180 * history[now].hdg;
- history[now].x = history[prev].x + history[now].dist * sin(r); // update current position
- history[now].y = history[prev].y + history[now].dist * cos(r);
-
- // We can't do anything until the history buffer is full
- if (hCount < HDG_LAG) {
- estLagHeading = history[now].hdg;
- // Until the fifo is full, only keep track of current gyro heading
- hCount++; // after n iterations the fifo will be full
- } else {
- // Now that the buffer is full, we'll maintain a Kalman Filter estimate that is
- // time-shifted to the past because the GPS output represents the system state from
- // the past. We'll also correct our history of gyro readings from the past to the
- // present
-
- ////////////////////////////////////////////////////////////////////////////////
- // UPDATE LAGGED ESTIMATE
-
- // Recover data from 1 second ago which will be used to generate
- // updated lag estimates
-
- // This represents the best estimate for heading... for one second ago
- // If we do nothing else, the robot will think it is located at a position 1 second behind
- // where it actually is. That means that any control feedback needs to have a longer time
- // constant than 1 sec or the robot will have unstable heading correction.
- // Use gps data when available, always use gyro data
-
- // Clamp heading to initial heading when we're not moving; hopefully this will
- // give the KF a head start figuring out how to deal with the gyro
- //
- // TODO 1 maybe we should only call the gps version after moving
- if (go) {
- estLagHeading = headingKalman(history[lag].dt, state[inState].gpsCourse_deg, useGps, history[lag].gyro, true);
- } else {
- estLagHeading = headingKalman(history[lag].dt, initialHeading, true, history[lag].gyro, true);
- }
-
- // TODO 1 are we adding history lag to lagprev or should we add lag+1 to lag or what?
- // Update the lagged position estimate
- history[lag].x = history[lagPrev].x + history[lag].dist * sin(estLagHeading);
- history[lag].y = history[lagPrev].y + history[lag].dist * cos(estLagHeading);
-
- ////////////////////////////////////////////////////////////////////////////////
- // UPDATE CURRENT ESTIMATE
-
- // Now we need to re-calculate the current heading and position, starting with the most recent
- // heading estimate representing the heading 1 second ago.
- //
- // Nuance: lag and now have already been incremented so that works out beautifully for this loop
- //
- // Heading is easy. Original heading - estimated heading gives a tiny error. Add this to all the historical
- // heading data up to present.
- //
- // For position re-calculation, we iterate HDG_LAG times up to present record. Haversine is way, way too slow,
- // trig calcs is marginal. Update rate is 10ms and we can't hog more than maybe 2-3ms as the outer
- // loop has logging work to do. Rotating each point is faster; pre-calculate sin/cos, etc. for rotation
- // matrix.
-
- // Low pass filter the error correction. Multiplying by 0.01, it takes HDG_LAG updates to correct a
- // consistent error; that's 0.10s/0.01 = 1 sec. 0.005 is 2 sec, 0.0025 is 4 sec, etc.
- errHeading = clamp180(estLagHeading - history[lag].hdg) * 0.01; // lopass filter error angle
-
- //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f\n", lag, estLagHeading, history[lag].hdg, estLagHeading - history[lag].hdg, errHeading);
- float cosA = cos(errHeading * PI / 180.0);
- float sinA = sin(errHeading * PI / 180.0);
- // Update position & heading from history[lag] through history[now]
- int i = lag;
- // TODO 2 parameterize heading lag -- for uBlox it seems to be ~ 600ms, for Venus, about 1000ms
- for (int j=0; j < HDG_LAG; j++) {
- //fprintf(stdout, "i=%d n=%d l=%d\n", i, now, lag);
- // Correct gyro-calculated headings from past to present including history[lag].hdg
- history[i].hdg = clamp360(history[i].hdg + errHeading);
- //if (history[i].hdg >= 360.0) history[i].hdg -= 360.0;
- //if (history[i].hdg < 0) history[i].hdg += 360.0;
- // Rotate x, y by errHeading around pivot point; no need to rotate pivot point (j=0)
- if (j > 0) {
- float dx = history[i].x-history[lag].x;
- float dy = history[i].y-history[lag].y;
- // rotation matrix
- history[i].x = history[lag].x + dx*cosA - dy*sinA;
- history[i].y = history[lag].y + dx*sinA + dy*cosA;
- }
- inc(i);
- }
- // increment lag pointer and wrap
- lagPrev = lag;
- inc(lag);
- }
- // "here" is the current position
- cartHere.set(history[now].x, history[now].y);
-
- //////////////////////////////////////////////////////////////////////////////
- // NAVIGATION UPDATE
- //////////////////////////////////////////////////////////////////////////////
-
- bearing = cartHere.bearingTo(config.cwpt[nextWaypoint]);
- distance = cartHere.distanceTo(config.cwpt[nextWaypoint]);
- float prevDistance = cartHere.distanceTo(config.cwpt[lastWaypoint]);
-
- // if within config.waypointDist distance threshold move to next waypoint
- // TODO 3 figure out how to output feedback on wpt arrival external to this function
- if (go) {
-
- // if we're within brakeDist of next or previous waypoint, run @ turn speed
- // This would normally mean we run at turn speed until we're brakeDist away
- // from waypoint 0, but we trick the algorithm by initializing prevWaypoint to waypoint 1
- if ( (thisTime - timeZero) < 3000 ) {
- setSpeed( config.startSpeed );
- } else if (distance < config.brakeDist || prevDistance < config.brakeDist) {
- setSpeed( config.turnSpeed );
- } else {
- setSpeed( config.topSpeed );
- }
-
- if (distance < config.waypointDist) {
- //fprintf(stdout, "Arrived at wpt %d\n", nextWaypoint);
- //speaker.beep(3000.0, 0.5); // non-blocking
- lastWaypoint = nextWaypoint;
- nextWaypoint++;
- }
-
- } else {
- setSpeed( 0.0 );
- }
- // Are we at the last waypoint?
- // currently handled external to this routine
-
- //////////////////////////////////////////////////////////////////////////////
- // OBSTACLE DETECTION & AVOIDANCE
- //////////////////////////////////////////////////////////////////////////////
- // TODO 2 limit steering angle based on object detection ?
- // or limit relative brg perhaps?
- // TODO 2 add vision obstacle detection and filtering
- // TODO 2 add ranger obstacle detection and filtering/fusion with vision
-
-
- //////////////////////////////////////////////////////////////////////////////
- // CONTROL UPDATE
- //////////////////////////////////////////////////////////////////////////////
-
- if (--control_count == 0) {
-
- steerAngle = steerCalc.pathPursuitSA(history[now].hdg, history[now].x, history[now].y,
- config.cwpt[lastWaypoint]._x, config.cwpt[lastWaypoint]._y,
- config.cwpt[nextWaypoint]._x, config.cwpt[nextWaypoint]._y);
-
- // Apply gain factor for near straight line
- // TODO 3 figure out a better, continuous way to deal with steering gain
- if (fabs(steerAngle) < config.steerGainAngle) steerAngle *= config.steerGain;
-
- // Curb avoidance
- /*
- if (sensors.rightRanger < config.curbThreshold) {
- steerAngle -= config.curbGain * (config.curbThreshold - sensors.rightRanger);
- }
- */
-
- setSteering( steerAngle );
-
- // PID control for throttle
- // TODO: 3 move all this PID crap into Actuators.cpp
- // TODO: 3 probably should do KF or something for speed/dist; need to address GPS lag, too
- // if nothing else, at least average the encoder speed over mult. samples
- if (desiredSpeed <= 0.1) {
- setThrottle( config.escZero );
- } else {
- // PID loop for throttle control
- // http://www.codeproject.com/Articles/36459/PID-process-control-a-Cruise-Control-example
- float error = desiredSpeed - nowSpeed;
- // track error over time, scaled to the timer interval
- integral += (error * speedDt);
- // determine the amount of change from the last time checked
- float derivative = (error - lastError) / speedDt;
- // calculate how much to drive the output in order to get to the
- // desired setpoint.
- int output = config.escZero + (config.speedKp * error) + (config.speedKi * integral) + (config.speedKd * derivative);
- if (output > config.escMax) output = config.escMax;
- if (output < config.escMin) output = config.escMin;
- //fprintf(stdout, "s=%.1f d=%.1f o=%d\n", nowSpeed, desiredSpeed, output);
- setThrottle( output );
- // remember the error for the next time around.
- lastError = error;
- }
-
- speedDt = 0; // reset dt to begin counting for next time
- control_count = CTRL_SKIP;
- }
-
- //////////////////////////////////////////////////////////////////////////////
- // DATA FOR LOGGING
- //////////////////////////////////////////////////////////////////////////////
-
- // Periodically, we enter a new SystemState into the FIFO buffer
- // The main loop handles logging and will catch up to us provided
- // we feed in new log entries slowly enough.
- // log_count initialized to 0 to begin with forcing initialization below
- // but no further updates until in go mode
- if (go && (log_count > 0)) --log_count;
- // Only enter new SystemState data when in "go" mode (armed to run, run,
- // and end run)
- // We should really only be adding to the state fifo if we're in 'go' mode
- if (log_count <= 0) {
- // Copy data into system state for logging
- inState++; // Get next state struct in the buffer
- inState &= SSBUF; // Wrap around
- ssBufOverrun = (inState == outState);
- // Need to clear out encoder distance counters; these are incremented
- // each time this routine is called.
- // TODO 1 is there anything we can't clear out?
- // TODO 1 use clearState()
- state[inState].lrEncDistance = 0;
- state[inState].rrEncDistance = 0;
- // initialize gps data
- state[inState].gpsLatitude = 0;
- state[inState].gpsLongitude = 0;
- state[inState].gpsHDOP = 0;
- state[inState].gpsCourse_deg = 0;
- state[inState].gpsSpeed_mps = 0;
- state[inState].gpsSats = 0;
- log_count = LOG_SKIP; // reset counter
- bufCount++;
- }
-
- // Log Data Timestamp
- state[inState].millis = timestamp;
-
- // TODO: 3 recursive filtering on each of the state values
- state[inState].voltage = sensors.voltage;
- state[inState].current = sensors.current;
- for (int i=0; i < 3; i++) {
- state[inState].m[i] = sensors.m[i];
- state[inState].g[i] = sensors.g[i];
- state[inState].gyro[i] = sensors.gyro[i];
- state[inState].a[i] = sensors.a[i];
- }
- state[inState].gTemp = sensors.gTemp;
- state[inState].lrEncSpeed = sensors.lrEncSpeed;
- state[inState].rrEncSpeed = sensors.rrEncSpeed;
- state[inState].lrEncDistance += sensors.lrEncDistance;
- state[inState].rrEncDistance += sensors.rrEncDistance;
- //state[inState].encHeading += (state[inState].lrEncDistance - state[inState].rrEncDistance) / TRACK;
- state[inState].estHeading = history[now].hdg; // gyro heading estimate, current, corrected
- state[inState].estLagHeading = history[lag].hdg; // gyro heading, estimate, lagged
- mapper.cartToGeo(cartHere, &here);
- state[inState].estLatitude = here.latitude();
- state[inState].estLongitude = here.longitude();
- state[inState].estX = history[now].x;
- state[inState].estY = history[now].y;
- state[inState].bearing = bearing;
- state[inState].distance = distance;
- state[inState].nextWaypoint = nextWaypoint;
- state[inState].gbias = gyroBias;
- state[inState].errHeading = errHeading;
- //state[inState].leftRanger = sensors.leftRanger;
- //state[inState].rightRanger = sensors.rightRanger;
- //state[inState].centerRanger = sensors.centerRanger;
- state[inState].steerAngle = steerAngle;
- // Copy AHRS data into logging data
- //state[inState].roll = ToDeg(ahrs.roll);
- //state[inState].pitch = ToDeg(ahrs.pitch);
- //state[inState].yaw = ToDeg(ahrs.yaw);
-
- // increment fifo pointers with wrap-around
- prev = now;
- inc(now);
-
- // timing
- tReal = timer.read_us() - tReal;
-
- ahrsStatus = 1;
-}
\ No newline at end of file
--- a/updater.h Thu Nov 29 18:34:22 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,41 +0,0 @@ -#ifndef __SCHEDULER_H -#define __SCHEDULER_H - -/** Updater is the main real time sensor update, estimation, and control routine that is - * called at 100Hz by a timer interrupt - */ - -/** initialize throttle to center position */ -void initThrottle(void); - -/** initialize steering to center position */ -void initSteering(void); - -/** attach the update routine to Ticker interrupt */ -void startUpdater(void); - -/** detach the update routine from Ticker interrupt */ -void stopUpdater(void); - -/** Returns the elapsed time taken by the updater routine on its most recent run */ -int getUpdateTime(void); - -/** Indicates to the updater that the vehicle should begin its run */ -void beginRun(void); - -/** Indicates to the updater that the vehicle should abort its run */ -void endRun(void); - -/** Tells the updater to re-initialize the navigation state */ -void restartNav(void); - -/** Sets the desired speed of the rover */ -void setSpeed(float speed); - -/** Sets the steering angle */ -void setSteering(float steerAngle); - -/** The function that is called at 100Hz. It reads sensors, performs estimation, and controls the robot */ -void update(void); - -#endif