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
Revision 0:9b27ebe1ce17, committed 2011-04-27
- Comitter:
- shimniok
- Date:
- Wed Apr 27 19:23:43 2011 +0000
- Commit message:
- Initial release
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ADC128S.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shimniok/code/ADC128S/#e304ec6ed416
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/DebounceIn.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AjK/code/DebounceIn/#91a2e988ba9d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FATFileSystem.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_unsupported/code/fatfilesystem/ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GeoPosition.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shimniok/code/GeoPosition/#b4feb3f54fa3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IncrementalEncoder.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shimniok/code/IncrementalEncoder/#dea4a931b267
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM303DLH.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shimniok/code/LSM303DLH/#aea5caec809c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PinDetect.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AjK/code/PinDetect/#cb3afc45028b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.cpp Wed Apr 27 19:23:43 2011 +0000
@@ -0,0 +1,457 @@
+/* mbed SDFileSystem Library, for providing file access to SD cards
+ * Copyright (c) 2008-2010, sford
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+/* Introduction
+ * ------------
+ * SD and MMC cards support a number of interfaces, but common to them all
+ * is one based on SPI. This is the one I'm implmenting because it means
+ * it is much more portable even though not so performant, and we already
+ * have the mbed SPI Interface!
+ *
+ * The main reference I'm using is Chapter 7, "SPI Mode" of:
+ * http://www.sdcard.org/developers/tech/sdcard/pls/Simplified_Physical_Layer_Spec.pdf
+ *
+ * SPI Startup
+ * -----------
+ * The SD card powers up in SD mode. The SPI interface mode is selected by
+ * asserting CS low and sending the reset command (CMD0). The card will
+ * respond with a (R1) response.
+ *
+ * CMD8 is optionally sent to determine the voltage range supported, and
+ * indirectly determine whether it is a version 1.x SD/non-SD card or
+ * version 2.x. I'll just ignore this for now.
+ *
+ * ACMD41 is repeatedly issued to initialise the card, until "in idle"
+ * (bit 0) of the R1 response goes to '0', indicating it is initialised.
+ *
+ * You should also indicate whether the host supports High Capicity cards,
+ * and check whether the card is high capacity - i'll also ignore this
+ *
+ * SPI Protocol
+ * ------------
+ * The SD SPI protocol is based on transactions made up of 8-bit words, with
+ * the host starting every bus transaction by asserting the CS signal low. The
+ * card always responds to commands, data blocks and errors.
+ *
+ * The protocol supports a CRC, but by default it is off (except for the
+ * first reset CMD0, where the CRC can just be pre-calculated, and CMD8)
+ * I'll leave the CRC off I think!
+ *
+ * Standard capacity cards have variable data block sizes, whereas High
+ * Capacity cards fix the size of data block to 512 bytes. I'll therefore
+ * just always use the Standard Capacity cards with a block size of 512 bytes.
+ * This is set with CMD16.
+ *
+ * You can read and write single blocks (CMD17, CMD25) or multiple blocks
+ * (CMD18, CMD25). For simplicity, I'll just use single block accesses. When
+ * the card gets a read command, it responds with a response token, and then
+ * a data token or an error.
+ *
+ * SPI Command Format
+ * ------------------
+ * Commands are 6-bytes long, containing the command, 32-bit argument, and CRC.
+ *
+ * +---------------+------------+------------+-----------+----------+--------------+
+ * | 01 | cmd[5:0] | arg[31:24] | arg[23:16] | arg[15:8] | arg[7:0] | crc[6:0] | 1 |
+ * +---------------+------------+------------+-----------+----------+--------------+
+ *
+ * As I'm not using CRC, I can fix that byte to what is needed for CMD0 (0x95)
+ *
+ * All Application Specific commands shall be preceded with APP_CMD (CMD55).
+ *
+ * SPI Response Format
+ * -------------------
+ * The main response format (R1) is a status byte (normally zero). Key flags:
+ * idle - 1 if the card is in an idle state/initialising
+ * cmd - 1 if an illegal command code was detected
+ *
+ * +-------------------------------------------------+
+ * R1 | 0 | arg | addr | seq | crc | cmd | erase | idle |
+ * +-------------------------------------------------+
+ *
+ * R1b is the same, except it is followed by a busy signal (zeros) until
+ * the first non-zero byte when it is ready again.
+ *
+ * Data Response Token
+ * -------------------
+ * Every data block written to the card is acknowledged by a byte
+ * response token
+ *
+ * +----------------------+
+ * | xxx | 0 | status | 1 |
+ * +----------------------+
+ * 010 - OK!
+ * 101 - CRC Error
+ * 110 - Write Error
+ *
+ * Single Block Read and Write
+ * ---------------------------
+ *
+ * Block transfers have a byte header, followed by the data, followed
+ * by a 16-bit CRC. In our case, the data will always be 512 bytes.
+ *
+ * +------+---------+---------+- - - -+---------+-----------+----------+
+ * | 0xFE | data[0] | data[1] | | data[n] | crc[15:8] | crc[7:0] |
+ * +------+---------+---------+- - - -+---------+-----------+----------+
+ */
+
+#include "SDFileSystem.h"
+
+#define SD_COMMAND_TIMEOUT 5000
+
+SDFileSystem::SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name) :
+ FATFileSystem(name), _spi(mosi, miso, sclk), _cs(cs) {
+ _cs = 1;
+}
+
+#define R1_IDLE_STATE (1 << 0)
+#define R1_ERASE_RESET (1 << 1)
+#define R1_ILLEGAL_COMMAND (1 << 2)
+#define R1_COM_CRC_ERROR (1 << 3)
+#define R1_ERASE_SEQUENCE_ERROR (1 << 4)
+#define R1_ADDRESS_ERROR (1 << 5)
+#define R1_PARAMETER_ERROR (1 << 6)
+
+// Types
+// - v1.x Standard Capacity
+// - v2.x Standard Capacity
+// - v2.x High Capacity
+// - Not recognised as an SD Card
+
+#define SDCARD_FAIL 0
+#define SDCARD_V1 1
+#define SDCARD_V2 2
+#define SDCARD_V2HC 3
+
+int SDFileSystem::initialise_card() {
+ // Set to 100kHz for initialisation, and clock card with cs = 1
+ _spi.frequency(100000);
+ _cs = 1;
+ for(int i=0; i<16; i++) {
+ _spi.write(0xFF);
+ }
+
+ // send CMD0, should return with all zeros except IDLE STATE set (bit 0)
+ if(_cmd(0, 0) != R1_IDLE_STATE) {
+ //fprintf(stderr, "No disk, or could not put SD card in to SPI idle state\n");
+ return SDCARD_FAIL;
+ }
+
+ // send CMD8 to determine whther it is ver 2.x
+ int r = _cmd8();
+ if(r == R1_IDLE_STATE) {
+ return initialise_card_v2();
+ } else if(r == (R1_IDLE_STATE | R1_ILLEGAL_COMMAND)) {
+ return initialise_card_v1();
+ } else {
+ //fprintf(stderr, "Not in idle state after sending CMD8 (not an SD card?)\n");
+ return SDCARD_FAIL;
+ }
+}
+
+int SDFileSystem::initialise_card_v1() {
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ _cmd(55, 0);
+ if(_cmd(41, 0) == 0) {
+ return SDCARD_V1;
+ }
+ }
+
+ fprintf(stderr, "Timeout waiting for v1.x card\n");
+ return SDCARD_FAIL;
+}
+
+int SDFileSystem::initialise_card_v2() {
+
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ _cmd(55, 0);
+ if(_cmd(41, 0) == 0) {
+ _cmd58();
+ return SDCARD_V2;
+ }
+ }
+
+ //fprintf(stderr, "Timeout waiting for v2.x card\n");
+ return SDCARD_FAIL;
+}
+
+int SDFileSystem::disk_initialize() {
+
+ int i = initialise_card();
+// printf("init card = %d\n", i);
+// printf("OK\n");
+
+ _sectors = _sd_sectors();
+
+ // Set block length to 512 (CMD16)
+ if(_cmd(16, 512) != 0) {
+ //fprintf(stderr, "Set 512-byte block timed out\n");
+ return 1;
+ }
+
+ _spi.frequency(1000000); // Set to 1MHz for data transfer
+ return 0;
+}
+
+int SDFileSystem::disk_write(const char *buffer, int block_number) {
+ // set write address for single block (CMD24)
+ if(_cmd(24, block_number * 512) != 0) {
+ return 1;
+ }
+
+ // send the data block
+ _write(buffer, 512);
+ return 0;
+}
+
+int SDFileSystem::disk_read(char *buffer, int block_number) {
+ // set read address for single block (CMD17)
+ if(_cmd(17, block_number * 512) != 0) {
+ return 1;
+ }
+
+ // receive the data
+ _read(buffer, 512);
+ return 0;
+}
+
+int SDFileSystem::disk_status() { return 0; }
+int SDFileSystem::disk_sync() { return 0; }
+int SDFileSystem::disk_sectors() { return _sectors; }
+
+// PRIVATE FUNCTIONS
+
+int SDFileSystem::_cmd(int cmd, int arg) {
+ _cs = 0;
+
+ // send a command
+ _spi.write(0x40 | cmd);
+ _spi.write(arg >> 24);
+ _spi.write(arg >> 16);
+ _spi.write(arg >> 8);
+ _spi.write(arg >> 0);
+ _spi.write(0x95);
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ int response = _spi.write(0xFF);
+ if(!(response & 0x80)) {
+ _cs = 1;
+ _spi.write(0xFF);
+ return response;
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+int SDFileSystem::_cmdx(int cmd, int arg) {
+ _cs = 0;
+
+ // send a command
+ _spi.write(0x40 | cmd);
+ _spi.write(arg >> 24);
+ _spi.write(arg >> 16);
+ _spi.write(arg >> 8);
+ _spi.write(arg >> 0);
+ _spi.write(0x95);
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ int response = _spi.write(0xFF);
+ if(!(response & 0x80)) {
+ return response;
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+
+
+int SDFileSystem::_cmd58() {
+ _cs = 0;
+ int arg = 0;
+
+ // send a command
+ _spi.write(0x40 | 58);
+ _spi.write(arg >> 24);
+ _spi.write(arg >> 16);
+ _spi.write(arg >> 8);
+ _spi.write(arg >> 0);
+ _spi.write(0x95);
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ int response = _spi.write(0xFF);
+ if(!(response & 0x80)) {
+ int ocr = _spi.write(0xFF) << 24;
+ ocr |= _spi.write(0xFF) << 16;
+ ocr |= _spi.write(0xFF) << 8;
+ ocr |= _spi.write(0xFF) << 0;
+// printf("OCR = 0x%08X\n", ocr);
+ _cs = 1;
+ _spi.write(0xFF);
+ return response;
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+
+int SDFileSystem::_cmd8() {
+ _cs = 0;
+
+ // send a command
+ _spi.write(0x40 | 8); // CMD8
+ _spi.write(0x00); // reserved
+ _spi.write(0x00); // reserved
+ _spi.write(0x01); // 3.3v
+ _spi.write(0xAA); // check pattern
+ _spi.write(0x87); // crc
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT * 1000; i++) {
+ char response[5];
+ response[0] = _spi.write(0xFF);
+ if(!(response[0] & 0x80)) {
+ for(int j=1; j<5; j++) {
+ response[i] = _spi.write(0xFF);
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return response[0];
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+
+int SDFileSystem::_read(char *buffer, int length) {
+ _cs = 0;
+
+ // read until start byte (0xFF)
+ while(_spi.write(0xFF) != 0xFE);
+
+ // read data
+ for(int i=0; i<length; i++) {
+ buffer[i] = _spi.write(0xFF);
+ }
+ _spi.write(0xFF); // checksum
+ _spi.write(0xFF);
+
+ _cs = 1;
+ _spi.write(0xFF);
+ return 0;
+}
+
+int SDFileSystem::_write(const char *buffer, int length) {
+ _cs = 0;
+
+ // indicate start of block
+ _spi.write(0xFE);
+
+ // write the data
+ for(int i=0; i<length; i++) {
+ _spi.write(buffer[i]);
+ }
+
+ // write the checksum
+ _spi.write(0xFF);
+ _spi.write(0xFF);
+
+ // check the repsonse token
+ if((_spi.write(0xFF) & 0x1F) != 0x05) {
+ _cs = 1;
+ _spi.write(0xFF);
+ return 1;
+ }
+
+ // wait for write to finish
+ while(_spi.write(0xFF) == 0);
+
+ _cs = 1;
+ _spi.write(0xFF);
+ return 0;
+}
+
+static int ext_bits(char *data, int msb, int lsb) {
+ int bits = 0;
+ int size = 1 + msb - lsb;
+ for(int i=0; i<size; i++) {
+ int position = lsb + i;
+ int byte = 15 - (position >> 3);
+ int bit = position & 0x7;
+ int value = (data[byte] >> bit) & 1;
+ bits |= value << i;
+ }
+ return bits;
+}
+
+int SDFileSystem::_sd_sectors() {
+
+ // CMD9, Response R2 (R1 byte + 16-byte block read)
+ if(_cmdx(9, 0) != 0) {
+ //fprintf(stderr, "Didn't get a response from the disk\n");
+ return 0;
+ }
+
+ char csd[16];
+ if(_read(csd, 16) != 0) {
+ //fprintf(stderr, "Couldn't read csd response from disk\n");
+ return 0;
+ }
+
+ // csd_structure : csd[127:126]
+ // c_size : csd[73:62]
+ // c_size_mult : csd[49:47]
+ // read_bl_len : csd[83:80] - the *maximum* read block length
+
+ int csd_structure = ext_bits(csd, 127, 126);
+ int c_size = ext_bits(csd, 73, 62);
+ int c_size_mult = ext_bits(csd, 49, 47);
+ int read_bl_len = ext_bits(csd, 83, 80);
+
+// printf("CSD_STRUCT = %d\n", csd_structure);
+
+ if(csd_structure != 0) {
+ //fprintf(stderr, "This disk tastes funny! I only know about type 0 CSD structures\n");
+ return 0;
+ }
+
+ // memory capacity = BLOCKNR * BLOCK_LEN
+ // where
+ // BLOCKNR = (C_SIZE+1) * MULT
+ // MULT = 2^(C_SIZE_MULT+2) (C_SIZE_MULT < 8)
+ // BLOCK_LEN = 2^READ_BL_LEN, (READ_BL_LEN < 12)
+
+ int block_len = 1 << read_bl_len;
+ int mult = 1 << (c_size_mult + 2);
+ int blocknr = (c_size + 1) * mult;
+ int capacity = blocknr * block_len;
+
+ int blocks = capacity / 512;
+
+ return blocks;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.h Wed Apr 27 19:23:43 2011 +0000
@@ -0,0 +1,81 @@
+/* mbed SDFileSystem Library, for providing file access to SD cards
+ * Copyright (c) 2008-2010, sford
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef MBED_SDFILESYSTEM_H
+#define MBED_SDFILESYSTEM_H
+
+#include "mbed.h"
+#include "FATFileSystem.h"
+
+/** Access the filesystem on an SD Card using SPI
+ *
+ * @code
+ * #include "mbed.h"
+ * #include "SDFileSystem.h"
+ *
+ * SDFileSystem sd(p5, p6, p7, p12, "sd"); // mosi, miso, sclk, cs
+ *
+ * int main() {
+ * FILE *fp = fopen("/sd/myfile.txt", "w");
+ * fprintf(fp, "Hello World!\n");
+ * fclose(fp);
+ * }
+ */
+class SDFileSystem : public FATFileSystem {
+public:
+
+ /** Create the File System for accessing an SD Card using SPI
+ *
+ * @param mosi SPI mosi pin connected to SD Card
+ * @param miso SPI miso pin conencted to SD Card
+ * @param sclk SPI sclk pin connected to SD Card
+ * @param cs DigitalOut pin used as SD Card chip select
+ * @param name The name used to access the virtual filesystem
+ */
+ SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name);
+ virtual int disk_initialize();
+ virtual int disk_write(const char *buffer, int block_number);
+ virtual int disk_read(char *buffer, int block_number);
+ virtual int disk_status();
+ virtual int disk_sync();
+ virtual int disk_sectors();
+
+protected:
+
+ int _cmd(int cmd, int arg);
+ int _cmdx(int cmd, int arg);
+ int _cmd8();
+ int _cmd58();
+ int initialise_card();
+ int initialise_card_v1();
+ int initialise_card_v2();
+
+ int _read(char *buffer, int length);
+ int _write(const char *buffer, int length);
+ int _sd_sectors();
+ int _sectors;
+
+ SPI _spi;
+ DigitalOut _cs;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Schedule.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shimniok/code/Schedule/#03d36a9a088b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SimpleFilter.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shimniok/code/SimpleFilter/#70348515ed2f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Steering.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shimniok/code/Steering/#a3b128cdb78b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TinyCHR6dm.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shimniok/code/TinyCHR6dm/#983f66650cd5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TinyGPS.lib Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shimniok/code/TinyGPS/#f522b8bdf987
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Apr 27 19:23:43 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9a9732ce53a1