disable subsystems when hardware disconnects

.gitignore

@@ -15,4 +15,6 @@ gcov-result
 
 docs/_build
 docs/api
-docs/doxyoutput
+docs/doxyoutput
+
+.DS_Store

ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp

@@ -1,5 +1,7 @@
 #include "agitator_subsystem.hpp"
 
+#include "subsystems/subsystem.hpp"
+
 namespace subsystems::agitator
 {
 using tap::algorithms::compareFloatClose;
@@ -44,9 +46,11 @@ void AgitatorSubsystem::refresh()
     return;
 #endif
 
+    setAmputated(!hardwareOk());
+
     float time = getTimeMilliseconds() / 1000.0f;
     float velocity = getShapedVelocity(time, 1.0f, 0.0f, ballsPerSecond);
-    bool killSwitch = drivers->isKillSwitched() || !flywheel->isActive();
+    bool killSwitch = drivers->isKillSwitched() || isAmputated() || !flywheel->isActive();
 
     agitator.setActive(!killSwitch);
     agitator.updateVelocity(velocity);
@@ -66,4 +70,14 @@ float AgitatorSubsystem::getShapedVelocity(float time, float a, float phi, float
 void AgitatorSubsystem::setBallsPerSecond(float bps) { ballsPerSecond = bps; }
 float AgitatorSubsystem::getPosition() { return agitator.measurePosition(); }
 float AgitatorSubsystem::getVelocity() { return agitator.measureVelocity(); }
+
+bool AgitatorSubsystem::hardwareOk()
+{
+#ifdef TARGET_HERO
+    return agitator.isOnline() && feeder.isOnline();
+#else
+    return agitator.isOnline();
+#endif
+}
+
 }  // namespace subsystems::agitator

ut-robomaster/src/subsystems/agitator/agitator_subsystem.hpp

@@ -5,6 +5,7 @@
 
 #include "robots/robot_constants.hpp"
 #include "subsystems/flywheel/flywheel_subsystem.hpp"
+#include "subsystems/subsystem.hpp"
 #include "utils/motors/motor_controller.hpp"
 
 #include "drivers.hpp"
@@ -14,7 +15,7 @@ namespace subsystems::agitator
 using flywheel::FlywheelSubsystem;
 using motors::MotorController;
 
-class AgitatorSubsystem : public tap::control::Subsystem
+class AgitatorSubsystem : public Subsystem
 {
 public:
     AgitatorSubsystem(src::Drivers *drivers, FlywheelSubsystem *flywheel, MotorConfig motor);
@@ -27,6 +28,8 @@ class AgitatorSubsystem : public tap::control::Subsystem
     float getPosition();
     float getVelocity();
 
+    bool hardwareOk() override;
+
 private:
     src::Drivers *drivers;
     FlywheelSubsystem *flywheel;

ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp

@@ -3,13 +3,14 @@
 #include "tap/algorithms/math_user_utils.hpp"
 
 #include "robots/robot_constants.hpp"
+#include "subsystems/subsystem.hpp"
 
 namespace subsystems::chassis
 {
 using namespace tap::algorithms;
 
 ChassisSubsystem::ChassisSubsystem(src::Drivers* drivers)
-    : tap::control::Subsystem(drivers),
+    : Subsystem(drivers),
       drivers(drivers),
       powerLimiter(drivers, ENERGY_BUFFER_LIMIT_THRESHOLD, ENERGY_BUFFER_CRIT_THRESHOLD),
       wheels{
@@ -29,9 +30,10 @@ void ChassisSubsystem::initialize()
 
 void ChassisSubsystem::refresh()
 {
+    setAmputated(!hardwareOk());
     for (int8_t i = 0; i < WHEELS; i++)
     {
-        wheels[i].setActive(!drivers->isKillSwitched());
+        wheels[i].setActive(!drivers->isKillSwitched() && !isAmputated());
         wheels[i].updateVelocity(targetWheelVels[i] / M_TWOPI);  // rad/s to rev/s
     }
 
@@ -64,6 +66,19 @@ void ChassisSubsystem::limitChassisPower()
     }
 }
 
+bool ChassisSubsystem::hardwareOk()
+{
+    for (int8_t i = 0; i < WHEELS; i++)
+    {
+        if (!wheels[i].isOnline())
+        {
+            return false;
+        }
+    }
+
+    return true;
+}
+
 void ChassisSubsystem::input(Vector2f move, float spin)
 {
     Vector2f v = move * MAX_LINEAR_VEL;

ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp

@@ -4,6 +4,7 @@
 
 #include "modm/math/geometry.hpp"
 #include "robots/robot_constants.hpp"
+#include "subsystems/subsystem.hpp"
 #include "utils/motors/motor_controller.hpp"
 #include "utils/power_limiter/power_limiter.hpp"
 
@@ -15,12 +16,13 @@ using namespace tap::communication::sensors::imu;
 using namespace modm;
 using motors::MotorController;
 
-class ChassisSubsystem : public tap::control::Subsystem
+class ChassisSubsystem : public Subsystem
 {
 public:
     ChassisSubsystem(src::Drivers* drivers);
     void initialize() override;
     void refresh() override;
+    bool hardwareOk() override;
 
     void limitChassisPower();
 

ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp

@@ -35,17 +35,27 @@ void FlywheelSubsystem::refresh()
 #ifdef DEMO_MODE
     return;
 #endif
+    setAmputated(!hardwareOk());
 
     bool killSwitch = drivers->isKillSwitched();
 
     for (int i = 0; i < FLYWHEELS; i++)
     {
-        motors[i].setActive(!killSwitch);
+        motors[i].setActive(!killSwitch && !isAmputated());
         motors[i].updateVelocity(velocity);
     }
 }
 
 void FlywheelSubsystem::setVelocity(float newVelocity) { velocity = newVelocity; }
 
-bool FlywheelSubsystem::isActive() { return velocity != 0.0f; }
+bool FlywheelSubsystem::isActive() { return !isAmputated() && velocity != 0.0f; }
+
+bool FlywheelSubsystem::hardwareOk()
+{
+    for (int i = 0; i < FLYWHEELS; i++)
+    {
+        if (!motors[i].isOnline()) return false;
+    }
+    return true;
+}
 }  // namespace subsystems::flywheel

ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.hpp

@@ -3,6 +3,7 @@
 #include "tap/control/subsystem.hpp"
 
 #include "robots/robot_constants.hpp"
+#include "subsystems/subsystem.hpp"
 #include "utils/motors/motor_controller.hpp"
 
 #include "drivers.hpp"
@@ -11,7 +12,7 @@ namespace subsystems::flywheel
 {
 using motors::MotorController;
 
-class FlywheelSubsystem : public tap::control::Subsystem
+class FlywheelSubsystem : public Subsystem
 {
 public:
     FlywheelSubsystem(src::Drivers* drivers);
@@ -24,6 +25,8 @@ class FlywheelSubsystem : public tap::control::Subsystem
 
     bool isActive();
 
+    bool hardwareOk() override;
+
 private:
     src::Drivers* drivers;
     MotorController motors[FLYWHEELS];

ut-robomaster/src/subsystems/odometry/odometry_subsystem.cpp

@@ -1,14 +1,15 @@
 #include "odometry_subsystem.hpp"
 
 #include "robots/robot_constants.hpp"
+#include "subsystems/subsystem.hpp"
 
 namespace subsystems::odometry
 {
 OdometrySubsystem::OdometrySubsystem(
     src::Drivers* drivers,
     ChassisSubsystem* chassis,
     TurretSubsystem* turret)
-    : tap::control::Subsystem(drivers),
+    : Subsystem(drivers),
       drivers(drivers),
       chassis(chassis),
       turret(turret),
@@ -17,7 +18,17 @@ OdometrySubsystem::OdometrySubsystem(
       chassisTracker(&chassisYaw, &chassisDisplacement) {};
 
 void OdometrySubsystem::initialize() {};
-void OdometrySubsystem::refresh() { chassisTracker.update(); }
+
+void OdometrySubsystem::refresh()
+{
+    setAmputated(!hardwareOk());
+    if (!isAmputated())
+    {
+        chassisTracker.update();
+    }
+}
+
+bool OdometrySubsystem::hardwareOk() { return chassis->hardwareOk() && turret->hardwareOk(); }
 
 Vector2f OdometrySubsystem::getPosition()
 {

ut-robomaster/src/subsystems/odometry/odometry_subsystem.hpp

@@ -10,6 +10,7 @@
 #include "subsystems/chassis/chassis_subsystem.hpp"
 #include "subsystems/odometry/observer_displacement.hpp"
 #include "subsystems/odometry/observer_yaw_world.hpp"
+#include "subsystems/subsystem.hpp"
 #include "subsystems/turret/turret_subsystem.hpp"
 #include "utils/robot_comms.hpp"
 
@@ -22,12 +23,13 @@ using modm::Vector2f;
 using tap::algorithms::odometry::Odometry2DTracker;
 using turret::TurretSubsystem;
 
-class OdometrySubsystem : public tap::control::Subsystem
+class OdometrySubsystem : public Subsystem
 {
 public:
     OdometrySubsystem(src::Drivers* drivers, ChassisSubsystem* chassis, TurretSubsystem* turret);
     void initialize() override;
     void refresh() override;
+    bool hardwareOk() override;
 
     Vector2f getPosition();
     Vector2f getLinearVelocity();

ut-robomaster/src/subsystems/subsystem.cpp

@@ -0,0 +1,11 @@
+#include "subsystem.hpp"
+
+namespace subsystems
+{
+Subsystem::Subsystem(src::Drivers* drivers) : tap::control::Subsystem(drivers) {}
+bool Subsystem::hardwareOk() { return true; }
+
+bool Subsystem::isAmputated() { return amputated; }
+
+void Subsystem::setAmputated(bool newAmputatedValue) { amputated = newAmputatedValue; }
+}  // namespace subsystems

ut-robomaster/src/subsystems/subsystem.hpp

@@ -0,0 +1,25 @@
+#pragma once
+#include "tap/control/subsystem.hpp"
+
+#include "drivers.hpp"
+
+namespace subsystems
+{
+class Subsystem : public tap::control::Subsystem
+{
+public:
+    Subsystem(src::Drivers *drivers);
+
+    ~Subsystem() = default;
+
+    virtual bool hardwareOk();
+    bool isAmputated();
+    void setAmputated(bool newAmputatedValue);
+
+protected:
+    src::Drivers *drivers;
+
+private:
+    bool amputated = false;
+};
+}  // namespace subsystems

ut-robomaster/src/subsystems/turret/turret_subsystem.cpp

@@ -2,11 +2,12 @@
 
 #include "modm/math.hpp"
 #include "robots/robot_constants.hpp"
+#include "subsystems/subsystem.hpp"
 
 namespace subsystems::turret
 {
 TurretSubsystem::TurretSubsystem(src::Drivers* drivers)
-    : tap::control::Subsystem(drivers),
+    : Subsystem(drivers),
       drivers(drivers),
 #if defined(TARGET_STANDARD) || defined(TARGET_HERO)
       yawEncoder(),
@@ -19,6 +20,8 @@ TurretSubsystem::TurretSubsystem(src::Drivers* drivers)
 {
 }
 
+bool TurretSubsystem::hardwareOk() { return yaw.isOnline() && pitch.isOnline(); }
+
 void TurretSubsystem::initialize()
 {
     yaw.initialize();
@@ -31,17 +34,19 @@ void TurretSubsystem::refresh()
     yawEncoder.update();
 #endif
 
+    setAmputated(!hardwareOk());
+
     yaw.updateMotorAngle();
     pitch.updateMotorAngle();
 
-    if (!isCalibrated && yaw.isOnline() && pitch.isOnline())
+    if (!isCalibrated && !isAmputated())
     {
         baseYaw = yaw.getAngle() / YAW_REDUCTION;
         basePitch = pitch.getAngle() / PITCH_REDUCTION - PITCH_MIN;
         isCalibrated = true;
     }
 
-    if (isCalibrated && !drivers->isKillSwitched())
+    if (isCalibrated && !drivers->isKillSwitched() && !isAmputated())
     {
         yaw.setAngle((baseYaw + getTargetLocalYaw()) * YAW_REDUCTION, DT);
         pitch.setAngle((basePitch + getTargetLocalPitch()) * PITCH_REDUCTION, DT);

ut-robomaster/src/subsystems/turret/turret_subsystem.hpp

@@ -6,6 +6,7 @@
 #include "drivers/as5600.hpp"
 #include "modm/math/geometry.hpp"
 #include "robots/robot_constants.hpp"
+#include "subsystems/subsystem.hpp"
 
 #include "double_yaw_motor.hpp"
 #include "drivers.hpp"
@@ -17,12 +18,13 @@ using driver::As5600;
 using modm::Vector3f;
 using tap::algorithms::ContiguousFloat;
 
-class TurretSubsystem : public tap::control::Subsystem
+class TurretSubsystem : public Subsystem
 {
 public:
     TurretSubsystem(src::Drivers* drivers);
     void initialize() override;
     void refresh() override;
+    bool hardwareOk() override;
 
     /// @brief Input target data from CV (relative to camera)
     void inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration);