v1.0 merge

Basically merges everything except Path, RoboPositioner, and MadgwickAHRS, as Path
and RoboPositioner are extremely experimental and MadgwickAHRS is used only by
them.

.gitignore

@@ -1,6 +1,7 @@
-src/Robot.cpp
-docs/
-Doxyfile
+ControlConfig.xml
+src/Robot.cpp
+docs/
+Doxyfile
 /Debug/
 /.cproject
 /.project

README.md

@@ -1,5 +1,19 @@
-ADBLib - (Somewhat) Advanced Dreadbot Library
+#ADBLib
+####(Somewhat) Advanced Dreadbot Library
 
-A libray for simple FRC robot code with the aim of simplifying/speeding up development.
+The simple and stupid library for making programming FRC robots easier.
+
+##Features
+
+* Easy-to-use, fast finite state machines
+* Prototype inertial guidance system + pathing system
+* New controller type allows for dynamic loading of controls, allowing drivers to have their own control scheme. Also eliminates the need to have hardware IDs in the code.
+* Support for a wide variety of drivebases, with the option to easily write your own drivebase, compatible with the rest of the library.
+* Global logging support, compatible with RoboRIOs.
+* Support for an arbitrary number USB cameras, with easy camera switching/image grabbing
+* Support for the MPU6005, a combination gyroscope/accelerometer/magnetomer.
+* Simplification/abstraction of pneumatics and PID motors
+
+##Documentation
+Documentation is available in browsable form [here](http://dreadbot.github.io/ADBLib/).
 
-Most of the main work is done on the develop branch. Go check that one out (ha), it's much more fun!

lib/I2Cdev.cpp

@@ -0,0 +1,185 @@
+#include "I2Cdev.h"
+
+using namespace ADBLib;
+
+// Turns out that the I2C interface we've been using is fundamentally flawed.
+// wooohoooo
+// In essence, the firmware is making attempts to access some buffers on the device by
+// opening entirely new device channels. As much as it's bad practice, it's necessary for
+// basic functionality.  The problem is that while writing the interface for I2C, I disregarded
+// all passed device channel parameters in favor of speed (i.e. reading from a static costant
+// inside the class).
+// Also, you're required to create a new I2C object specifically for the magnetometer.
+
+I2C* I2Cdev::device;
+
+// Constructor
+I2Cdev::I2Cdev() {}
+
+// Initializes static data members
+void I2Cdev::initialize()
+{
+    I2Cdev::device = new I2C(I2C::kOnboard, I2Cdev::devAddr);
+    SmartDashboard::PutBoolean("MPU-9150 detected", I2Cdev::device->AddressOnly());
+}
+
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data)
+{
+    uint8_t b;
+    uint8_t count = readByte(devAddr, regAddr, &b);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data)
+{
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    /xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = readByte(devAddr, regAddr, &b)) != 0)
+    {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data)
+{
+    return readBytes(devAddr, regAddr, 1, data);
+}
+
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @return Number of bytes read (-1 indicates failure)
+ */
+int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
+{
+    //int8_t count = 0;
+    bool success = device->Read(regAddr, length, data);
+    if (success)
+    {
+        return length;
+    }
+    else
+    {
+        return -1;
+    }
+}
+
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data)
+{
+    uint8_t b;
+    readByte(devAddr, regAddr, &b);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return writeByte(devAddr, regAddr, b);
+}
+
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
+{
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    if (readByte(devAddr, regAddr, &b) != 0)
+    {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+        return writeByte(devAddr, regAddr, b);
+    }
+    else
+    {
+        return false;
+    }
+}
+
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
+{
+    return writeBytes(devAddr, regAddr, 1, &data);
+}
+
+
+/** Write multiple bytes to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of bytes to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data)
+{
+    bool status = true;
+    // bool Write(uint8_t registerAddress, uint8_t data);
+    for (uint8_t i = 0; i < length; ++i)
+    {
+        status = status && device->Write(regAddr, data[i]);
+    }
+    return status;
+}

lib/I2Cdev.h

@@ -0,0 +1,54 @@
+// I2Cdev.h
+
+// Adapted from Jeff Rowberg's excellent I2C library <https://github.com/sparkfun/MPU-9150_Breakout/tree/master/firmware>
+// This falls under the MIT license in his name.
+
+// Functionally, this is only a conversion layer and further abstraction of WPILib's I2C drivers.
+// It comes in this package as an
+
+#pragma once
+
+#include "WPILib.h"
+#include <string>
+
+#define MPU9150_I2C_ADR_LOW 0x68  // invensense eval board
+#define MPU9150_I2C_ADR_HIGH 0x69
+
+#define MPU9150_I2C_ADR MPU9150_I2C_ADR_LOW // If all else fails, change this
+
+namespace ADBLib
+{
+	template <typename T>
+	void assert(T toassert, std::string msg)
+	{
+		if (toassert)
+			SmartDashboard::PutBoolean("Assert::" + msg, true);
+		else
+			SmartDashboard::PutBoolean("Assert::" + msg, false);
+	}
+
+	class I2Cdev
+	{
+	public:
+		I2Cdev();
+		static void initialize();
+
+		static int8_t readBit(uint8_t _devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data);
+		static int8_t readBits(uint8_t _devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data);
+		static int8_t readByte(uint8_t _devAddr, uint8_t regAddr, uint8_t *data);
+		// readBytes implements all core functionality
+		static int8_t readBytes(uint8_t _devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+
+		static bool writeBit(uint8_t _devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+		static bool writeBits(uint8_t _devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+		static bool writeByte(uint8_t _devAddr, uint8_t regAddr, uint8_t data);
+		// writeBytes implements all core write functionality
+		static bool writeBytes(uint8_t _devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+
+		const static uint8_t devAddr = MPU9150_I2C_ADR;
+		static I2C* device;
+
+	private:
+		const static uint16_t readTimeout = 1000;
+	};
+}

lib/Library Notes.txt → lib/README.md

@@ -1,28 +1,28 @@
-This folder contains third party libraries from an external source. Their
-copyright/miscellaneous notices are displayed below.
-_________________________________________________________________________
-*--------------------------------PUGIXML--------------------------------*
-This software is based on pugixml library (http://pugixml.org).
-pugixml is Copyright (C) 2006-2014 Arseny Kapoulkine. 
-_________________________________________________________________________
-*---------------------------------HYDRA---------------------------------*
-Copyright (c) 2015 Christopher Myers
-
-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.
-_________________________________________________________________________
+#Library Notes
+This folder contains third party libraries from an external source. Their
+copyright/miscellaneous notices are displayed below.
+
+##PugiXML
+This software is based on pugixml library (http://pugixml.org).
+pugixml is Copyright (C) 2006-2014 Arseny Kapoulkine.
+
+##Hydra Engine
+Copyright (c) 2015 Sourec
+
+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.