Initial commit

.gitignore

@@ -0,0 +1,2 @@
+.vscode
+.pio

include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

lib/Chassis/src/chassis-params.h

@@ -0,0 +1,7 @@
+/**
+ * TODO: Adjust these to get good FK updates. They're not horrible, but neither are they good.
+ */
+
+const float ROBOT_RADIUS = 7;
+const float LEFT_TICKS_PER_CM = 50;
+const float RIGHT_TICKS_PER_CM = 50;

lib/Chassis/src/chassis.cpp

@@ -0,0 +1,183 @@
+#include "chassis.h"
+#include "Romi32U4MotorTemplate.h"
+
+Romi32U4EncodedMotor<LEFT_XOR, LEFT_B, PWM_L, DIR_L, OCR_L> leftMotor("L");
+Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor("R");
+
+/**
+ * Because it's declared static, we initialize Chassis::loopFlag here.
+ */
+uint8_t Chassis::loopFlag = 0;
+
+/**
+ * For taking snapshots and raising the flag.
+ */
+void Chassis::Timer4OverflowISRHandler(void) 
+{
+    loopFlag++;
+
+    leftMotor.speed = leftMotor.CalcEncoderDelta();
+    rightMotor.speed = rightMotor.CalcEncoderDelta();
+}
+
+/**
+ * ISR for timing. On Timer4 overflow, we take a 'snapshot' of the encoder counts 
+ * and raise a flag to let the program it is time to execute the PID calculations.
+ */
+ISR(TIMER4_OVF_vect)
+{
+   Chassis::Timer4OverflowISRHandler();
+}
+
+/**
+ * Sets up a hardware timer on Timer4 to manage motor control on a precise schedule.
+ * 
+ * We set the timer to set an interrupt flag on overflow, which is handled
+ * by ISR(TIMER4_OVF_vect) below.
+ */
+void Chassis::InitializeMotorControlTimer(void)
+{
+    Serial.println("InitTimer");
+    // Disable interupts while we mess with the Timer4 registers
+    cli(); 
+  
+    // Set up Timer4
+    TCCR4A = 0x00; // Disable output to pins
+    TCCR4B = 0x0A; // Sets the prescaler -- see pp. 167-8 in datasheet
+    TCCR4C = 0x00; // Disables output to pins (but see below for buzzer)
+    TCCR4D = 0x00; // Normal mode: count up and roll-over
+
+    /**
+     * Calculate TOP based on prescaler and loop duration. Note that loop is in integer ms --
+     * there may be some rounding. Multiples of 4 ms will be exact.
+     */
+    uint16_t top = ((CONTROL_LOOP_PERIOD_MS * 16000ul) >> 9) - 1; // divides by 512
+
+    /**
+     * Here we do a little trick to allow full 10-bit register access. 
+     * We have 2 _bits_ in TC4H that we can use to add capacity to TOP.
+     * 
+     * Note that the maximum period is limited by TOP = 0x3FF. If you want
+     * a longer period, you'll need to adjust the pre-scaler.
+     * 
+     * There is no minumum period, but precision breaks down with low values, 
+     * unless you adjust the pre-scaler, but the encoder resolution is limited,
+     * so you only want to go so fast.
+    */
+    uint8_t highbits = top / 256;
+    uint8_t lowbits = top - highbits;
+    TC4H = highbits; OCR4C = lowbits;
+
+    // Enable overflow interrupt
+    TIMSK4 = 0x04; 
+
+    /**
+     * Uncommenting the following lines will pipe the timer signal to pin 6, 
+     * which controls the buzzer. The pin will toggle at the loop rate, which
+     * allows you to check that the timing is correct. It will also make a lot
+     * of noise, so do so sparingly.
+     */
+    // TCCR4C = 0x04
+    // pinMode(6, OUTPUT);
+
+    // Re-enable interrupts
+    sei(); 
+
+    Serial.println("/InitTimer");
+}
+
+void Chassis::InititalizeChassis(void)
+{
+    InitializeMotorControlTimer();
+    InitializeMotors();
+}
+
+/**
+ * The main Chassis loop.
+ */
+bool Chassis::ChassisLoop(Twist& velocity)
+{
+    bool retVal = false;
+
+    if(loopFlag)
+    {
+        if(loopFlag > 1) Serial.println("Missed an update in Robot::RobotLoop()!");
+
+#ifdef __LOOP_DEBUG__
+        Serial.print(millis());
+        Serial.print('\n');
+#endif
+
+        // motor updates
+        UpdateMotors();
+
+        /* Update the wheel velocity so it gets back to Robot. */
+        velocity = CalcOdomFromWheelMotion();
+
+        loopFlag = 0;
+
+        retVal = true;
+    }
+
+    return retVal;
+}
+
+/**
+ * Some motor methods.
+ */
+void Chassis::InitializeMotors(void)
+{
+    Romi32U4MotorBase::InitializePWMTimerAndInterrupts();
+
+    leftMotor.InitializeMotor();
+    rightMotor.InitializeMotor();
+}
+
+void Chassis::SetMotorEfforts(int16_t left, int16_t right) 
+{
+    leftMotor.SetMotorEffortDirect(left); 
+    rightMotor.SetMotorEffortDirect(right);
+}
+
+void Chassis::UpdateMotors(void)
+{
+    leftMotor.ControlMotorSpeed();
+    rightMotor.ControlMotorSpeed();
+}
+
+/**
+ * SetWheelSpeeds converts the linear wheel speeds (axes relative to ground) to motor speeds.
+ */
+void Chassis::SetWheelSpeeds(float leftSpeedCMperSec, float rightSpeedCMperSec)
+{
+    /** 
+     * TODO: Check the code below. You did this in Lab 1, so we give you the calcs.
+     */
+    leftMotor.SetTargetSpeed(leftSpeedCMperSec * LEFT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_MS / 1000.);
+    rightMotor.SetTargetSpeed(rightSpeedCMperSec * RIGHT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_MS / 1000.);
+}
+
+void Chassis::SetTwist(const Twist& twist)
+{
+    /**
+     * TODO: Complete SetTwist() to call SetWheelSpeeds() from target u and omega
+     */
+}
+
+Twist Chassis::CalcOdomFromWheelMotion(void)
+{
+    Twist velocity;
+    /**
+     * TODO: Calculate velocities from wheel motion, which are held in leftMotor.spped and rightMotor.speed.
+     * Note that you might want to calculate the deltas instead of speeds (to save some floating point maths). 
+     * 
+     * In that case, you should return a Pose instead of a Twist.
+     */
+
+#ifdef __NAV_DEBUG__
+    TeleplotPrint("u", velocity.u);
+    TeleplotPrint("omega", velocity.omega);
+#endif
+
+    return velocity;
+}

lib/Chassis/src/chassis.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <Arduino.h>
+
+#include "chassis-params.h"
+#include "utils.h"
+
+class Chassis
+{
+protected:
+
+    /**
+     * You can change the control loop period, but you should use multiples of 4 ms to 
+     * avoid rounding errors.
+     */
+    const uint16_t CONTROL_LOOP_PERIOD_MS = 20;
+
+    /**
+     * loopFlag is used to tell the program when to update. It is set when Timer4
+     * overflows (see InitializeMotorControlTimer). Some of the calculations are too
+     * lengthy for an ISR, so we set a flag and use that to key the calculations.
+     * 
+     * Note that we use in integer so we can see if we've missed a loop. If loopFlag is
+     * more than 1, then we missed a cycle.
+     */
+    static uint8_t loopFlag;
+
+public:
+    Chassis(void) {}
+    void InititalizeChassis(void);
+
+    /* Where the bulk of the work for the motors gets done. */
+    bool ChassisLoop(Twist&);
+
+    /* Needed for managing motors. */
+    static void Timer4OverflowISRHandler(void);
+
+public:
+    /**
+     * SetTwist eliminates the need for DriveAt and TurnAt
+     */
+    void SetTwist(const Twist& twist);
+    Twist CalcOdomFromWheelMotion(void);
+
+    /**
+     * A utility function for converting robot speed to wheel speed. Left 
+     * public so that you can test more easily.
+     */
+    void SetWheelSpeeds(float, float);
+    void Stop(void) { SetWheelSpeeds(0, 0); }
+
+protected:
+    /**
+     * Initialization and Setup routines
+     */
+    void InitializeMotorControlTimer(void);
+    void InitializeMotors(void);
+    
+    void UpdateMotors(void);
+    void SetMotorEfforts(int16_t, int16_t);
+};

lib/Chassis/src/utils.cpp

@@ -0,0 +1,10 @@
+#include "utils.h"
+
+void TeleplotPrint(const char* var, float value)
+{
+    Serial.print('>');
+    Serial.print(var);
+    Serial.print(':');
+    Serial.print(value);
+    Serial.print('\n');
+}

lib/Chassis/src/utils.h

@@ -0,0 +1,32 @@
+#pragma once
+#include <Arduino.h>
+
+void TeleplotPrint(const char* var, float value);
+
+/**
+ * Pose includes information about the 2D pose of a robot: x, y, and heading.
+ */
+struct Pose
+{
+    float x = 0;
+    float y = 0;
+    float theta = 0;
+
+    Pose(void) {}
+    Pose(float x_, float y_, float th_) : x(x_), y(y_), theta(th_) {}
+};
+
+/**
+ * Twist is very similar to Pose, but we make a separate struct to avoid confusion.
+ * 
+ * Whereas Pose is position/heading, Twist contains velocity/ang. vel.
+ */
+struct Twist
+{
+    float u = 0;
+    float v = 0; // This will always be 0 in the robot frame.
+    float omega = 0;
+
+    Twist(void) {}
+    Twist(float u_, float v_, float om_) : u(u_), v(v_), omega(om_) {}
+};

lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

lib/Romi32U4Motors/src/Romi32U4MotorParameters.h

@@ -0,0 +1,5 @@
+#define KP_MOTOR    5.0
+#define KI_MOTOR    0.5
+#define KD_MOTOR    0.0             // don't use
+
+#define INTEGRAL_CAP_MOTOR  0.0     // if you want to add an integral cap

lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp

@@ -0,0 +1,18 @@
+#include "Romi32U4MotorTemplate.h"
+#include <PCint.h>
+
+void leftISR(void)
+{
+    leftMotor.ProcessEncoderTick();
+}
+
+void rightISR(void)
+{
+    rightMotor.ProcessEncoderTick();
+}
+
+void Romi32U4MotorBase::AttachInterrupts(void) 
+{
+    attachPCInt(digitalPinToPCInterrupt(LEFT_XOR), leftISR);
+    attachInterrupt(digitalPinToInterrupt(RIGHT_XOR), rightISR, CHANGE);
+}

lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h

@@ -0,0 +1,269 @@
+#pragma once
+
+#include <Arduino.h>
+#include <FastGPIO.h>
+#include "Romi32U4MotorParameters.h"
+
+// define the motor pins here
+#define PWM_L 10
+#define PWM_R 9
+#define DIR_L 16
+#define DIR_R 15
+
+#define LEFT_XOR   8
+#define LEFT_B     IO_E2
+#define RIGHT_XOR  7
+#define RIGHT_B    23
+
+#define OCR_L 0x8A
+#define OCR_R 0x88
+
+class Romi32U4MotorBase
+{
+protected: 
+    // For prettier printing
+    const String name;
+
+    // Used to control the motors in different ways
+    enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED};
+    volatile CTRL_MODE ctrlMode = CTRL_DIRECT;
+
+    // Used to manage PID coefficients; note that the defaults stink for speed control.
+    float Kp = KP_MOTOR;
+    float Ki = KI_MOTOR;
+    float Kd = KD_MOTOR;
+
+    // Used to keep track of the target speed, in counts / interval.
+    float targetSpeed = 0;
+
+    /**
+     * This is the speed of the motor, in "encoder counts / encoder interval".
+     * The encoder interval is set in Robot::InitializeMotorControlTimer.
+     */ 
+    volatile int16_t speed = 0;
+
+    // For tracking the error integral (sum)
+    float sumError = 0;
+
+    // For tracking derivative (difference)
+    float prevError = 0;
+
+    // Maximum effort
+    int16_t maxEffort = 420;
+
+    // Keeps track of encoder changes
+    volatile int16_t prevCount;
+    volatile int16_t encCount;
+    volatile int8_t lastA;
+    volatile int8_t lastB;
+
+    /**
+     * Sets the duty cycle. Must be declared for specific motor.
+     */
+    virtual void SetEffort(int16_t effort) = 0;
+
+    /** 
+     * Used to set the motor _effort_ directly, which is mostly used for testing.
+     * 
+     * Calling this function will switch the control mode to DIRECT, if needed, meaning speed control is lost.
+     */
+    void SetMotorEffortDirect(int16_t effort)
+    {
+        ctrlMode = CTRL_DIRECT;
+        SetEffort(effort);
+    }
+
+    /**
+     * calcEncoderDelta() takes a 'snapshot of the encoders and 
+     * stores the change since the last call, which has units of "encoder ticks/motor interval" 
+     * 
+     * We also use the function for zeroing the delta (for example, when switching control modes),
+     * so interrupts are cleared when accessing encCount.
+     */
+    int16_t CalcEncoderDelta(void)
+    {
+        cli();
+        int16_t currCount = encCount;
+        sei();
+
+        int16_t speed = currCount - prevCount;
+        prevCount = currCount;
+
+        return speed;
+    }
+
+    /**
+     * Sets the target speed in "encoder ticks/16 ms interval"
+     * */
+    void SetTargetSpeed(float target)
+    {
+        targetSpeed = target;
+
+        if(ctrlMode != CTRL_SPEED)
+        {
+            // Reset the error integral if we are switching from another mode
+            // Otherwise, the robot may jump due to residual integral
+            sumError = 0;
+
+            // Also set prevCount to encCount so to avoid speed jumps when switching mode
+            CalcEncoderDelta();
+        }
+
+        ctrlMode = CTRL_SPEED;
+    }
+
+    /**
+     * ControlMotorSpeed implements the PID controller. It should _not_ be called by user code.
+     * Instead, ControlMotorSpeed is called from Chassis::UpdateMotors, which is on a timer schedule.
+    */
+    void ControlMotorSpeed(void)
+    {
+        if(ctrlMode == CTRL_SPEED)
+        {
+            /**
+             * TODO: Implement PI controller. Only P at the moment.
+             */
+
+            // Calculate the error in speed
+            float error = targetSpeed - speed;
+
+            sumError+= error;
+
+            int16_t effort = Kp * error + Ki * sumError;
+
+            // Set the effort for the motor
+            SetEffort(effort);
+
+#ifdef __MOTOR_DEBUG__
+                Serial.print('>');
+                Serial.print(name);
+                Serial.print("_target:");
+                Serial.println(targetSpeed);
+                Serial.print('>');
+                Serial.print(name);
+                Serial.print("_speed:");
+                Serial.println(speed);
+                Serial.print('>');
+                Serial.print(name);
+                Serial.print("_error:");
+                Serial.println(error);
+#endif
+        }    
+    }
+
+    static void AttachInterrupts(void);
+
+    /**
+     * InitializeMotorPWMTimer() should be called near the beginning of the program.
+     * It sets up Timer4 to run at 38 kHz, which is used to both drive the PWM signal for the motors
+     * and (tangentially) allow for a 38 kHz signal on pin 11, which can be used, say, to drive an
+     * IR LED at a common rate.
+     * 
+     * Timer 1 has the following configuration:
+     *  prescaler of 1
+     *  outputs enabled on channels A (pin 9), B (pin 10) and C (pin 11)
+     *  fast PWM mode
+     *  top of 420, which will be the max speed
+     *  frequency is then: 16 MHz / [1 (prescaler) / (420 + 1)] = 38.005 kHz
+     * */
+    static void InitializePWMTimerAndInterrupts(void)
+    {
+        Serial.println("InitMotor()");
+
+        noInterrupts(); //disable interrupts while we set Timer1 registers
+
+        TCCR1A = 0xA2; //0b10100010; //Fast PWM + enable A and B; change to 0xAA to enable C on pin 11
+        TCCR1B = 0x19; //0b00011001; //Fast PWM
+        ICR1 = 420;    //runs at 38kHz; lowers speed for given effort by 5% from Pololu version
+
+        //set all three outputs to zero
+        OCR1A = 0;
+        OCR1B = 0;
+        OCR1C = 0; //can be used to create 38 kHz signal on pin 11; must enable output in TCCR1A above
+        
+        /**
+         * Call a static function to set up the left and right motor interrupts
+         */
+        AttachInterrupts();
+
+        interrupts(); //re-enable interrupts
+
+        Serial.println("/InitMotor()");
+    }
+
+public:
+    Romi32U4MotorBase(const String& nm) : name(nm) {}
+    void SetPIDCoeffs(float p, float i, float d) {Kp = p; Ki = i; Kd = d; sumError = 0;}
+};
+
+template <uint8_t encXOR, uint8_t encB, uint8_t PWM, uint8_t DIR, uint8_t OCR> 
+    class Romi32U4EncodedMotor : public Romi32U4MotorBase
+{
+protected:
+    void InitializeMotor(void)
+    {
+        FastGPIO::Pin<PWM>::setOutputLow();
+        FastGPIO::Pin<DIR>::setOutputLow();
+
+        InitializeEncoder();
+    }
+
+    /**
+     * SetEffort is used internally to set the motor effort without changing the control mode.
+     */
+    void SetEffort(int16_t effort)
+    {
+        bool reverse = 0;
+
+        if (effort < 0)
+        {
+            effort = -effort; // Make speed a positive quantity.
+            reverse = 1;      // Reverse the direction.
+        }
+        if (effort > maxEffort)
+        {
+            effort = maxEffort;
+        }
+
+        FastGPIO::Pin<DIR>::setOutput(reverse);
+
+        /**
+         * This line is a little esoteric, but it sets the duty cycle for the appropriate PWM pin.
+         * Note that TOP for the PWM is 420, so the duty cycle will be (effort / 420) * 100 (in %).
+         */
+        _SFR_MEM16(OCR) = effort;
+    }
+
+    void InitializeEncoder(void)
+    {    
+        Serial.println("InitEnc()");
+
+        // Set the pins as pulled-up inputs.
+        FastGPIO::Pin<encXOR>::setInputPulledUp();
+        FastGPIO::Pin<encB>::setInputPulledUp();
+
+        // Initialize the variables so that the speed will start as 0
+        lastB = FastGPIO::Pin<encB>::isInputHigh();
+        lastA = FastGPIO::Pin<encXOR>::isInputHigh() ^ lastB;
+
+        Serial.println("/InitEnc()");
+    }
+
+public:
+    Romi32U4EncodedMotor(const String& name) : Romi32U4MotorBase(name) {}
+    void ProcessEncoderTick(void) 
+    {
+        bool newB = FastGPIO::Pin<encB>::isInputHigh();
+        bool newA = FastGPIO::Pin<encXOR>::isInputHigh() ^ newB;
+
+        encCount += (lastA ^ newB) - (newA ^ lastB);
+
+        lastA = newA;
+        lastB = newB;
+    }
+
+    friend class Chassis; // Allow Chassis to call protected methods directly
+};
+
+extern Romi32U4EncodedMotor<LEFT_XOR, LEFT_B, PWM_L, DIR_L, OCR_L> leftMotor;
+extern Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor;

lib/Servo32u4/src/servo32u4.cpp

@@ -0,0 +1,185 @@
+#include <servo32u4.h>
+
+uint16_t Servo32U4Base::setMinMaxMicroseconds(uint16_t min, uint16_t max)
+{
+    // swap if in the wrong place
+    if(min > max) {uint16_t temp = min; min = max; max = temp;}
+
+    usMin = min;
+    usMax = max;
+
+    return usMax - usMin; //return the range, in case the user wants to do a sanity check
+}
+
+void Servo32U4Pin5::attach(void) 
+{
+    pinMode(5, OUTPUT); // set pin as OUTPUT
+
+    cli();
+
+    // clear then set the OCR3A bits (pin 5)
+    TCCR3A = 0x82; //WGM
+    TCCR3B = 0x1A; //WGM + CS = 8
+    ICR3 = 39999; //20ms
+
+    sei();
+
+    isAttached = true;
+}
+
+void Servo32U4Pin5::detach(void)
+{
+    cli();
+
+    // clear the OCR3A bits
+    TCCR3A &= 0x7f; //cancel OCR3A
+    sei();
+
+    isAttached = false;
+}
+
+void Servo32U4Pin5::writeMicroseconds(uint16_t microseconds)
+{
+    if (!isAttached)
+    {
+        attach();
+    }
+
+    microseconds = constrain(microseconds, usMin, usMax);
+
+    //prescaler is 8, so 1 timer count = 0.5 us
+    OCR3A = (microseconds << 1) - 1; // multiplies by 2
+}
+
+void Servo32U4Pin6::attach(void) 
+{
+    pinMode(6, OUTPUT); // set pin as OUTPUT
+
+    cli();
+
+    // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
+    TCCR4C |= 0x05;
+
+    sei();
+
+    isAttached = true;
+}
+
+void Servo32U4Pin6::detach(void) 
+{
+    cli();
+
+    // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
+    TCCR4C &= ~0x05;
+
+    sei();
+
+    isAttached = false;
+}
+
+
+// Resolution is 64 us; not great, but shouldn't be too constraining
+void Servo32U4Pin6::writeMicroseconds(uint16_t microseconds)
+{
+    if (!isAttached)
+    {
+        attach();
+    }
+
+    microseconds = constrain(microseconds, usMin, usMax);
+
+    //prescaler is 512, so 1 timer count = 32 us
+    //but be sure to set TC4H first!
+    TC4H = 0;
+    OCR4D = (microseconds >> 5) - 1; // divides by 32
+}
+
+void Servo32U4Pin13::attach(void) 
+{
+    pinMode(13, OUTPUT); // set pin as OUTPUT
+
+    cli();
+
+    // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
+    TCCR4A |= 0x82;
+
+    sei();
+
+    isAttached = true;
+}
+
+void Servo32U4Pin13::detach(void) 
+{
+    cli();
+
+    // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
+    // If you're not using it for anything else, you can safely just set TCCR4A = 0,
+    // but we'll cancel the bits we set above
+    TCCR4A &= ~0x82;
+
+    sei();
+
+    isAttached = false;
+}
+
+// Resolution is 64 us; not great, but shouldn't be too constraining
+void Servo32U4Pin13::writeMicroseconds(uint16_t microseconds)
+{
+    if (!isAttached)
+    {
+        attach();
+    }
+
+    microseconds = constrain(microseconds, usMin, usMax);
+
+    //prescaler is 512, so 1 timer count = 32 us
+    //but be sure to set TC4H first!
+    TC4H = 0;
+    OCR4A = (microseconds >> 5) - 1; // divides by 32
+}
+
+void Servo32U4Pin12::attach(void) 
+{
+    pinMode(12, OUTPUT); // set pin as OUTPUT
+
+    cli();
+
+    // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
+    TCCR4C |= 0x05;
+
+    //Invert the output on pin 12
+    TCCR4B |= 0x80;
+
+    sei();
+
+    isAttached = true;
+}
+
+void Servo32U4Pin12::detach(void) 
+{
+    cli();
+
+    // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
+    TCCR4C = 0x00;
+    TCCR4B &= ~0x80;
+
+    sei();
+
+    isAttached = false;
+}
+
+// Resolution is 64 us; not great, but shouldn't be too constraining
+void Servo32U4Pin12::writeMicroseconds(uint16_t microseconds)
+{
+    if (!isAttached)
+    {
+        attach();
+    }
+
+    microseconds = constrain(microseconds, usMin, usMax);
+
+    //prescaler is 512, so 1 timer count = 32 us
+    //but be sure to set TC4H first!
+    TC4H = 0;
+    OCR4D = (microseconds >> 5) - 1; // divides by 32
+}

lib/Servo32u4/src/servo32u4.h

@@ -0,0 +1,182 @@
+#pragma once
+
+#include <Arduino.h>
+
+/** \file Manages servos on up to three pins: 
+ *  pin 5, 
+ *  pin 13, and
+ *  either pin 6 OR 12 (don't use 6 and 12 simultaneously!).
+ * 
+ * THIS IS DESIGNED TO WORK WITH THE ROMI SET UP WITH THE CHASSIS CLASS
+ * 
+ * The Chassis class sets up Timer4, which is needed for pins 6, 12, and 13!
+ * 
+ * Pin 5 uses Timer3 and has the fewest restrictions as well as the highest precision (0.5 us).
+ * 
+ * The other three have potential conflicts and much lower resolution (64 us).
+ * 
+ * Pin 13 uses Timer4, but shares functionality with the bootloader, which flashes the LED connected
+ * to that pin. So, your servo will go a bit nuts when you upload -- be careful!
+ * 
+ * Pins 6 and 12 use Timer4, but share the same output compare -- except that they are inverted.
+ * Practically, that means you can use one or the other. Note that pin 6 is shared with the buzzer,
+ * so you'll need to cut the buzzer trace if you want to use that pin and maintain your sanity.
+ * 
+ * If using the Chassis class, Timer4 is used for the control loop, so be sure you don't create any 
+ * conflicts.
+ * 
+ * writeMicroseconds is protected so that you don't call it directly (which will make it move 
+ * really fast). Instead, set the target and call update() regularly.
+ * 
+ * */
+
+/** \class Servo32U4Base
+ * \brief Base class class for servos.
+ * 
+ * Each derived class controls a specific pin (obvious from the name).
+ * 
+ * Defaults to a range of 1000 - 2000 us, but can be customized.
+ */
+class Servo32U4Base
+{
+protected:
+    uint16_t usMin = 1000;
+    uint16_t usMax = 2000;
+
+    uint8_t feedbackPin = -1;
+    bool isAttached = false;
+
+    uint16_t targetPos = 1500;
+    uint16_t currentPos = 1500;
+
+public:
+    // Virtual functions defined for each specific class
+    virtual void attach(void) = 0;
+    virtual void detach(void) = 0;
+    void setTargetPos(uint16_t target) {targetPos = target;}
+
+    /**
+     * update() moves the servo towards the target position. You will want to 
+     * change it to return a bool to detect the event of reaching the target.
+     */
+    void update(void) 
+    {
+        if(targetPos == currentPos) {} // no need to update
+
+        else if(abs(targetPos - currentPos) <= 40) currentPos = targetPos;
+        else if(targetPos > currentPos) currentPos += 40;
+        else if(targetPos < currentPos) currentPos -= 40;
+
+        writeMicroseconds(currentPos);
+    }
+
+    uint16_t setMinMaxMicroseconds(uint16_t min, uint16_t max);
+
+    virtual void writeMicroseconds(uint16_t microseconds) = 0;
+};
+
+/** \class Servo32U4Pin5
+ * \brief A servo class to control a servo on pin 5.
+ * 
+ * Servo32U4 uses output compare on Timer3 to control the pulse to the servo. 
+ * The 16-bit Timer3 is set up with a pre-scaler of 8, TOP of 39999 + 1 => 20 ms interval.
+ * 
+ * OCR3A controls the pulse on pin 5 -- this servo must be on pin 5! 
+ * 
+ * Defaults to a range of 1000 - 2000 us, but can be customized.
+ */
+class Servo32U4Pin5 : public Servo32U4Base
+{
+public:
+    void attach(void);
+    void detach(void);
+
+protected:    
+    void writeMicroseconds(uint16_t microseconds);
+};
+
+/** \class Servo32U4Pin6
+ * \brief A servo class to control a servo on pin 6.
+ * 
+ * Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo. 
+ * _In Chassis::init(),
+ * The 8-bit Timer4 is set up with a pre-scaler of 1024, TOP of 249 + 1 => 16 ms interval.
+ * 
+ * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK, 
+ * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach()
+ * 
+ * OCR4D controls the pulse on pin 6 -- this servo must be on pin 6! 
+ * 
+ * Note that pin 6 controls the buzzer, so you'll go crazy if you don't cut the buzzer trace. 
+ * See: https://www.pololu.com/docs/0J69/3.2 for how to cut the trace.
+ * 
+ * Defaults to a range of 1000 - 2000 us, but can be customized.
+ * 
+ * Note that because we're using an 8-bit timer, resolution is only 64 us.
+ */
+class Servo32U4Pin6 : public Servo32U4Base
+{
+public:
+    void attach(void);
+    void detach(void);
+
+protected:
+    void writeMicroseconds(uint16_t microseconds);
+};
+
+/** \class Servo32U4Pin13
+ * \brief A servo class to control a servo on pin 13.
+ * 
+ * Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo. 
+ * _In Chassis::init(),
+ * The 8-bit Timer4 is set up with a pre-scaler of 1024, TOP of 249 + 1 => 16 ms interval.
+ * 
+ * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK, 
+ * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach()
+ * 
+ * OCR4A controls the pulse on pin 13 -- this servo must be on pin 13! 
+ * 
+ * Note that there is a useful LED on pin 13 -- you'll lose that functionality.
+ * 
+ * Pin 13 is also used during the upload process, so your servo will go crazy when uploading!
+ * 
+ * Defaults to a range of 1000 - 2000 us, but can be customized.
+ * 
+ * Note that because we're using an 8-bit timer, resolution is only 64 us.
+ */
+class Servo32U4Pin13 : public Servo32U4Base
+{
+public:
+    void attach(void);
+    void detach(void);
+
+protected:
+    void writeMicroseconds(uint16_t microseconds);
+};
+
+/** \class Servo32U4Pin12
+ * \brief A servo class to control a servo on pin 12.
+ * 
+ * Servo32U4Pin12 uses output compare on Timer4 to control the pulse to the servo. 
+ * _In Chassis::init(),
+ * 
+ * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK, 
+ * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach()
+ * 
+ * ^OCR4D controls the pulse on pin 12 -- this servo _must_ be on pin 12!
+ * 
+ * DO NOT USE IN CONJUNCTION WITH Servo32U4Pin6
+ * 
+ * Defaults to a range of 1000 - 2000 us, but can be customized.
+ * 
+ * Note that because we're using an 8-bit timer, resolution is only 64 us.
+ */
+class Servo32U4Pin12 : public Servo32U4Base
+{
+public:
+    void attach(void);
+    void detach(void);
+
+protected:
+    void writeMicroseconds(uint16_t microseconds);
+};

platformio.ini

@@ -0,0 +1,29 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:a-star32U4]
+platform = atmelavr
+board = a-star32U4
+framework = arduino
+
+monitor_speed = 115200
+
+lib_deps = 
+    Wire
+    https://github.com/gcl8a/IRDecoder
+    https://github.com/gcl8a/Button 
+    https://github.com/gcl8a/event_timer
+    https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities
+
+build_flags = 
+;    -D__MOTOR_DEBUG__
+;    -D__LOOP_DEBUG__
+;    -D__IMU_DEBUG__
+    -D__NAV_DEBUG__

src/main.cpp

@@ -0,0 +1,25 @@
+#include <Arduino.h>
+#include "robot.h"
+
+Robot robot;
+
+void setup() 
+{
+  Serial.begin(115200);
+
+/**
+ * If you define __SETUP_DEBUG__ in your .ini file, this line will make the program wait
+ * until the Serial is set up so you can debug. WARNING: If you do that, you _must_ open 
+ * the Serial Monitor for anything to happen!
+ */
+#ifdef __SETUP_DEBUG__
+  while(!Serial){delay(5);}
+#endif
+
+  robot.InitializeRobot();
+}
+
+void loop() 
+{
+  robot.RobotLoop();
+}

src/robot-nav.cpp

@@ -0,0 +1,72 @@
+/**
+ * robot-nav.cpp is where you should put navigation routines.
+ */
+
+#include "robot.h"
+
+void Robot::UpdatePose(const Twist& twist)
+{
+    /**
+     * TODO: Add your FK algorithm to update currPose here.
+     */
+
+#ifdef __NAV_DEBUG__
+    TeleplotPrint("x", currPose.x);
+    TeleplotPrint("y", currPose.y);
+    TeleplotPrint("theta", currPose.theta);
+#endif
+
+}
+
+/**
+ * Sets a destination in the lab frame.
+ */
+void Robot::SetDestination(const Pose& dest)
+{
+    /**
+     * TODO: Turn on LED, as well.
+     */
+    Serial.print("Setting dest to: ");
+    Serial.print(dest.x);
+    Serial.print(", ");
+    Serial.print(dest.y);
+    Serial.print('\n');
+
+    destPose = dest;
+    robotState = ROBOT_DRIVE_TO_POINT;
+}
+
+bool Robot::CheckReachedDestination(void)
+{
+    bool retVal = false;
+    /**
+     * TODO: Add code to check if you've reached destination here.
+     */
+
+    return retVal;
+}
+
+void Robot::DriveToPoint(void)
+{
+    if(robotState == ROBOT_DRIVE_TO_POINT)
+    {
+        /**
+         * TODO: Add your IK algorithm here. 
+         */
+
+#ifdef __NAV_DEBUG__
+        // Print useful stuff here.
+#endif
+
+        /**
+         * TODO: Call chassis.SetTwist() to command the motion, based on your calculations above.
+         */
+    }
+}
+
+void Robot::HandleDestination(void)
+{
+    /**
+     * TODO: Stop and change state. Turn off LED.
+     */
+}

src/robot-remote.cpp

@@ -0,0 +1,83 @@
+/**
+ * robot-remote.cpp implements features of class Robot that are related to processing
+ * remote control commands. It also manages modes. You might want to trim away some of 
+ * the code that isn't needed later in the term.
+ */
+#include "robot.h"
+
+#include <ir_codes.h>
+#include <IRdecoder.h>
+
+/**
+ * IRDecoder decoder is declared extern in IRdecoder.h (for ISR purposes), 
+ * so we _must_ name it decoder.
+ */
+#define IR_PIN 17
+IRDecoder decoder(IR_PIN);
+
+void Robot::HandleKeyCode(int16_t keyCode)
+{ 
+    Serial.println(keyCode);
+
+    // Regardless of current state, if ENTER is pressed, go to idle state
+    if(keyCode == ENTER_SAVE) { EnterIdleState(); keyString = "";}
+
+
+    // If STOP/MODE is pressed, it toggles control mode (auto <-> teleop)
+    else if(keyCode == STOP_MODE) 
+    {
+        if(robotCtrlMode == CTRL_AUTO) {EnterTeleopMode(); }
+        else if(robotCtrlMode == CTRL_TELEOP) {EnterAutoMode(); }
+        EnterIdleState(); // Idle to avoid surprises
+        keyString = "";
+    }
+
+    if(robotCtrlMode == CTRL_AUTO)
+    {
+        /**
+         * TODO: Add destinations.
+         */
+        switch(keyCode)
+        {
+            case NUM_2:
+                SetDestination(Pose(60, 0, 0)); 
+                break;
+            case NUM_4:
+                break;
+            case NUM_6:
+                break;
+            case NUM_8:
+                break;
+        }
+        keyString = "";
+    }
+
+    else if(robotCtrlMode == CTRL_TELEOP)
+    {
+        switch(keyCode)
+        {
+            case UP_ARROW:
+                chassis.SetTwist(Twist(20, 0, 0));
+                break;
+            case RIGHT_ARROW:
+                break;
+            case DOWN_ARROW:
+                break;
+            case LEFT_ARROW:
+                break;
+        }
+        keyString = "";
+    }    
+}
+
+void Robot::EnterTeleopMode(void)
+{
+    Serial.println("-> TELEOP");
+    robotCtrlMode = CTRL_TELEOP;
+}
+
+void Robot::EnterAutoMode(void)
+{
+    Serial.println("-> AUTO");
+    robotCtrlMode = CTRL_AUTO;
+}

src/robot.cpp

@@ -0,0 +1,62 @@
+#include "robot.h"
+#include <IRdecoder.h>
+
+void Robot::InitializeRobot(void)
+{
+    chassis.InititalizeChassis();
+
+    /**
+     * Initialize the IR decoder. Declared extern in IRdecoder.h; see robot-remote.cpp
+     * for instantiation and setting the pin.
+     */
+    decoder.init();
+
+    /**
+     * TODO: Set pin 13 HIGH when navigating and LOW when destination is reached.
+     * Need to set as OUTPUT here.
+     */
+}
+
+void Robot::EnterIdleState(void)
+{
+    chassis.Stop();
+
+    Serial.println("-> IDLE");
+    robotState = ROBOT_IDLE;
+}
+
+/**
+ * The main loop for your robot. Process both synchronous events (motor control),
+ * and asynchronous events (IR presses, distance readings, etc.).
+*/
+void Robot::RobotLoop(void) 
+{
+    /**
+     * Handle any IR remote keypresses.
+     */
+    int16_t keyCode = decoder.getKeyCode();
+    if(keyCode != -1) HandleKeyCode(keyCode);
+
+    /**
+     * Run the chassis loop, which handles low-level control.
+     */
+    Twist velocity;
+    if(chassis.ChassisLoop(velocity))
+    {
+        // We do FK regardless of state
+        UpdatePose(velocity);
+
+        /**
+         * Here, we break with tradition and only call these functions if we're in the 
+         * DRIVE_TO_POINT state. CheckReachedDestination() is expensive, so we don't want
+         * to do all the maths when we don't need to.
+         * 
+         * While we're at it, we'll toss DriveToPoint() in, as well.
+         */ 
+        if(robotState == ROBOT_DRIVE_TO_POINT)
+        {
+            DriveToPoint();
+            if(CheckReachedDestination()) HandleDestination();
+        }
+    }
+}

src/robot.h

@@ -0,0 +1,64 @@
+#pragma once
+
+#include "chassis.h"
+
+class Robot
+{
+protected:
+    /**
+     * We define some modes for you. SETUP is used for adjusting gains and so forth. Most
+     * of the activities will run in AUTO. You shouldn't need to mess with these.
+     */
+    enum ROBOT_CTRL_MODE
+    {
+        CTRL_TELEOP,
+        CTRL_AUTO,
+    };
+    ROBOT_CTRL_MODE robotCtrlMode = CTRL_AUTO;
+
+    /**
+     * robotState is used to track the current task of the robot. You will add new states as 
+     * the term progresses.
+     */
+    enum ROBOT_STATE 
+    {
+        ROBOT_IDLE,
+        ROBOT_DRIVE_TO_POINT,
+    };
+    ROBOT_STATE robotState = ROBOT_IDLE;
+
+    /* Define the chassis*/
+    Chassis chassis;
+
+    // For managing key presses
+    String keyString;
+
+    /**
+     * For tracking current pose and the destination.
+     */
+    Pose currPose;
+    Pose destPose;
+    
+public:
+    Robot(void) {keyString.reserve(10);}
+    void InitializeRobot(void);
+    void RobotLoop(void);
+
+protected:
+    /* For managing IR remote key presses*/
+    void HandleKeyCode(int16_t keyCode);
+
+    /* State changes */    
+    void EnterIdleState(void);
+
+    /* Mode changes */
+    void EnterTeleopMode(void);
+    void EnterAutoMode(void);
+
+    // /* Navigation methods.*/
+    void UpdatePose(const Twist& u);
+    void SetDestination(const Pose& destination);
+    void DriveToPoint(void);
+    bool CheckReachedDestination(void);
+    void HandleDestination(void);
+};

test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html