making tests run and integrating new pedals system stuff

lib/interfaces/include/AnalogSensorsInterface.h

@@ -5,7 +5,7 @@
 #include <algorithm>
 #include <SysClock.h>
 
-enum AnalogSensorStatus_e
+enum class AnalogSensorStatus_e
 {
     ANALOG_SENSOR_GOOD = 0,
     ANALOG_SENSOR_CLAMPED = 1,
@@ -54,9 +54,9 @@ class AnalogChannel
     {
         float conversion = lastSample * scale + offset;
         float clampedConversion = std::min(std::max(conversion, clampLow), clampHigh);
-        AnalogSensorStatus_e returnStatus = ANALOG_SENSOR_GOOD;
+        AnalogSensorStatus_e returnStatus = AnalogSensorStatus_e::ANALOG_SENSOR_GOOD;
         if (clamp && (conversion > clampHigh || conversion < clampLow))
-            returnStatus = ANALOG_SENSOR_CLAMPED;
+            returnStatus = AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED;
         return {
             lastSample,
             clamp ? clampedConversion : conversion,

lib/interfaces/include/SteeringEncoderInterface.h

@@ -3,7 +3,7 @@
 
 #include <tuple>
 
-enum SteeringEncoderStatus_e
+enum class SteeringEncoderStatus_e
 {
     STEERING_ENCODER_NOMINAL = 0,
     STEERING_ENCODER_MARGINAL = 1,

lib/interfaces/src/ORBIS_BR10.cpp

@@ -45,15 +45,15 @@ SteeringEncoderConversion_s OrbisBR10::convert()
 
     if (status_ & (ORBIS_BR10_BITMASK_GENERAL_ERROR | ORBIS_BR10_BITMASK_DETAILED_COUNTER_ERROR))
     {
-        returnConversion.status = STEERING_ENCODER_ERROR;
+        returnConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_ERROR;
     }
     else if (status_ == 0)
     {
-        returnConversion.status = STEERING_ENCODER_NOMINAL;
+        returnConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL;
     }
     else
     {
-        returnConversion.status = STEERING_ENCODER_MARGINAL;
+        returnConversion.status = SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL;
     }
     return returnConversion;
 }

lib/mock_interfaces/AMSInterface.h

@@ -1,16 +1,47 @@
-#ifndef AMSINTERFACE
-#define AMSINTERFACE
+#ifndef __AMS_INTERFACE_H__
+#define __AMS_INTERFACE_H__
+
+#include "SysClock.h"
+
+const unsigned long HEARTBEAT_INTERVAL                      = 20;   // milliseconds
+const unsigned long PACK_CHARGE_CRIT_TOTAL_THRESHOLD        = 420;
+const unsigned long PACK_CHARGE_CRIT_LOWEST_CELL_THRESHOLD  = 35000;
+
+const float DEFAULT_INIT_TEMP       = 40.0;
+const float DEFAULT_INIT_VOLTAGE    = 3.5;
+const float DEFAULT_TEMP_ALPHA      = 0.8;
+const float DEFAULT_VOLTAGE_ALPHA   = 0.8;
 
 class AMSInterface
 {
 public:
-    bool ok_high_;
-    bool heartbeat_status_;
-    AMSInterface() {}
-    bool ok_high() { return ok_high_; };
-    bool heartbeat_check(unsigned long curr_time) { return heartbeat_status_; };
+    AMSInterface(int sw_ok_pin, float init_temp, float init_volt, float temp_alpha, float volt_alpha){
+        // Set pin mode
+    };
+        /* Initialize interface pin mode */
+    void init(const SysTick_s &tick);
+
+    /* Write to Main ECU */
+    // Initialize output value
+    void set_start_state();
+    // Set output value
+    void set_state_ok_high(bool ok_high);
+
+    /* Monitor AMS state */
+    void set_heartbeat(const SysTick_s &tick);
+    bool heartbeat_received(const SysTick_s &tick);
+    bool is_below_pack_charge_critical_low_thresh();
+    bool is_below_pack_charge_critical_total_thresh();
+    bool pack_charge_is_critical();    
+
+    /* IIR filtered AMS readings */
+    float get_filtered_max_cell_temp();
+    float get_filtered_min_cell_voltage();
 
+    /* Retrieve CAN */
 private:
+    /* AMS CAN messages */
+    // Outbound
 };
 
-#endif /* AMSINTERFACE */
+#endif /* __AMS_INTERFACE_H__ */

lib/mock_interfaces/AnalogSensorsInterface.h

@@ -0,0 +1,64 @@
+#ifndef __ANALOGSENSOR_H__
+#define __ANALOGSENSOR_H__
+
+#include <tuple>
+#include <algorithm>
+
+enum class AnalogSensorStatus_e
+{
+    ANALOG_SENSOR_GOOD = 0,
+    ANALOG_SENSOR_CLAMPED = 1,
+};
+
+struct AnalogConversion_s
+{
+    int raw;
+    float conversion;
+    AnalogSensorStatus_e status;
+};
+
+template <int N>
+struct AnalogConversionPacket_s
+{
+    AnalogConversion_s conversions[N];
+};
+
+class AnalogChannel
+{
+public:
+// Data
+    AnalogChannel(); 
+// Functions
+    /// @brief Calculate sensor output and whether result is in sensor's defined bounds. DOES NOT SAMPLE.
+    /// @return Sensor's calculated output in real units, whether the result was clamped (AnalogSensorStatus_s)
+    AnalogConversion_s convert();
+};
+
+template <int N>
+class AnalogMultiSensor
+{
+public:
+    AnalogConversionPacket_s<N> data;
+// Functions
+
+    /// @brief Used by systems to get data out of this device when it's self-actualizing sampling & conversion.
+    /// @return Const ref to last data conversion.
+    const AnalogConversionPacket_s<N>& get()
+    {
+        return data;
+    }
+
+    /// @brief Performs unit conversions on all channels
+    void convert()
+    {
+        // for (int i = 0; i < N; i++)
+        // {
+        //     data.conversions[i] = channels_[i].convert();
+        // }
+    }
+
+    /// @brief Commands the underlying device to sample all channels and internally store the results
+    void sample();
+};
+
+#endif /* __ANALOGSENSOR_H__ */

lib/mock_interfaces/DashboardInterface.h

@@ -1,11 +1,48 @@
 #ifndef DASHBOARDINTERFACE
 #define DASHBOARDINTERFACE
+/* Enum for the modes on the dial, corresponds directly to dial index pos. */
+enum DialMode_e
+{
+    MODE_1,
+    MODE_2,
+    ACCEL_LAUNCH_CONTROL,
+    SKIDPAD,
+    AUTOCROSS,
+    ENDURANCE,
+};
+
+/* Enum for defined LED colors. ON will be LED's default color on dashboard*/
+enum LEDColors_e
+{
+    OFF,
+    ON,
+    YELLOW,
+    RED,
+};
+
+/* Enum to index the LED array. Each LED in the CAN message is represented in no particular order. */
+enum DashLED_e
+{
+    BOTS_LED,
+    LAUNCH_CONTROL_LED,
+    MODE_LED,
+    MECH_BRAKE_LED,
+    COCKPIT_BRB_LED,
+    INERTIA_LED,
+    GLV_LED,
+    CRIT_CHARGE_LED,
+    START_LED,
+    MC_ERROR_LED,
+    IMD_LED,
+    AMS_LED,
+};
+
 class DashboardInterface
 {
 private:
 public:
     bool start_button_status_;
-    bool start_button_pressed() { return  start_button_status_; };
+    bool startButtonPressed() { return  start_button_status_; };
 };
 
 #endif /* DASHBOARDINTERFACE */

lib/mock_interfaces/SteeringEncoderInterface.h

@@ -0,0 +1,33 @@
+#ifndef __UPPERSTEERINGSENSOR_H__
+#define __UPPERSTEERINGSENSOR_H__
+
+#include <tuple>
+
+enum class SteeringEncoderStatus_e
+{
+    STEERING_ENCODER_NOMINAL = 0,
+    STEERING_ENCODER_MARGINAL = 1,
+    STEERING_ENCODER_ERROR = 2,
+};
+
+struct SteeringEncoderConversion_s
+{
+    float angle;
+    SteeringEncoderStatus_e status;
+};
+
+class SteeringEncoderInterface
+{
+public:
+// Functions
+    /// @brief Commands the underlying steering sensor to sample and hold the result
+    virtual void sample();
+    /// @brief Calculate steering angle and whether result is in sensor's defined bounds. DOES NOT SAMPLE.
+    /// @return Calculated steering angle in degrees, upperSteeringStatus_s
+    virtual SteeringEncoderConversion_s convert();
+    /// @brief Set the upper steering sensor's offset. 0 degrees should be centered.
+    /// @param newOffset 
+    virtual void setOffset(float newOffset);
+};
+
+#endif /* __UPPERSTEERINGSENSOR_H__ */

lib/mock_interfaces/WatchdogInterface.h

@@ -0,0 +1,34 @@
+#ifndef __WATCHDOG_INTERFACE_H__
+#define __WATCHDOG_INTERFACE_H__
+
+#include "SysClock.h"
+const unsigned long WATCHDOG_KICK_INTERVAL = 7;    // milliseconds
+
+class WatchdogInterface
+{
+private:
+    /* Watchdog last kicked time */
+    unsigned long watchdog_time;
+
+    /* Watchdog output state */
+    bool watchdog_state;
+
+    /* Hardware interface pins */
+    int pin_watchdog_input_;
+
+public:
+    WatchdogInterface(int wd_input_pin){};
+
+    /* Initialize interface pin mode */
+    void init(const SysTick_s &tick);
+
+    /* Write to Main ECU */
+    // Initialize output value
+    void set_start_state();
+
+    /* Kick watchdog */
+    void kick_watchdog(const SysTick_s &tick);
+
+};
+
+#endif /* __WATCHDOG_INTERFACE_H__ */

lib/state_machine/include/MCUStateMachine.tpp

@@ -1,5 +1,5 @@
 
-// #include "MCUStateMachine.h"
+#include "MCUStateMachine.h"
 template <typename DrivetrainSysType>
 void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long current_millis)
 {
@@ -22,17 +22,17 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
     case CAR_STATE::TRACTIVE_SYSTEM_ACTIVE:
     {
         // TODO migrate to new pedals system
-        PedalsSystemData_s data;
-        // if (!drivetrain_->hv_over_threshold_on_drivetrain())
-        // {
-        //     set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis);
-        //     break;
-        // }
-        // if (dashboard_->start_button_pressed() && pedals_->mech_brake_active(data))
-        // {
-        //     set_state_(CAR_STATE::ENABLING_INVERTERS, current_millis);
-        //     break;
-        // }
+        auto data = pedals_->getPedalsSystemData();
+        if (!drivetrain_->hv_over_threshold_on_drivetrain())
+        {
+            set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis);
+            break;
+        }
+        if (dashboard_->startButtonPressed() && (data.pedalsCommand == PedalsCommanded_e::PEDALS_BRAKE_PRESSED))
+        {
+            set_state_(CAR_STATE::ENABLING_INVERTERS, current_millis);
+            break;
+        }
         break;
     }
 
@@ -105,7 +105,7 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
 
     case CAR_STATE::READY_TO_DRIVE:
     {
-
+        auto data = pedals_->getPedalsSystemData();
         if (!drivetrain_->hv_over_threshold_on_drivetrain())
         {
             set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis);
@@ -126,7 +126,7 @@ void MCUStateMachine<DrivetrainSysType>::tick_state_machine(unsigned long curren
         if (
             // bms_->ok_high() &&
             // imd_->ok_high() &&
-            !pedals_->max_duration_of_implausibility_exceeded(current_millis))
+            !data.implausibilityExceededMaxDuration)
         {
             // drivetrain_->command_drivetrain(controller_mux_->get_drivetrain_input(pedals_data, dashboard_data));
         }

lib/systems/include/PedalsSystem.h

@@ -14,14 +14,14 @@ const float PEDALS_RAW_TOO_LOW          = 0.5 / 5 * 4096;       // FSAE T.4.2.10
 const float PEDALS_RAW_TOO_HIGH         = 4.5 / 5 * 4096;       // FSAE T.4.2.10 Pedals are implausible above 4.5V raw reading
 
 // Enums
-enum PedalsStatus_e
+enum class PedalsStatus_e
 {
     PEDALS_NOMINAL = 0,
     PEDALS_MARGINAL = 1,
     PEDALS_IMPLAUSIBLE = 2,
 };
 
-enum PedalsCommanded_e
+enum class PedalsCommanded_e
 {
     PEDALS_NONE_PRESSED = 0,
     PEDALS_ACCEL_PRESSED = 1,
@@ -34,6 +34,7 @@ struct PedalsSystemData_s
     PedalsCommanded_e pedalsCommand;
     PedalsStatus_e accelStatus;
     PedalsStatus_e brakeStatus;
+    bool implausibilityExceededMaxDuration;
     bool persistentImplausibilityDetected;
     float accelPercent;
     float brakePercent;
@@ -69,17 +70,18 @@ class PedalsSystem
     }
     PedalsSystem()
     {
-        parameters_ = {
-            .pedalsImplausiblePercent = PEDALS_IMPLAUSIBLE_PERCENT,
-            .pedalsMarginalPercent = PEDALS_MARGINAL_PERCENT,
-            .pedalsRawTooLow = PEDALS_RAW_TOO_LOW,
-            .pedalsRawTooHigh = PEDALS_RAW_TOO_HIGH,
-        };
+        parameters_.pedalsImplausiblePercent = PEDALS_IMPLAUSIBLE_PERCENT;
+        parameters_.pedalsMarginalPercent = PEDALS_MARGINAL_PERCENT;
+        parameters_.pedalsRawTooLow = PEDALS_RAW_TOO_LOW;
+        parameters_.pedalsRawTooHigh = PEDALS_RAW_TOO_HIGH;
+        parameters_.accelPressedThreshold = 0.1;
+        parameters_.brakePressedThreshold = 0.05;
+
     }
 
 // Functions
 
-    bool max_duration_of_implausibility_exceeded(unsigned long t);
+    // bool max_duration_of_implausibility_exceeded(unsigned long t);
     void tick(
         const SysTick_s &sysClock, 
         const AnalogConversion_s &accel1,