C33

.clang-format

@@ -4,5 +4,18 @@ TabWidth: 4
 IndentWidth: 4
 ColumnLimit: 0
 AlignConsecutiveAssignments: Consecutive
-AllowShortCaseLabelsOnASingleLine: true
 AlignConsecutiveDeclarations: Consecutive
+
+AllowShortBlocksOnASingleLine: Always
+AllowShortCaseExpressionOnASingleLine: true
+AllowShortCaseLabelsOnASingleLine: true
+AllowShortCompoundRequirementOnASingleLine: true
+AllowShortEnumsOnASingleLine: true
+AllowShortFunctionsOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: Never
+AllowShortLambdasOnASingleLine: All
+AllowShortLoopsOnASingleLine: true
+
+AlignAfterOpenBracket: BlockIndent
+BinPackArguments: false
+BinPackParameters: false

.gitignore

@@ -1,37 +1,4 @@
-# Prerequisites
-*.d
-
-# Compiled Object files
-*.slo
-*.lo
-*.o
-*.obj
-
-# Precompiled Headers
-*.gch
-*.pch
-
-# Compiled Dynamic libraries
-*.so
-*.dylib
-*.dll
-
-# Fortran module files
-*.mod
-*.smod
-
-# Compiled Static libraries
-*.lai
-*.la
-*.a
-*.lib
-
-# Executables
-*.exe
-*.out
-*.app
-
-build/
-.vscode/c_cpp_properties.json
-
-WiFi_secrets.hpp
+.pio
+.vscode/
+.cache/
+compile_commands.json

src/ODriveEnums.h → include/ODriveEnums.h

@@ -1,6 +1,7 @@
 
-#ifndef ODriveEnums_h
-#define ODriveEnums_h
+#pragma once
+
+#include <array>
 
 // ODrive.GpioMode
 enum ODriveGpioMode {
@@ -205,5 +206,3 @@ enum ODriveCanError {
     CAN_ERROR_NONE              = 0x00000000,
     CAN_ERROR_DUPLICATE_CAN_IDS = 0x00000001,
 };
-
-#endif

include/README

@@ -0,0 +1,37 @@
+
+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 convention is to give header files names that end with `.h'.
+
+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

src/SparkFun_BNO080_Arduino_Library.h → include/SparkFun_BNO080_Arduino_Library.h

@@ -48,7 +48,9 @@
 #else
 
 // The catch-all default is 32
+#ifndef I2C_BUFFER_LENGTH
 #define I2C_BUFFER_LENGTH 32
+#endif
 
 #endif
 //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=

src/balancer.hpp → include/balancer.hpp

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <Arduino.h>
+
 #include "bot_can.hpp"
 #include "imu_wrapper.hpp"
 #include "pid.hpp"
@@ -69,13 +71,14 @@ struct BotController {
         Error
     };
 
+    static constexpr std::array StateText = {"Idle", "Active", "Error"};
+
     void begin() {
         vertical_timer.reset();
     }
 
     void step() {
         // Blink orange LED at 1 sec
-        bot::blink(1000);
 
         // Run State Machine
         state = run_state_machine(state);
@@ -91,8 +94,8 @@ struct BotController {
         const float drive_cmd = 0.0f;  // rx.drive * (1.0f - abs(steer_cmd));
 
         // Motor speeds
-        const float vel_right = bot_can.right_motor.get_encoder_estimates_msg.Vel_Estimate * (settings.kWheelDiameter * bot::kPi);         // [m/s] right wheel speed
-        const float vel_left  = -1.0f * bot_can.left_motor.get_encoder_estimates_msg.Vel_Estimate * (settings.kWheelDiameter * bot::kPi);  // [m/s] left wheel speed
+        const float vel_right = +1.0f * bot_can.right_motor.get_encoder_estimates_msg.Vel_Estimate * (settings.kWheelDiameter * bot::kPi);  // [m/s] right wheel speed
+        const float vel_left  = -1.0f * bot_can.left_motor.get_encoder_estimates_msg.Vel_Estimate * (settings.kWheelDiameter * bot::kPi);   // [m/s] left wheel speed
 
         // TODO:  Verify yaw rate calculation matches gyro reading
         const float vel_actual = (vel_right + vel_left) / 2.0f + (d2r(imu.pitch_rate) * settings.kComHeight);  // [m/s] Vehicle speed
@@ -112,16 +115,33 @@ struct BotController {
             bot_can.left_motor.set_input_torque_msg.Input_Torque  = 0.0f;
         }
 
-        Serial.print("Pitch: ");
-        Serial.print(imu.pitch);
+        // Serial.print("Pitch: ");
+        // Serial.println(imu.pitch);
 
-        Serial.print("\tVel: ");
-        Serial.println(vel_actual);
+        // Serial.print("\tL: ");
+        // Serial.print(vel_left);
+        // Serial.print("\tR: ");
+        // Serial.print(vel_right);
+        // Serial.println();
     }
 
     State run_state_machine(State state) {
         State next_state = state;
 
+        const bool left_error  = bot_can.left_motor.heartbeat_msg.Axis_Error != 0;
+        const bool right_error = bot_can.right_motor.heartbeat_msg.Axis_Error != 0;
+
+        digitalWrite(PIN_D7, left_error || right_error);
+        if (left_error) {
+            Serial.print("Left Error: ");
+            Serial.println(bot_can.left_motor.heartbeat_msg.Axis_Error, HEX);
+        }
+
+        if (right_error) {
+            Serial.print("Right Error: ");
+            Serial.println(bot_can.right_motor.heartbeat_msg.Axis_Error, HEX);
+        }
+
         switch (state) {
             case State::Idle: {
                 // If pitch is within 5 degrees for 2 seconds, enable motors
@@ -138,32 +158,39 @@ struct BotController {
 
             case State::Active: {
                 // Check for errors
-                bool pitch_over  = fabsf(imu.pitch) > 30.0f;
-                bool imu_timeout = imu.getIsTimedOut();
-
-                bool left_error  = bot_can.left_motor.heartbeat_msg.Axis_Error != 0;
-                bool right_error = bot_can.right_motor.heartbeat_msg.Axis_Error != 0;
+                const bool pitch_over  = fabsf(imu.pitch) > 30.0f;
+                const bool imu_timeout = imu.getIsTimedOut();
 
                 if (pitch_over || left_error || right_error) {
                     vertical_timer.reset();
 
                     bot_can.setAxisStates(AXIS_STATE_IDLE);
                     next_state = State::Idle;
+
+                    if (pitch_over) {
+                        Serial.println("Pitch Over");
+                    }
                 }
 
                 if (imu_timeout) {
                     bot_can.setAxisStates(AXIS_STATE_IDLE);
                     next_state = State::Error;
+                    Serial.println("IMU Timeout");
                 }
 
             } break;
 
             case State::Error:
+                Serial.println("Error State!");
             default: {
                 bot_can.setAxisStates(AXIS_STATE_IDLE);
+                Serial.println("Invalid State!");
             } break;
         }
 
+        if (state != next_state)
+            Serial.println(StateText[static_cast<int>(next_state)]);
+
         return next_state;
     }