Camera state synchronization and other stuff

src/comms/config_data/controller.hpp

@@ -44,6 +44,7 @@ enum class GenericControllerMotorUse : uint32_t {
     Feeder,
     NearFeeder,
     FarFeeder,
+    UpperFeeder,
 };
 
 
@@ -67,6 +68,7 @@ enum class GenericControllerStateUse : uint32_t {
     ShooterBallVelocity,
     FeederBallPosition,
     LowerFeederBallPosition,
+    UpperFeederBallPosition,
 };
 /// @brief This enum represents the different types of controllers that can be configured in the config.
 enum class ControllerType : uint32_t {
@@ -93,6 +95,11 @@ enum class SubControllerType : uint32_t {
 
     FullStatePositionController,
     FullStateVelocityController,
+
+    UpperFeederPositionController,
+    UpperFeederVelocityController,
+    LowerFeederPositionController,
+    LowerFeederVelocityController,
 };
 /// @brief Gains for a sub controller.
 struct Gains {
@@ -125,6 +132,10 @@ struct GearRatios {
     float ball_to_flywheel_rad = 0.0;
     /// @brief direction of the feeder motor either 1 or -1
     int feeder_direction = 0;
+    /// @brief direction of the upper feeder motor either 1 or -1
+    int upper_feeder_direction = 0;
+    /// @brief direction of the lower feeder motor either 1 or -1
+    int lower_feeder_direction = 0;
 };
 /// @brief Subcontroller configuration for a controller.
 struct SubController {

src/comms/config_data/sensor.hpp

@@ -158,6 +158,14 @@ struct StereoCamTrigger : Comms::CommsData{
     uint32_t digital_trigger_pin_1 = 0;
     /// @brief The digital pin connected to the frame trigger input on the second stereo camera.
     uint32_t digital_trigger_pin_2 = 0;
+    /// @brief The digital pin connected to line1 on the first stereo camera. 
+    uint32_t camera_1_line_1_pin = 0;
+    /// @brief The digital pin connected to line2 on the first stereo camera. 
+    uint32_t camera_1_line_2_pin = 0;
+    /// @brief The digital pin connected to line1 on the second stereo camera.
+    uint32_t camera_2_line_1_pin = 0;
+    /// @brief The digital pin connected to line2 on the second stereo camera.
+    uint32_t camera_2_line_2_pin = 0;
     /// @brief The frames per second that the cameras should be triggered at. This is used to calculate the frequency of the interrupt timers for the camera trigger.
     uint32_t fps = 0;
     /// @brief The width of the trigger pulse, in microseconds. This is used to define the square wave that triggers the camera frame exposures.

src/comms/data/comms_data.hpp

@@ -36,6 +36,8 @@ enum class TypeLabel : uint16_t {
     StereoCameraTriggerConfig,
     StateConfig,
     TransmitterConfig,
+    StartStereoTrigger,
+    StopStereoTrigger,
 };
 
 /// @brief Converts a TypeLabel to a string.
@@ -102,6 +104,10 @@ inline std::string to_string(TypeLabel type_label) {
         return "StateConfig";
     case TypeLabel::TransmitterConfig:
         return "TransmitterConfig";
+    case TypeLabel::StartStereoTrigger:
+        return "StartStereoTrigger";
+    case TypeLabel::StopStereoTrigger:
+        return "StopStereoTrigger";
     // no default case, so the compiler will warn us if we forget a case
     }
 

src/comms/data/data_structs.hpp

@@ -25,4 +25,6 @@
 
 #include "comms_ref_data.hpp"
 
+#include "stereo_cam_trigger_data.hpp"
+
 #endif // DATA_STRUCTS_HPP

src/comms/data/hive_data.cpp

@@ -9,7 +9,7 @@ extern "C" void reset_teensy(void);
 namespace Comms {
 
 void HiveData::set_data(CommsData* data) {
-    Serial.printf("HiveData::set_data received type label %d\n", static_cast<uint8_t>(data->type_label));
+    // Serial.printf("HiveData::set_data received type label %d\n", static_cast<uint8_t>(data->type_label));
     // place the data in the mega struct
     switch (data->type_label) {
     case TypeLabel::TestData: {
@@ -110,6 +110,18 @@ void HiveData::set_data(CommsData* data) {
         Serial.printf("Transmitter %u received\n", static_cast<uint32_t>(transmitter->transmitter_type));
         break;
     }
+    case TypeLabel::StartStereoTrigger: {
+        StartStereoTrigger* start_trigger = static_cast<StartStereoTrigger*>(data);
+        stereo_cam_start_stop.start_received = true;
+        Serial.printf("Start stereo trigger for %u received\n", static_cast<uint32_t>(start_trigger->camera_trigger_name));
+        break;
+    }
+    case TypeLabel::StopStereoTrigger: {
+        StopStereoTrigger* stop_trigger = static_cast<StopStereoTrigger*>(data);
+        stereo_cam_start_stop.stop_received = true;
+        Serial.printf("Stop stereo trigger for %u received\n", static_cast<uint32_t>(stop_trigger->camera_trigger_name));
+        break;
+    }
     default:
         safety::safety_procedure("HiveData::set_data: Invalid type label given to place in mega struct: %u\n", static_cast<uint32_t>(data->type_label));
     }

src/comms/data/hive_data.hpp

@@ -23,6 +23,8 @@ struct HiveData {
     /// @brief Override state received from Hive; This is used to override the robot state estimate on firmware with a new one.
     OverrideState override_state_data;
 
+    /// @brief stores the stereo camera trigger start and stop information
+    StereoCamStartStop stereo_cam_start_stop;
     /// @brief The configuration data filled as config sections are received over comms. This should only be used after all config sections have been received.
     Cfg::RobotConfig config;
 };

src/comms/data/robot_state_data.hpp

@@ -33,5 +33,4 @@ struct OverrideState : Comms::CommsData {
     State::Raw state[static_cast<size_t>(Cfg::StateName::StateNameCount)] = { {0, 0, 0} };
     /// @brief Whether to actively override firmware's estimated state with this incoming state.
     uint64_t active = false;
-};
-
+};

src/comms/data/stereo_cam_trigger_data.hpp

@@ -2,6 +2,7 @@
 
 #include "comms/data/comms_data.hpp"            // for CommsData
 #include "comms/config_data/sensor.hpp"
+#include "controls/state.hpp"
 
 /// @brief Structure to send stereo cam trigger data to comms.
 struct StereoCamTriggerData : Comms::CommsData {
@@ -12,11 +13,35 @@ struct StereoCamTriggerData : Comms::CommsData {
     StereoCamTriggerData(Cfg::SensorName name) : CommsData(Comms::TypeLabel::StereoCamTriggerData, Comms::PhysicalMedium::Ethernet, Comms::Priority::Medium, sizeof(StereoCamTriggerData)), camera_trigger_name(name) { }
     /// @brief The name of the camera trigger that this data corresponds to.
     Cfg::SensorName camera_trigger_name = Cfg::SensorName::UnsetSensorName;
-
-    /// State matching has not been designed yet.
+    /// @brief the state of the robot at the time of stereo trigger
+    State::Raw state[static_cast<size_t>(Cfg::StateName::StateNameCount)] = { {0, 0, 0} };
 
     /// @brief Print the stereo camera trigger data to the serial console for debugging purposes.
     void print() const {
-        printf("StereoCamTriggerData - camera_trigger_name: %lu\n", static_cast<uint32_t>(camera_trigger_name));
+        printf("StereoCamTriggerData - camera_trigger_name: %lu\n", static_cast<unsigned long>(camera_trigger_name));
     }
-};
+};
+
+/// @brief Comms Packet used to reset the stereo trigger counters, should be renamed
+struct StartStereoTrigger : Comms::CommsData {
+    StartStereoTrigger() : CommsData(Comms::TypeLabel::StartStereoTrigger, Comms::PhysicalMedium::Ethernet, Comms::Priority::High, sizeof(StartStereoTrigger)) { }
+    /// @brief the name of the stereo camera trigger
+    Cfg::SensorName camera_trigger_name = Cfg::SensorName::UnsetSensorName;
+};
+
+/// @brief Comms Packet that is currently unused
+struct StopStereoTrigger : Comms::CommsData {
+    StopStereoTrigger() : CommsData(Comms::TypeLabel::StopStereoTrigger, Comms::PhysicalMedium::Ethernet, Comms::Priority::High, sizeof(StopStereoTrigger)) { }
+    /// @brief the name of the stereo camera trigger
+    Cfg::SensorName camera_trigger_name = Cfg::SensorName::UnsetSensorName;
+};
+
+/// @brief used to keep track of the stereo cam state changes received from hive
+struct StereoCamStartStop {
+    /// @brief if the stereo camera is being triggered
+    bool running = false;
+    /// @brief if a start request was received in the last packet
+    bool start_received = false;
+    /// @brief if a stop request was recieved in the last packet
+    bool stop_received = false;
+};

src/controls/controller.cpp

@@ -449,32 +449,52 @@ void LowerFeederController::validate(const RobotStateMap& reference_map, const R
 
 void LowerFeederController::step(RobotStateMap& reference_map, RobotStateMap& estimate_map) {
     float dt = timer.delta();
-    pidp.kp = full_state_position_controller.gains.p;
-    pidp.ki = full_state_position_controller.gains.i;
-    pidp.kd = full_state_position_controller.gains.d;
-    pidp.kf = full_state_position_controller.gains.f;
-
-    pidv.kp = full_state_velocity_controller.gains.p;
-    pidv.ki = full_state_velocity_controller.gains.i;
-    pidv.kd = full_state_velocity_controller.gains.d;
-    pidv.kf = full_state_velocity_controller.gains.f;
+    upper_pidp.kp = upper_position_controller.gains.p;
+    upper_pidp.ki = upper_position_controller.gains.i;
+    upper_pidp.kd = upper_position_controller.gains.d;
+    upper_pidp.kf = upper_position_controller.gains.f;
+
+    upper_pidv.kp = upper_velocity_controller.gains.p;
+    upper_pidv.ki = upper_velocity_controller.gains.i;
+    upper_pidv.kd = upper_velocity_controller.gains.d;
+    upper_pidv.kf = upper_velocity_controller.gains.f;
+
+    lower_pidp.kp = lower_position_controller.gains.p;
+    lower_pidp.ki = lower_position_controller.gains.i;
+    lower_pidp.kd = lower_position_controller.gains.d;
+    lower_pidp.kf = lower_position_controller.gains.f;
+
+    lower_pidv.kp = lower_velocity_controller.gains.p;
+    lower_pidv.ki = lower_velocity_controller.gains.i;
+    lower_pidv.kd = lower_velocity_controller.gains.d;
+    lower_pidv.kf = lower_velocity_controller.gains.f;
 
+    upper_pidp.setpoint = estimate_map[lower_feeder_position_state].get_position();
+    upper_pidp.measurement = estimate_map[upper_feeder_position_state].get_position();
+
+    upper_pidv.setpoint = reference_map[upper_feeder_position_state].get_velocity();
+    upper_pidv.measurement = estimate_map[upper_feeder_position_state].get_velocity();
 
-    pidp.setpoint = reference_map[lower_feeder_position_state].get_position();
-    pidp.measurement = estimate_map[lower_feeder_position_state].get_position();
+    lower_pidp.setpoint = reference_map[upper_feeder_position_state].get_position();
+    lower_pidp.measurement = estimate_map[lower_feeder_position_state].get_position();
 
-    pidv.setpoint = reference_map[lower_feeder_position_state].get_velocity();
-    pidv.measurement = estimate_map[lower_feeder_position_state].get_velocity();
+    lower_pidv.setpoint = reference_map[lower_feeder_position_state].get_velocity();
+    lower_pidv.measurement = estimate_map[lower_feeder_position_state].get_velocity();
 
-    float outputp = pidp.filter(dt, true, true);
-    float outputv = pidv.filter(dt, true, false);
-    float output = (outputp + outputv) * controller_config.gear_ratios.feeder_direction;
+    float upper_outputp = upper_pidp.filter(dt, true, true);
+    float upper_outputv = upper_pidv.filter(dt, true, false);
+    float upper_output = (upper_outputp + upper_outputv) * controller_config.gear_ratios.upper_feeder_direction;
+
+    float lower_outputp = lower_pidp.filter(dt, true, true);
+    float lower_outputv = lower_pidv.filter(dt, true, false);
+    float lower_output = (lower_outputp + lower_outputv) * controller_config.gear_ratios.lower_feeder_direction;
 
-    // Serial.printf("Feeder Velocity Setpoint: %f, Measurement: %f, output: %f\n", pidv.setpoint, pidv.measurement, output);
+    // Serial.printf("Feeder Velocity Setpoint: %f, Measurement: %f, output: %f\n", lower_pidv.setpoint, lower_pidv.measurement, output);
     // Serial.printf("lower feeder reference position: %f, reference velocity: %f, estimate position: %f, estimate velocity: %f\n", 
     //                 reference_map[lower_feeder_position_state].get_position(), reference_map[lower_feeder_position_state].get_velocity(),
     //                 estimate_map[lower_feeder_position_state].get_position(), estimate_map[lower_feeder_position_state].get_velocity());
-
-    near_feeder_motor->write_motor_torque(output);
-    far_feeder_motor->write_motor_torque(-output);
+    upper_feeder_motor->write_motor_torque(upper_output);    
+
+    near_feeder_motor->write_motor_torque(lower_output);
+    far_feeder_motor->write_motor_torque(-lower_output);
 }

src/controls/controller.hpp

@@ -452,20 +452,30 @@ struct FeederController : public Controller {
 /// @brief Controller for the lower ball feeder on bottom fed
 struct LowerFeederController : public Controller {
     private:
-        /// @brief control position of the feeder
-        Cfg::SubController full_state_position_controller;
-        /// @brief control velocity of the feeder
-        Cfg::SubController full_state_velocity_controller;
+        /// @brief control position of the upper feeder
+        Cfg::SubController upper_position_controller;
+        /// @brief control velocity of the upper feeder
+        Cfg::SubController upper_velocity_controller;
+        /// @brief control position of the lower feeder
+        Cfg::SubController lower_position_controller;
+        /// @brief control velocity of the lower feeder
+        Cfg::SubController lower_velocity_controller;
         /// @brief filter for calculating pid position controller outputs
-        PIDFilter pidp;
+        PIDFilter upper_pidp;
         /// @brief filter for calculating pid velocity controller outputs
-        PIDFilter pidv;
+        PIDFilter upper_pidv;
+        /// @brief filter for calculating pid position controller outputs
+        PIDFilter lower_pidp;
+        /// @brief filter for calculating pid velocity controller outputs
+        PIDFilter lower_pidv;
         /// @brief motor attached to the feeder
         std::shared_ptr<Motor> near_feeder_motor;
         /// @brief motor attached to the feeder
         std::shared_ptr<Motor> far_feeder_motor;
-        /// @brief state name for the feeder position
-        const Cfg::StateName& feeder_position_state;
+        /// @brief motor attached to the upper feeder
+        std::shared_ptr<Motor> upper_feeder_motor;
+        /// @brief state name for the upper feeder position
+        const Cfg::StateName& upper_feeder_position_state;
         /// @brief state name for the lower feeder position
         const Cfg::StateName& lower_feeder_position_state;
 
@@ -477,13 +487,16 @@ struct LowerFeederController : public Controller {
         /// @param can reference to the CAN manager to get motor objects so we can write directly to motors
         /// @param available_motors list of motor names that are available to be used; this is to prevent multiple controllers from trying to control the same motor
         LowerFeederController(const Cfg::Controller& controller_config, CANManager& can, std::vector<Cfg::MotorName>& available_motors) : Controller(controller_config),
-            feeder_position_state(controller_config.get_state_name_by_generic_use(Cfg::GenericControllerStateUse::FeederBallPosition)),
+            upper_feeder_position_state(controller_config.get_state_name_by_generic_use(Cfg::GenericControllerStateUse::UpperFeederBallPosition)),
             lower_feeder_position_state(controller_config.get_state_name_by_generic_use(Cfg::GenericControllerStateUse::LowerFeederBallPosition)) {
-            full_state_position_controller = controller_config.get_sub_controller_by_type(Cfg::SubControllerType::FullStatePositionController);
-            full_state_velocity_controller = controller_config.get_sub_controller_by_type(Cfg::SubControllerType::FullStateVelocityController);
-
+            lower_position_controller = controller_config.get_sub_controller_by_type(Cfg::SubControllerType::LowerFeederPositionController);
+            lower_velocity_controller = controller_config.get_sub_controller_by_type(Cfg::SubControllerType::LowerFeederVelocityController);
+            upper_position_controller = controller_config.get_sub_controller_by_type(Cfg::SubControllerType::UpperFeederPositionController);
+            upper_velocity_controller = controller_config.get_sub_controller_by_type(Cfg::SubControllerType::UpperFeederVelocityController);
+
             near_feeder_motor = get_motor_by_generic_use(Cfg::GenericControllerMotorUse::NearFeeder, can, available_motors);
             far_feeder_motor = get_motor_by_generic_use(Cfg::GenericControllerMotorUse::FarFeeder, can, available_motors);
+            upper_feeder_motor = get_motor_by_generic_use(Cfg::GenericControllerMotorUse::UpperFeeder, can, available_motors);
         }
 
         /// @brief sends motor commands based on a reference and estimated state
@@ -497,8 +510,10 @@ struct LowerFeederController : public Controller {
         /// @brief reset the controller
         inline void reset() {
             Controller::reset();
-            pidp.sumError = 0.0;
-            pidv.sumError = 0.0;
+            upper_pidp.sumError = 0.0;
+            upper_pidv.sumError = 0.0;
+            lower_pidp.sumError = 0.0;
+            lower_pidv.sumError = 0.0;
             lower_feeder_error_monitor = ErrorMonitor{};
         }
 };

src/controls/robot_state_map.hpp

@@ -47,6 +47,15 @@ class RobotStateMap {
         sendable.data.time = millis();
         sendable.send_to_comms();
     }
+
+    /// @brief copies the input state map into this state map
+    /// @param states the robot state map that will be copied from
+    void fill_state_array(State::Raw states[static_cast<size_t>(Cfg::StateName::StateNameCount)]) const {
+        for (const auto& [state_name, state] : robot_state) {
+            states[static_cast<size_t>(state_name)] = state.get_raw();
+        }
+    }
+
     /// @brief Print the state map to the serial monitor
     void print();
     /// @brief Update the state map from a comms packet. This will convert the comms packet to the state map format and then update the state map values
@@ -56,4 +65,4 @@ class RobotStateMap {
 private:
     /// @brief Map of state names to their corresponding state objects
     std::map<Cfg::StateName, State> robot_state;
-};
+};

src/main.cpp

@@ -6,6 +6,7 @@
 #include "controls/reference_governor.hpp"
 #include "git_info.h"
 
+#include "robot_state_map.hpp"
 #include "safety.hpp"
 #include "sensors/buff_encoder.hpp"
 #include "state.hpp"
@@ -24,6 +25,7 @@
 
 #include "sensor_manager.hpp"
 #include <TeensyDebug.h>
+#include <wiring.h>
 
 #include "comms/data/hive_data.hpp"
 #include "comms/data/sendable.hpp"
@@ -49,6 +51,8 @@ Comms::CommsLayer comms_layer;
 Profiler prof;
 #endif
 
+std::unique_ptr<RobotStateMap> estimated_state_map_interrupt_safe;
+
 SensorManager sensor_manager;
 EstimatorManager estimator_manager;
 ControllerManager controller_manager;
@@ -147,6 +151,7 @@ int main() {
 
     // variables for use in main
     RobotStateMap estimated_state_map(config.states);
+    estimated_state_map_interrupt_safe = std::make_unique<RobotStateMap>(config.states);
     RobotStateMap reference_map(config.states);                   
     RobotStateMap target_state_map(config.states);// Temp ungoverned state
     RobotStateMap hive_state_map_offset(config.states);// Hive offset state
@@ -236,6 +241,10 @@ int main() {
         estimator_manager.step(estimated_state_map, override_request);
         // estimated_state_map.print();
 
+        noInterrupts();
+            *estimated_state_map_interrupt_safe = estimated_state_map;
+        interrupts();
+
         override_request = false;
 
         if ((feed - estimated_state_map[Cfg::StateName::Feeder].get_position() > 2 && transmitter_manager.is_teensy_mode()) ||