Michael Shimniok
The software I used to run my Sparkfun AVC 2011 entry, a 1:10 scale RC truck called \"Data Bus\". This is the final revision of the code as-run on April 23, 2011, the date of the contest, on the 3rd heat.
Dependencies: TinyCHR6dm PinDetect mbed IncrementalEncoder Servo Schedule DebounceIn SimpleFilter LSM303DLH Steering
Diff: main.cpp
- Revision:
- 0:9b27ebe1ce17
diff -r 000000000000 -r 9b27ebe1ce17 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Apr 27 19:23:43 2011 +0000
@@ -0,0 +1,1485 @@
+/** Code for "Data Bus" UGV entry for Sparkfun AVC 2011
+ * http://bot-thoughts.com/
+ */
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// INCLUDES
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#include "mbed.h"
+#include "TinyGPS.h"
+#include "SDFileSystem.h"
+#include "ADC128S.h"
+#include "PinDetect.h"
+#include "LSM303DLH.h"
+#include "Servo.h"
+#include "IncrementalEncoder.h"
+#include "Steering.h"
+#include "Schedule.h"
+#include "GeoPosition.h"
+#include "TinyCHR6dm.h"
+#include "SimpleFilter.h"
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// DEFINES
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#define WHERE(x) debug->printf("%d\n", __LINE__);
+
+#define clamp360(x) \
+ while ((x) >= 360.0) (x) -= 360.0; \
+ while ((x) < 0) (x) += 360.0;
+#define clamp180(x) ((x) - floor((x)/360.0) * 360.0 - 180.0);
+
+#define absf(x) (x *= (x < 0.0) ? -1 : 1)
+
+#define UPDATE_PERIOD 50 // update period in ms
+#define GYRO_UPDATE UPDATE_PERIOD/5 // gyro update period in ms
+
+#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
+
+// Error correction gains
+#define COMPASS_GAIN 0.25
+#define YAW_GAIN 0.25
+
+// 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
+
+// Waypoint queue parameters
+#define MAXWPT 10
+#define ENDWPT 9999.0
+
+// Chassis specific parameters
+#define WHEEL_CIRC 0.321537 // m; calibrated with 4 12.236m runs. Measured 13.125" or 0.333375 wheel dia
+#define WHEELBASE 0.290
+#define TRACK 0.280
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// GLOBAL OBJECTS
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// OUTPUT
+DigitalOut confStatus(LED1); // Config file status LED
+DigitalOut logStatus(LED2); // Log file status LED
+DigitalOut gps2Status(LED3); // GPS fix status LED
+DigitalOut ahrsStatus(LED4); // GPS fix status LED
+DigitalOut sonarStart(p18); // Sends signal to start sonar array pings
+DigitalOut flasher(p10); // Autonomous mode warning flasher
+DigitalOut steerServo(p21); // After shutdown, pass pulses through to servo
+//Serial lcd(p9, NC); // LCD module, TX only
+
+// INPUT
+InterruptIn receiver(p23); // RC Receiver steering channel
+DigitalOut buttonPower(p19); // Power the button(s)
+//PinDetect upButton(p17);
+PinDetect selectButton(p20); // Input selectButton
+//PinDetect downButton(p18);
+
+// VEHICLE
+Servo steering(p21); // Steering Servo
+Servo throttle(p22); // ESC
+Schedule go; // Throttle profile, dead stop to full speed
+Schedule stop; // Throttle profile, full speed to dead stop
+Steering steerCalc(TRACK, WHEELBASE); // steering calculator
+int maxSpeed=520; // Servo setting for max speed
+
+// SENSORS
+//HMC6352 compass(p28, p27); // Driver for compass
+LSM303DLH compass3d(p28, p27); // Driver for compass
+//I2C cam(p28, p27); // CMUcam I2C bridge
+//ADC128S adc(p5, p6, p7, p15); // ADC128S102 8ch ADC driver; mosi, miso, sclk, cs
+ADC128S adc(p11, p12, p13, p14); // ADC128S102 8ch ADC driver; mosi, miso, sclk, cs
+IncrementalEncoder left(p30); // Left wheel encoder
+IncrementalEncoder right(p29); // Right wheel encoder
+//Serial gps1(p26, p25); // GPS1, Locosys LS20031
+Serial ahrs(p26, p25); // CHR-6dm AHRS
+Serial gps2(p9, p10); // GPS2, iGPS-500
+Serial *dev; // For use with bridge
+//TinyGPS gps1Parse; // GPS NMEA parser
+TinyCHR6dm ahrsParser; // CHR-6dm AHRS parser
+TinyGPS gps2Parse; // GPS NMEA parser
+
+// COMM
+Serial pc(USBTX, USBRX); // PC usb communications
+Serial *debug = &pc;
+
+// MISC
+Timer timer; // For main loop scheduling
+SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs
+LocalFileSystem local("etc"); // Create the local filesystem under the name "local"
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// GLOBAL VARIABLES
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// GPS Variables
+double gps1_lat; // latitude
+double gps1_lon; // longitude
+double gps2_lat; // latitude
+double gps2_lon; // longitude
+unsigned long age = 0; // gps fix age
+float gps1_hdop = 0.0; // gps horizontal dilution of precision
+float gps2_hdop = 0.0; // gps horizontal dilution of precision
+int year; // gps date variables
+byte month;
+byte day;
+byte hour;
+byte minute;
+byte second;
+byte hundredths;
+
+// schedule for LED warning flasher
+Schedule blink;
+
+// Useful globals
+float declination; // compass declination for local correction
+bool goGoGo = false; // signal to start moving
+bool done = false; // signal that we're done navigating
+
+// Gyro Variables
+float gyro = 0; // gyro reading
+float gyroBias = 2027.0; // gyro bias in raw 12-bit ADC value
+unsigned int temp = 0; // gyro temp reading
+float gyroSens = 4.89; // in LSB/deg/sec since we're using ratiometric ADC and gyro
+float gyroSum[UPDATE_PERIOD/GYRO_UPDATE]; // for summijng/averaging gyro between DR / compass updates
+int gyroCount = 0; // counter for gyroSum array
+
+// Navigation Variables
+GeoPosition wpt[MAXWPT]; // course waypoints
+unsigned int wptCount = 0; // number of waypoints configured
+int wptCurrent = 0; // current waypoint
+GeoPosition here1; // current gps position
+GeoPosition here2; // current gps position
+GeoPosition dr_here; // current dead reckoning position
+GeoPosition dr_here_last; // DR position at last GPS packet
+
+// Misc
+FILE *fp = 0;
+bool buttonPressed = false;
+bool buttonReleased = false;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// FUNCTION DEFINITIONS
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+FILE *initLogfile(void);
+void initButtons(void);
+void initCompass(void);
+void initGPS(void);
+void initAHRS(void);
+void initFlasher(void);
+void initSteering(void);
+void initThrottle(void);
+void initDR(void);
+void loadConfig(float &interceptDist, float &declination, float &compassGain, float &yawGain);
+void doGPS(TinyGPS &parse, GeoPosition &here, DigitalOut status, char *date, char *time);
+void autonomousMode(void);
+void findGPSBias(double *brg, double *dist, int n, GeoPosition place);
+void idleMode(void);
+void compassCalibrate(void);
+void servoCalibrate(void);
+void serialBridge(Serial &gps);
+void instrumentCheck(void);
+float compassHeading(void);
+float gyroRate(unsigned int adc);
+float sonarDistance(unsigned int adc);
+float irDistance(unsigned int adc);
+
+// 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.
+//
+void assertButton() {
+ buttonPressed = true;
+ buttonReleased = false;
+}
+
+void deassertButton() {
+ buttonPressed = false;
+ buttonReleased = true;
+}
+
+// Use interrupt handler to receive AHRS serial comm and parse
+// it using TinyCHR6dm library
+//
+void recv1() {
+ while (ahrs.readable())
+ ahrsParser.parse(ahrs.getc());
+}
+
+void recv2() {
+ while (gps2.readable())
+ gps2Parse.encode(gps2.getc());
+}
+
+char getInput(const char *prompt)
+{
+ char c;
+
+ if (prompt) pc.printf("%s ", prompt);
+ while (!pc.readable())
+ wait(0.001);
+ c = pc.getc();
+ pc.printf("%c\n", c);
+
+ return c;
+}
+
+int main()
+{
+ // Send data back to the PC
+ pc.baud(115200);
+
+ //receiver.rise(&killSwitch); // Detects remote kill switch / rc takeover
+ initButtons(); // Initialize input buttons
+ initCompass();
+ initSteering();
+ initThrottle();
+ // initFlasher(); // Initialize autonomous mode flasher
+ initGPS();
+
+ // Insert menu system here w/ timeout
+ bool autoBoot=true;
+/*
+ pc.printf("Booting to autonomous mode in 10 seconds, any key to abort.\n ");
+ for (int i=1; i > 0; i--) {
+ pc.printf("%d...", i);
+ if (pc.readable()) {
+ autoBoot = false;
+ while (pc.readable()) pc.getc(); // clear buffer
+ break;
+ }
+ wait(1.0);
+ }
+ pc.printf("\n");
+*/
+
+ char cmd;
+
+ while (1) {
+
+ if (autoBoot) {
+ autoBoot = false;
+ cmd = 'a';
+ } else {
+ pc.printf("==============\nData Bus Menu\n==============\n");
+ pc.printf("(a)utonomous mode\n");
+ pc.printf("(s)erial bridge\n (1) Port 1 (2) Port 2 (GPS)\n");
+ pc.printf("(c)ompass calibration\n");
+ pc.printf("(i)nstrument check\n");
+ //pc.printf("(s)ervo calibration\n");
+ cmd = getInput("\nSelect from the above:");
+ }
+
+ switch (cmd) {
+ case 'a' :
+ autonomousMode();
+ break;
+ case 's' :
+ // which GPS
+ cmd = getInput("GPS2 (2) or AHRS(1)?");
+ switch (cmd) {
+ case '1' :
+ serialBridge(ahrs);
+ break;
+ case '2' :
+ serialBridge(gps2);
+ break;
+ default :
+ break;
+ }
+ break;
+ case 'i' :
+ instrumentCheck();
+ break;
+ case 'c' :
+ compassCalibrate();
+ break;
+ default :
+ break;
+ }
+ }
+
+}
+
+// Handle data from a GPS (there may be two GPS's so needed to put the code in one routine
+//
+void doGPS(TinyGPS &parse, GeoPosition &here, DigitalOut status, char *date, char *time)
+{
+ double lat, lon;
+ unsigned long age;
+
+ parse.reset_ready(); // reset the flags
+ //pc.printf("%d GPS RMC are ready\n", millis);
+ parse.f_get_position(&lat, &lon, &age);
+ parse.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
+
+ sprintf(date, "%02d/%02d/%4d", month, day, year);
+ sprintf(time, "%02d:%02d:%02d.%03d", hour, minute, second, hundredths);
+
+ float hdop = parse.f_hdop();
+
+ // Want to blink the LED when GPS update arrives
+ // must toggle opposite
+ //status = (hdop > 0.0 && hdop < 10.0) ? 0 : 1;
+
+ // Bearing and distance to waypoint
+ here.set(lat, lon);
+
+ //pc.printf("HDOP: %.1f gyro: %d\n", gps1_hdop, gyro);
+
+ return;
+}
+
+
+
+// convert character to an int
+//
+int ctoi(char c)
+{
+ int i=-1;
+
+ if (c >= '0' && c <= '9') {
+ i = c - '0';
+ }
+
+ //printf("char: %c int %d\n", c, i);
+
+ return i;
+}
+
+
+// convert string to floating point
+//
+double cvstof(char *s)
+{
+ double f=0.0;
+ double mult = 0.1;
+ bool neg = false;
+ //char dec = 1;
+
+ // leading spaces
+ while (*s == ' ' || *s == '\t') {
+ s++;
+ if (*s == 0) break;
+ }
+
+ // What about negative numbers?
+ if (*s == '-') {
+ neg = true;
+ s++;
+ }
+
+ // before the decimal
+ //
+ while (*s != 0) {
+ if (*s == '.') {
+ s++;
+ break;
+ }
+ f = (f * 10.0) + (double) ctoi(*s);
+ s++;
+ }
+ // after the decimal
+ while (*s != 0 && *s >= '0' && *s <= '9') {
+ f += (double) ctoi(*s) * mult;
+ mult /= 10;
+ s++;
+ }
+
+ // if we were negative...
+ if (neg) f = -f;
+
+ return f;
+}
+
+// copy t to s until delimiter is reached
+// return location of delimiter+1 in t
+char *split(char *s, char *t, int max, char delim)
+{
+ int i = 0;
+
+ if (s == 0 || t == 0)
+ return 0;
+
+ while (*t != 0 && *t != delim && i < max) {
+ *s++ = *t++;
+ i++;
+ }
+ *s = 0;
+
+ return t+1;
+}
+
+#define MAXBUF 64
+// load configuration from filesystem
+void loadConfig(float &interceptDist, float &declination, float &compassGain, float &yawGain)
+{
+// FILE *fp;
+ char buf[MAXBUF]; // buffer to read in data
+ char tmp[MAXBUF]; // temp buffer
+ char *p;
+ double lat, lon;
+ bool declFound = false;
+
+ // Just to be safe let's wait
+ //wait(2.0);
+
+ pc.printf("opening config file...\n");
+
+ fp = fopen("/etc/config.txt", "r");
+ if (fp == 0) {
+ pc.printf("Could not open config.txt\n");
+ } else {
+ wptCount = 0;
+ declination = 0.0;
+ while (!feof(fp)) {
+ fgets(buf, MAXBUF-1, fp);
+ p = split(tmp, buf, MAXBUF, ','); // split off the first field
+ switch (tmp[0]) {
+ case 'W' : // 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);
+ if (wptCount < MAXWPT) {
+ wpt[wptCount].set(lat, lon);
+ wptCount++;
+ }
+ break;
+ case 'G' : // Gyro Bias
+ p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp
+ gyroBias = (float) cvstof(tmp);
+ break;
+ case 'D' : // Compass Declination
+ p = split(tmp, p, MAXBUF, ','); // split off the declination to tmp
+ declination = (float) cvstof(tmp);
+ declFound = true;
+ break;
+ case 'I' : // Intercept distance
+ p = split(tmp, p, MAXBUF, ','); // split off the number to tmp
+ interceptDist = (float) cvstof(tmp);
+ break;
+ case 'S' : // Speed maximum
+ p = split(tmp, p, MAXBUF, ','); // split off the number to tmp
+ maxSpeed = atoi(tmp);
+ //pc.printf("tmp:%s maxSpeed:%d\n", tmp, maxSpeed);
+ break;
+ case 'E' :
+ p = split(tmp, p, MAXBUF, ','); // split off the number to tmp
+ compassGain = (float) cvstof(tmp);
+ p = split(tmp, p, MAXBUF, ','); // split off the number to tmp
+ yawGain = (float) cvstof(tmp);
+ default :
+ break;
+ } // switch
+ } // while
+
+ // Did we get the values we were looking for?
+ if (wptCount > 0 && declFound) {
+ confStatus = 1;
+ }
+
+ } // if fp
+
+ if (fp != 0)
+ fclose(fp);
+
+ pc.printf("Intercept Dist: %.1f\n", interceptDist);
+ pc.printf("Declination: %.1f\n", declination);
+ pc.printf("MaxSpeed: %d\n", maxSpeed);
+ pc.printf("CompassGain; %.3f YawGain: %.3f\n", compassGain, yawGain);
+ for (int w = 0; w < MAXWPT && w < wptCount; w++) {
+ pc.printf("Waypoint #%d lat: %.6f lon: %.6f\n", w, wpt[w].latitude(), wpt[w].longitude());
+ }
+
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// INITIALIZATION ROUTINES
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// Find the next unused filename of the form logger##.csv where # is 0-9
+//
+FILE *initLogfile()
+{
+ FILE *fp = 0;
+ char myname[64];
+
+ pc.printf("Opening log file...\n");
+
+ while (fp == 0) {
+ if ((fp = fopen("/log/test.txt", "r")) == 0) {
+ pc.printf("Waiting for filesystem to come online...");
+ wait(0.200);
+ }
+ }
+ fclose(fp);
+
+ for (int i = 0; i < 1000; i++) {
+ sprintf(myname, "/log/log%04d.csv", i);
+ //pc.printf("Try file: <%s>\n", myname);
+ if ((fp = fopen(myname, "r")) == 0) {
+ //pc.printf("File not found: <%s>\n", myname);
+ break;
+ } else {
+ //pc.printf("File exists: <%s>\n", myname);
+ fclose(fp);
+ }
+ }
+
+ fp = fopen(myname, "w");
+ if (fp == 0) {
+ pc.printf("file write failed: %s\n", myname);
+ } else {
+ //status = true;
+ pc.printf("opened %s for writing\n", myname);
+ fprintf(fp, "Millis, Gyro, GyroHdg, CompassHeading, Latitude, Longitude, Age, Date, Time, Altitude, Course, Speed, HDOP, Bearing, Distance, Left Enc, Right Enc, Gyro Temp, OdoHdg\n");
+ //fclose(fp);
+ }
+
+ return fp;
+}
+
+
+void initButtons()
+{
+ // Set up button (plugs into two GPIOs, active low
+ buttonPower = 0;
+ selectButton.mode(PullUp);
+ selectButton.setSamplesTillAssert(20);
+ selectButton.setAssertValue(0); // active low logic
+ selectButton.setSampleFrequency(20); // 20us
+ selectButton.attach_asserted( &assertButton );
+ selectButton.attach_deasserted( &deassertButton );
+}
+
+void initCompass()
+{
+ // Initialize compass; continuous mode, periodic set/reset, 20Hz measurement rate.
+ //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+
+ // Set calibration parameters for this particular LSM303DLH
+ //compass3d.setOffset(29.50, -0.50, 4.00);
+ //compass3d.setScale(1.00, 1.03, 1.21);
+ compass3d.setOffset(44.50, 5.00, -0.50); // Apr 11 testing
+ compass3d.setScale(1.00, 1.04, 1.29); // Apr 11 testing
+
+ //compass3d._debug = &pc;
+}
+
+
+void initGPS()
+{
+ // Initialize the GPS comm and handler
+ //gps1.baud(57600); // LOCOSYS LS20031
+
+ // Set LCD baud rate ; has to be 4800
+ // because we share with 4800 bps GPS\
+ // send chr(124) and CTRL-L
+ //gps2.baud(9600);
+ //gps2.printf("Data Bus");
+ //gps2.printf("%c%c", 124, 12);
+
+ gps2.baud(4800); // Pharos iGPS-500
+
+ // Synchronize with GPS
+ //gps1Parse.reset_ready();
+ gps2Parse.reset_ready();
+}
+
+void initAHRS()
+{
+ char data[MAX_BYTES];
+ int status;
+ int ok = 0; // counts number of command_complete messages
+ int c = 10; // timeout for status
+
+ ahrs.baud(115200);
+ ahrs.attach(recv1, Serial::RxIrq);
+ wait(0.5);
+
+ // Configure AHRS to use only acceleromters and gyro, no magnetometer
+ data[0] = Accel_EN;
+ ahrsParser.send_packet(&ahrs, SET_EKF_CONFIG, 1, data);
+ c = 10;
+ while (!ahrsParser.statusReady() && c-- > 0)
+ wait(0.1);
+ status = ahrsParser.status();
+ if (status == PT_COMMAND_COMPLETE) ok++;
+ pc.printf("SET_EKF_CONFIG: %02x %s\n", status, ahrsParser.statusString(status));
+
+ ahrsParser.send_packet(&ahrs, ZERO_RATE_GYROS, 0, 0);
+ c = 10;
+ while (!ahrsParser.statusReady() && c-- > 0)
+ wait(0.1);
+ status = ahrsParser.status();
+ if (status == PT_COMMAND_COMPLETE) ok++;
+ pc.printf("ZERO_RATE_GYROS: %02x %s\n", status, ahrsParser.statusString(status));
+
+ data[0] = 0; // 20Hz
+ ahrsParser.send_packet(&ahrs, SET_BROADCAST_MODE, 1, data);
+ c = 10;
+ while (!ahrsParser.statusReady() && c-- > 0)
+ wait(0.1);
+ status = ahrsParser.status();
+ if (status == PT_COMMAND_COMPLETE) ok++;
+ pc.printf("SET_BROADCAST_MODE: %02x %s\n", status, ahrsParser.statusString(status));
+
+ ahrsParser.send_packet(&ahrs, EKF_RESET, 0, 0);
+ c = 10;
+ while (!ahrsParser.statusReady() && c-- > 0)
+ wait(0.1);
+ status = ahrsParser.status();
+ if (status == PT_COMMAND_COMPLETE) ok++;
+ pc.printf("EKF_RESET: %02x %s\n", status, ahrsParser.statusString(status));
+ //ahrs.printf("%s", ahrsParser.send_packet()) // <--mag calibration here//
+
+ if (ok == 4) ahrsStatus = 1; // turn on status LED
+
+ ahrsParser.resetReady();
+}
+
+
+void initDR()
+{
+ dr_here.set(wpt[0]); // Initialize Dead Reckoning to starting waypoint
+ dr_here_last.set(wpt[0]);
+}
+
+
+void initFlasher()
+{
+ // Set up flasher schedule; 3 flashes every 80ms
+ // for 80ms total, with a 9x80ms period
+ blink.max(9);
+ blink.scale(80);
+ blink.mode(Schedule::repeat);
+ blink.set(0, 1); blink.set(2, 1); blink.set(4, 1);
+}
+
+
+void initSteering()
+{
+ // Setup steering servo
+ steering = 0.5;
+ steering.calibrate(0.005, 45.0);
+}
+
+
+void initThrottle()
+{
+ throttle = 0.5;
+ throttle.calibrate(0.0005, 45.0);
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// OPERATIONAL MODE FUNCTIONS
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+void autonomousMode()
+{
+ // Navigation
+ float interceptDist = 1.5; // Course correction intercept distance ; GPS_MAX_HDOP calculates to 20* SA
+
+ // Compass Variables
+ float compassHdg = 0.0; // compass heading
+ float compassHdg_last = -999; // heading at last GPS packet
+ float compassGain = COMPASS_GAIN;
+
+ // AHRS Variables
+ float yaw = 0.0; // course
+ float yawHdg = -1.0; // heading calculated from AHRS
+ float yawRate = 0.0; // calculated rate of change in yaw
+ float initialHdg = -999; // initial heading (course)
+ float yawGain = YAW_GAIN;
+
+ // Variables we calculate
+ double bearing = 0.0; // bearing to next waypoint
+ double distance = 0.0; // distance to next waypoint
+ float heading = 0.0; // estimated heading
+ float relativeBrg = 0.0; // relative bearing to next waypoint
+ double gps2_bearing = 0.0; // bearing, gps2
+ double gps2_distance = 0.0; // distance, gps2
+ float gps2_heading = 0.0; // current heading, gps2
+ double dr_bearing = 0.0; // bearing, dead reckoning
+ double dr_distance = 0.0; // distance, dead reckoning
+ float dr_heading = 0.0; // dead reckoning heading
+ float odo_heading = 0.0; // heading calc from odometry
+ double err_bearing = 0.0; // bearing between dr and gps
+ double err_distance = 0.0; // distance between dr and gps
+ float err_yaw = 0.0; // error in ahrs yaw versus gps heading.
+ float err_compass = 0.0; // error in compass versus gps heading
+ double gps2_off_dist = 0.0; // distance from gps fix to starting point
+ double gps2_off_brg = 0.0; // bearing from gps fix to starting point
+ double encDistance = 0.0; // encoder average distance recorded
+ double encSpeed = 0.0; // encoder calculated speed
+
+ goGoGo = false;
+
+ loadConfig(interceptDist, declination, compassGain, yawGain); // Load the waypoints, declination, and other config stuff
+ steerCalc.setIntercept(interceptDist); // Setup steering calculator based on intercept distance
+
+ go.set(UPDATE_PERIOD, 20, 650, maxSpeed, Schedule::hold); // Set throttle profile
+ //stop.set(UPDATE_PERIOD, 10, 500, 400, Schedule::hold); // Set brake profile
+ // TODO: Parameterize max brake
+ // obstacle: brake, speed up
+ // turn: brake or slowdown, speed up again
+ // brake speed up
+
+ initAHRS();
+
+ wptCurrent = 0;
+ initDR(); // initalize dead reckoning
+ wptCurrent++; // Point to the next waypoint; first wpt is the starting point
+ done = false;
+ buttonReleased = buttonPressed = false;
+ bool started = false; // flag to indicate robot has exceeded min speed.
+
+ // TODO--make sure all variables are initialized here
+ yawHdg = -1.0;
+ gyroCount = 0;
+ err_bearing = 0.0;
+ err_distance = 0.0;
+ err_compass = 0.0;
+ err_yaw = 0.0;
+
+
+ fp = initLogfile(); // Open the log file in sprintf format string style; numbers go in %d
+ wait(0.2);
+
+ //gps1.attach(recv1, Serial::RxIrq);
+ ahrs.attach(recv1, Serial::RxIrq);
+ gps2.attach(recv2, Serial::RxIrq);
+
+ // Figure out the "offset" or "bias" of the GPS reading
+ // since we know the robot's starting point
+
+ // 20110419 1052
+ findGPSBias(&gps2_off_brg, &gps2_off_dist, 2, wpt[0]);
+
+ // Find average compass heading and use for
+ // initializing base heading used by AHRS gyro+accel
+ // Compass LSM303DLH runs at 15Hz = .06666 sec
+ pc.printf("Computing initial yaw heading...\n");
+ initialHdg = 0.0;
+ for (int i = 0; i < 10; i++) {
+ float hdg = compassHeading();
+ initialHdg += hdg / 10.0;
+ pc.printf("Compass: %.2f Initial Heading: %.2f\n", hdg, initialHdg);
+ wait(0.05);
+ }
+ initialHdg = 91.0; // hard code due to issues
+ pc.printf("Initial Heading: %.2f\n", initialHdg);
+ odo_heading = initialHdg;
+
+ //while (!ahrsParser.dataReady()); // sync to ahrs
+ //ahrsParser.resetReady();
+ timer.reset(); // Keep track of milliseconds, elapsed
+ timer.start();
+
+ float lastYaw = ahrsParser.readYaw(); // try to capture rate of change
+ float initialHdgFilt = initialHdg; // initialize the initial heading filter
+ bool gpsSync = false; // Allows us to sync closer to actual fix time
+ int syncTime = 0; // stores next millisecond count at which to save DR data
+ bool firstGPS = 0; // is this the first GPS packet? If so, don't calc errors
+
+ // Main loop
+ //
+ while(done == false) {
+
+ int millis = timer.read_ms();
+
+ // reset led status; allows blinking, 10ms long
+ if ((millis % UPDATE_PERIOD) == 10) {
+ logStatus = (fp != 0) ? 1 : 0;
+ //ahrsStatus = (gps1Parse.f_hdop() > 0.0 && gps1Parse.f_hdop() < 10.0) ? 1 : 0;
+ gps2Status = (gps2Parse.f_hdop() > 0.0 && gps2Parse.f_hdop() < 10.0) ? 1 : 0;
+ }
+
+ // Warning flasher
+ //if (blink.ticked(millis)) {
+ // flasher = blink.get();
+ //}
+
+ // 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) {
+ if (go.ticked(millis)) {
+ throttle = go.get() / 1000.0;
+ //pc.printf("throttle: %0.3f\n", throttle.read());
+ }
+ if (buttonPressed && started) {
+ pc.printf(">>>>>>>>>>>>>>>>>>>>>>> HALT\n");
+ done = true;
+ goGoGo = false;
+ }
+ } else {
+ if (buttonPressed == true) {
+ pc.printf(">>>>>>>>>>>>>>>>>>>>>>> GO GO GO\n");
+ goGoGo = true;
+ buttonPressed = false;
+ }
+ }
+
+
+ // UPDATE EVERY UPDATE_PERIOD ms (50Hz)
+ // The compass updates only ever 50ms
+ // The gyro updates however fast we can read it
+ //
+
+ if ((millis % UPDATE_PERIOD) == 0) {
+ unsigned int adcResult[8];
+
+ //WHERE();
+
+ adc.setChannel(0);
+ for (int i=0; i < 8; i++) {
+ adcResult[i] = adc.read();
+ }
+
+ temp = adcResult[TEMP_CHAN];
+ // <== read/temp calibrate here
+ // Convert gyro adc reading to heading
+ // TODO:
+ // automatically calculate null at start
+ //gyro = gyroRate(adcResult[GYRO_CHAN]);
+
+ yaw = ahrsParser.readYaw(); // <-- this may have been causing trouble before. 20110420 1249
+ // 20110421
+ yawRate = yaw - lastYaw;
+ lastYaw = yaw;
+
+ // If we're stopped, set initialHdg to compass
+ // But do a running average / leaky integrator to reduce some noise
+ // 20110421
+ if (encSpeed == 0) {
+ findGPSBias(&gps2_off_brg, &gps2_off_dist, 1, wpt[0]);
+ initialHdgFilt += compassHdg - (initialHdgFilt * 0.25);
+ initialHdg = initialHdgFilt * 0.25;
+ while (initialHdg < 0.0) initialHdg += 360.0;
+ while (initialHdg >= 360.0) initialHdg -= 360.0;
+ }
+
+ // Substitute 6dof AHRS yaw for 1-axis gyro and 6dof compass
+ yawHdg = initialHdg + yaw;
+
+ while (yawHdg < 0) yawHdg += 360.0;
+ while (yawHdg >= 360.0) yawHdg -= 360.0;
+
+ compassHdg = compassHeading();
+
+ // pc.printf("Gyro: %d GyroHdg: %.1f\n", gyro, yaw);
+
+ // Need a better filtered heading output
+ // for now just use gyro heading as it seems to be most accurate
+ // provided the bias is set right.
+
+ // Odometry
+ unsigned int leftCount = left.read();
+ unsigned int rightCount = right.read();
+
+ // TODO--> sanity check on encoders; if difference between them
+ // is huge, what do we do? Slipping wheel? Skidding wheel?
+
+ double leftDist = (WHEEL_CIRC / 32) * (double) leftCount;
+ double rightDist = (WHEEL_CIRC / 32) * (double) rightCount;
+ encDistance = (leftDist + rightDist) / 2.0;
+ encSpeed = encDistance / (UPDATE_PERIOD * 0.001);
+
+ odo_heading += (leftDist - rightDist) / TRACK;
+
+ if ((millis % 500) == 0) {
+ pc.printf("Odo heading: %.1f\n", odo_heading);
+ }
+
+ // 20110421
+ if (encSpeed < GPS_MIN_SPEED) {
+ dr_heading = yawHdg;
+ } else {
+ started = true;
+ dr_heading = compassHdg;
+
+ // Yaw error
+ // Let yaw track closely behind compass which
+ // tracks closely behind GPS
+ err_yaw = clamp180(compassHdg - yawHdg);
+
+ // fix yaw by tweaking the initial heading
+ // which is a kludge but... hey it's 2 days before the competition
+ initialHdg += err_yaw * yawGain;
+ clamp360(initialHdg);
+
+ }
+ while (dr_heading < 0) dr_heading += 360.0;
+ while (dr_heading >= 360.0) dr_heading -= 360.0;
+
+ //pc.printf("%d dr_heading: %.2f\n", __LINE__, dr_heading);
+
+ // Dead Reckoning Position estimation, very simple stuff
+ // lat: north +y, south -y ; cos(hdg)
+ // long: west -x, east +x ; sin(hdg)
+ // 1852m = 1 nautical mile = 1 deg lat / long
+ dr_here.move(dr_heading, encDistance);
+
+ ////////////////////////////////
+ // Correct position here
+
+ //
+ //
+ ////////////////////////////////
+
+ dr_bearing = wpt[wptCurrent].bearing(dr_here);
+ dr_distance = wpt[wptCurrent].distance(dr_here);
+
+ //pc.printf("dr_heading: %.1f encDistance: %.3f\n", dr_heading, encDistance);
+
+ // Log dead reckoning info
+ logStatus = 0;
+ fprintf(fp, "%d, %.1f, %.1f, %.1f, %.8f, %.8f, , , , , , %.1f, %.1f, , %.1f, %.3f, %d, %d, %d, %.1f\n",
+ millis, yaw, yawHdg, compassHdg, dr_here.latitude(), dr_here.longitude(), dr_heading, encSpeed,
+ dr_bearing, dr_distance, left.readTotal(), right.readTotal(), temp, odo_heading);
+
+ //pc.printf("Yaw = %.1f\n", ahrsParser.readYaw());
+
+ // synchronize when RMC and GGA sentences received w/ AHRS
+ if (gps2Parse.rmc_ready() && gps2Parse.gga_ready()) {
+ char gps2date[32], gps2time[32];
+
+ // We synchronize on the first GPS packet that doesn't have GSV sentences
+ // then every 1000ms after, we will save DR data for error comparison
+ if (!gps2Parse.gsv_ready() && !gpsSync) {
+ syncTime = millis + 1000;
+ gpsSync = true;
+ }
+
+ doGPS(gps2Parse, here2, gps2Status, gps2date, gps2time);
+
+ if (gps2_hdop < GPS_MAX_HDOP) {
+
+ // Correct here2 for offset (bias)
+ here2.move(gps2_off_brg, gps2_off_dist);
+
+ //pc.printf("GPS2 %d HDOP: %.1f Compass: %.1f Gyro: %d Gyro Temp: %d\n", millis, gps1Parse.f_hdop(), compassHdg, gyro, temp);
+
+ gps2_bearing = wpt[wptCurrent].bearing(here2);
+ gps2_distance = wpt[wptCurrent].distance(here2);
+ gps2_heading = gps2Parse.f_course();
+
+ //pc.printf("GPS2: lat: %.6f lon: %.6f wpt[%d]: lat:%.6f lon:%.6f distance=%.2f\n",
+ // here2.latitude(), here2.longitude(), wptCurrent,
+ // wpt[wptCurrent].latitude(), wpt[wptCurrent].longitude(), distance);
+
+ // TODO --> Estimate new DR position here
+ // simple: take a weighted average midpoint
+ // TODO --> gradually move dr position by using same err distance/bearing but move a fraction of the distance
+ // over the next n samples between gps readings
+ // We have to address GPS lag. The easy (but probably incorrect way) is to use the DR info at the last time
+ // we received a GPS update. Because the GPS is spitting out CSV sentences every 5 seconds or so, that
+ // throws off the timing.
+
+ if (!firstGPS) {
+
+ // 20110421
+ // Position error
+ err_distance = here2.distance(dr_here_last);
+ err_bearing = here2.bearing(dr_here_last);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // Error Correction
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // The strategy is: rely on GPS when hdop < 2; corrected by distance from initial starting point. At
+ // hdop > 2, gps position becomes unreliable, so fall back on compass and/or AHRS yaw and dead reckoning.
+ // The latter is pretty reliable. But heading isn't. Compass err varies based on, apparently, mag field
+ // from the motor/wires. We'll stick to a set speed and correct compass error while relying on gps. That
+ // plus position correction should keep DR up to date with the last gps fix with hdop < GPS_MAX_HDOP
+ // Meanwhile, AHRS yaw seems to be filtered to the point that bandwidth can't track even moderate turn
+ // rates. The exact cause is yet undiscovered. The onboard compass has a distortion I've been unable
+ // to resolve, so we're running only gyro+accelerometer
+ //
+ // At higher speeds, we can trust gps course, but at lower speeds we can't. Initially when we start, the
+ // most reliable heading/DR information comes from encoders+AHRS. At speed = 0, yaw and compass are very
+ // reliable, so yaw initial heading is set from compass. So we'll use dead reckoning until the vehicle
+ // reaches a higher speed, at which point, position info comes from gps and is used for yaw/compass/position
+ // correction.
+ //
+ // I wished I could figure out a mathematically fancy way to do this but I'm confounded by all the different
+ // sensor errors. KF is the only thing I've sort-of learned so far and I don't yet see how I can use it
+ // for the types of (apparently non-gaussian) noise/error I'm seeing.
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // 20110421
+ if (encSpeed >= GPS_MIN_SPEED) {
+ // Heading error
+ err_compass = compassHdg_last - gps2_heading;
+ if (err_compass < -180.0) err_compass += 360.0;
+ if (err_compass > 180.0) err_compass -= 360.0;
+ // we'll use err_compass when computing compass heading from now on
+
+ // fix compass by tweaking declination
+ // again, no time to do anything elegant, just hacking here
+ declination += err_compass * compassGain;
+ clamp360(declination);
+
+ // 20110421
+ fprintf(fp, "ERR: err_dist: %.5f err_brg: %.2f compassHdg_last: %.2f gps2_heading: %.2f err_compass: %.5f declination: %.2f\n",
+ err_distance, err_bearing, compassHdg_last, gps2_heading, err_compass, declination);
+ }
+ } // if (!firstGPS)
+
+ // 20110421
+ // Save currnt DR position / heading
+ // change this so that after gpsSync == true, we only save this
+ // when millis == syncTime, then syncTime += 1000
+ dr_here_last.set( dr_here );
+ compassHdg_last = compassHdg;
+ firstGPS = false;
+
+ // less simple: find distance between & bearing, mult dist by weight, plot new position with move()
+ // harder: kalman filter
+ // ALSO---> need to do some kind of sanity check. If distance between each exceeds threshold, then one is bad
+ // how do we guess which? Will gps ever be way off with low hdop?
+ // maybe compare to predicted location based on last heading/speed?
+ // should we convert to UTM?
+ }
+
+ logStatus = 0;
+ //WHERE();
+ fprintf(fp, "%d, %.1f, %.1f, %.1f, %.8f, %.8f, GPS2, %s, %s, %.2f, %.1f, %.1f, %.1f, %.1f, %.3f, , , , %d, \n",
+ millis, gyro, yawHdg, compassHdg, here2.latitude(), here2.longitude(), gps2date, gps2time,
+ gps2Parse.f_altitude(), // <-- was hanging here... but probably due to goof up somewhere else. Stack corruption??
+ gps2_heading, gps2Parse.f_speed_mph(), gps2Parse.f_hdop(), bearing, distance, temp);
+ //WHERE();
+
+ }
+
+ //////////////////////////////////////////////////////////////////////////////
+ // Update steering and throttle
+ //////////////////////////////////////////////////////////////////////////////
+ if ((millis % 100) == 0) {
+
+ // TODO --> Calculate bearing/distance from GPS and DR
+
+ // Might be able to do kalman filter here but... meanwhile:
+ // 1) Weighted averaging of heading using GPS and Gyro to account for drift
+ // 2) Correct GPS position for bias (brg/dist)
+ // 3) Take corrected GPS position, go back 2 seconds, and find error brg/dist with DR at that time
+ // 4) calculate a weighted average position based on DR and GPS, hdop and SV change
+ // 5) apportion some of the error back into the GPS position bias (brg/dist) weighted on hdop and SV changes
+
+
+ //////////////////////////////////////////////////////////////////////////////
+ // Navigation -- see error correction above
+ // We'll always navigate off DR but only because we're supposed to be
+ // correcting it as we go via GPS
+ //////////////////////////////////////////////////////////////////////////////
+ // 20110421
+ bearing = dr_bearing;
+ distance = dr_distance;
+ heading = dr_heading;
+
+ relativeBrg = bearing - heading; // 0 is desired heading
+
+// limit steering angle based on object detection ?
+// or limit relative brg perhaps?
+
+ // if correction angle is < -180, express as negative degree
+ if (relativeBrg < -180.0) relativeBrg += 360.0;
+ if (relativeBrg > 180.0) relativeBrg -= 360.0;
+
+ float steerAngle = steerCalc.calcSA(relativeBrg);
+
+ // Convert steerAngle to servo value
+ // Testing determined near linear conversion between servo ms setting and steering angle
+ // up to 20*. Assumes a particular servo library with range = 0.005
+ // In that case, f(SA) = servoPosition = 0.500 + SA/762.5
+ // between 20 and 24* the slop is approximately 475
+ // What if we ignore the linearity and just set to a max angle
+ // also range is 0.535-0.460 --> slope = 800
+ //steering = 0.500 + (double) steerAngle / 762.5;
+
+ if (encSpeed < GPS_MIN_SPEED) {
+ steering = 0.495;
+ } else {
+ steering = 0.500 + (double) steerAngle / 808.0;
+ }
+ pc.printf("Wpt #%d: rel bearing: %.1f, bearing: %.1f, distance: %.5f\n", wptCurrent, relativeBrg, bearing, distance);
+
+ // if within 3m move to next waypoint
+ // PARAMETERIZE DISTANCE
+ if (distance < 3.0) {
+ pc.printf("Arrived at wpt %d\n", wptCurrent);
+ wptCurrent++;
+ }
+
+ // Are we at the last waypoint?
+ if (wptCurrent == wptCount) {
+ pc.printf("Arrived at final destination. Done.\n");
+ done = true;
+ }
+
+ }
+
+
+/*
+ if ((millis % 1000) == 0) {
+ pc.printf("Wpt #%d: rel bearing: %.1f, bearing: %.1f, distance: %.5f\n", wptCurrent, relativeBrg, bearing, distance);
+ pc.printf("dr_heading: %.1f encDistance: %.3f\n", dr_heading, encDistance);
+ pc.printf("err_dist: %.5f\nerr_brg: %.1f\n\n", err_distance, err_bearing);
+ }
+ */
+
+ }
+
+ } // while
+
+ // shut 'er down!
+ throttle = 0.500;
+ steering = 0.500;
+
+ if (fp) {
+ fclose(fp);
+ logStatus = 0;
+ pc.printf("closed log file.\n");
+ }
+
+ //ahrsStatus = 0;
+ gps2Status = 0;
+ confStatus = 0;
+ flasher = 0;
+
+} // autonomousMode
+
+
+void idleMode()
+{
+ while (1) {
+ ahrsStatus = 0;
+ wait(0.2);
+ ahrsStatus = 1;
+ wait(0.2);
+ }
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// UTILITIY FUNCTIONS
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+// Takes n position readings and averages the gps position
+// in a pretty rudimentary fashion, returning bearing and distance
+// from the specified place to the average position
+void findGPSBias(double *brg, double *dist, int n, GeoPosition place)
+{
+ // GPS Position averaging
+ double div = (double) n;
+ double latavg = 0;
+ double lonavg = 0;
+ double lat = 0;
+ double lon = 0;
+ gps2Parse.reset_ready();
+
+ pc.printf("Calculating GPS offset...\n");
+
+ while (n > 0) {
+ while (gps2Parse.rmc_ready() == false)
+ wait(0.2);
+
+ gps2Parse.reset_ready();
+ gps2Parse.f_get_position(&lat, &lon, &age);
+
+ if (gps2Parse.f_hdop() > 0 && gps2Parse.f_hdop() < 2.0) {
+ pc.printf("GPS lat: %.7f lon %.7f\n", lat, lon);
+ latavg += lat / div;
+ lonavg += lon / div;
+ n--;
+ } else {
+ pc.printf("Waiting for good GPS fix\n");
+ gps2Status = 1;
+ wait(0.1);
+ gps2Status = 0;
+ }
+
+ }
+ pc.printf("AVG lat: %.7f lon %.7f\n", latavg, lonavg);
+ GeoPosition avg(latavg, lonavg);
+ *dist = place.distance( avg );
+ *brg = place.bearing( avg );
+ // Now that we've calculated a bearing and distance to a quickie offset/bias
+ // we can subtract this from all future GPS readings by simply taking the
+ // gps position and doing a move(gps2_off_brg, gps2_off_dist)
+ // maybe do adjustments based on how far off predicted DR position is from
+ // GPS reported pos. I'd love to be able to factor in SV and HDOP info too
+}
+
+
+void compass3dCalibrate()
+{
+ int tick;
+ FILE *fp;
+
+ pc.printf("Entering calibration mode for 30 seconds\nRotate compass thru 360 degree sphere\n");
+ for (int i=10; i > 0; i--) {
+ pc.printf("%d...", i);
+ if (pc.readable()) {
+ char cmd = pc.getc();
+ }
+ wait(1.0);
+ }
+ pc.printf("Begin calibration\n");
+
+ fp = fopen("/log/compass.txt", "w");
+
+ if (fp == 0) {
+ pc.printf("Error opening file\n");
+ } else {
+ // printf(fp, "# magX, magY, magZ\n");
+ timer.reset();
+ timer.start();
+ tick = 30;
+ while (tick > 0) {
+ // run every 20ms
+ // get all three axes
+ // printf(fp, "%d, %d, %d\n"
+ if (timer.read_ms() > 1000) {
+ pc.printf("%d\n", tick--);
+ timer.reset();
+ }
+ wait(0.2);
+ }
+ fclose(fp);
+ pc.printf("\nData saved to compass.txt\n\n");
+ }
+}
+
+
+// HMC6352 compass calibration mode
+void compassCalibrate()
+{
+ int tick;
+
+ pc.printf("Entering calibration mode for 60 seconds\nRotate compass at least once through 360 degrees\n");
+ for (int i=10; i > 0; i--) {
+ pc.printf("%d...", i);
+ if (pc.readable()) {
+ char cmd = pc.getc();
+ }
+ wait(1.0);
+ }
+ pc.printf("Begin calibration\n");
+
+ //compass.setCalibrationMode(HMC6352_ENTER_CALIB);
+ timer.start();
+ tick = 60;
+ while (tick > 0) {
+ if (timer.read_ms() > 1000) {
+ pc.printf("%d\n", tick--);
+ timer.reset();
+ }
+ wait(0.2);
+ }
+ //compass.setCalibrationMode(HMC6352_EXIT_CALIB);
+ pc.printf("Calibration complete.\n");
+}
+
+void servoCalibrate()
+{
+}
+
+void bridgeRecv()
+{
+ while (dev && dev->readable()) {
+ pc.putc(dev->getc());
+ }
+}
+
+void serialBridge(Serial &gps)
+{
+ char x;
+ int count = 0;
+ bool done=false;
+
+ pc.printf("\nEntering serial bridge in 2 seconds, +++ to escape\n\n");
+ wait(2.0);
+ //dev = &gps;
+ gps.attach(NULL, Serial::RxIrq);
+ while (!done) {
+ if (pc.readable()) {
+ x = pc.getc();
+ // escape sequence
+ if (x == '+') {
+ if (++count >= 3) done=true;
+ } else {
+ count = 0;
+ }
+ gps.putc(x);
+ }
+ if (gps.readable()) {
+ pc.putc(gps.getc());
+ }
+ }
+}
+
+
+void instrumentCheck()
+{
+ bool done = false;
+ float compassHdg = 0;
+ float initialHdg = 0;
+ float ahrsHdg = 0;
+ unsigned int adcResult[8] = {0,0,0,0,0,0,0,0};
+ //unsigned int sonar, gyro, temp;
+ Timer timer;
+ SimpleFilter ranger(8);
+
+ initialHdg = compassHeading();
+ initAHRS();
+
+ timer.reset();
+ timer.start();
+
+ buttonReleased = buttonPressed = false;
+
+ pc.printf("press e to exit\n");
+ while (!done) {
+ if (buttonPressed) done=true;
+ while (pc.readable()) {
+ if (pc.getc() == 'e') {
+ done = true;
+ break;
+ }
+ }
+
+ int millis = timer.read_ms();
+
+ if ((millis % 20) == 0) {
+
+ if ((millis % 100) == 0) {
+ sonarStart = 1;
+ wait_us(25);
+ sonarStart = 0;
+ wait_us(50);
+ }
+
+ adc.setChannel(0);
+ for (int i = 0; i < 8; i++) {
+ adcResult[i] = adc.read();
+ }
+ ranger.filter(adcResult[0]);
+
+ if ((millis % 1000) == 0) {
+ compassHdg = compassHeading();
+
+ ahrsHdg = initialHdg + ahrsParser.readYaw();
+ while (ahrsHdg >= 360.0) ahrsHdg -= 360.0;
+ while (ahrsHdg < 0) ahrsHdg += 360.0;
+
+ char data[4];
+
+ // CMUcam1 I2C code goes here
+
+ pc.printf("-----e to exit-----\nCompass: %.1f\nAHRS: %.1f\nGyro: %.3f\nGyro Temp: %d\nSonar Left: %.2f\nSonar Right: %.2f\nIR Left: %.5f\nIR Right: %.5f\nEncLeft: %d\nEncRight: %d\n\n",
+ compassHdg, ahrsHdg, gyroRate(adcResult[GYRO_CHAN]), adcResult[TEMP_CHAN],
+ sonarDistance(adcResult[SONARLEFT_CHAN]), sonarDistance(adcResult[SONARRIGHT_CHAN]),
+ irDistance(adcResult[IRLEFT_CHAN]), irDistance(adcResult[IRRIGHT_CHAN]), left.readTotal(), right.readTotal());
+
+ pc.printf("IR filter: %d %.1f\n", ranger.value(), irDistance(ranger.value()));
+
+ pc.printf("\nCam Obj Box: (%d,%d) (%d,%d)\n\n", data[0], data[1], data[2], data[3]);
+
+ for (int i = 0; i < 8; i++) {
+ pc.printf("ADC(%d)=%d\n", i, adcResult[i]);
+ }
+ pc.printf("\n");
+ }
+ wait_ms(2);
+ }
+ }
+ // clear input buffer
+ while (pc.readable()) pc.getc();
+}
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+// ADC CONVERSION FUNCTIONS
+///////////////////////////////////////////////////////////////////////////////////////////////////////
+float compassHeading() {
+ //Timer t;
+ //int usec1, usec2;
+ //t.reset();
+ //t.start();
+ //float compassHdg = compass.sample() / 10.0; // read compass
+ //float compassHdg = compass3d.heading2d();
+ ////WHERE();
+ //__disable_irq(); // Disable Interrupts
+ //usec1 = t.read_us();
+ float compassHdg = compass3d.heading();
+ //usec2 = t.read_us();
+ //__enable_irq(); // Enable Interrupts
+
+ //pc.printf("usec1: %d, usec2: %d, usec: %d\n", usec1, usec2, usec2-usec1);
+
+ //WHERE();
+ compassHdg -= declination; // Correct for local declination
+ clamp360(compassHdg);
+
+ return compassHdg;
+}
+
+
+// returns rate in */sec
+//
+// Gyro is ratiometric and so is ADC, so we can cancel out
+// errors due to voltage supply by putting sensitivity in
+// terms of adc reading and calculate rate from that.
+// (adc - bias) / sens
+// UPDATE_PERIOD in ms, 5.91mV/degree/sec is scale factor
+//
+// TODO: use constants and defines
+//
+float gyroRate(unsigned int adc)
+{
+ float rate = 0.0;
+
+ rate = ((float) (adc - gyroBias)) / gyroSens;
+
+ return rate;
+}
+
+// returns distance in m for LV-EZ1 sonar
+//
+float sonarDistance(unsigned int adc)
+{
+ float distance = 9999.9;
+
+ // EZ1 uses 9.8mV/inch @ 5V or scaling factor of Vcc / 512
+ // so we can eliminate Vcc changes by simply converting the 0-512 inch range
+ // to the ADC's 0-4096 range
+ distance = ((float) adc) * (512 * 0.0254) / 4096; // distance converted to inch then meter
+
+ return distance;
+}
+
+// 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(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);
+}
\ No newline at end of file