implement hardwareok in chassis

tildezero · tildezero · commit ab12087eb52c · 2024-10-25T22:34:55.000Z
diff --git a/.DS_Store b/.DS_Store
diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp
@@ -31,9 +31,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
     }
 
@@ -71,6 +72,19 @@ void ChassisSubsystem::runHardwareTests()
     // TODO
 }
 
+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;
diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp
@@ -5,6 +5,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"
 
@@ -18,7 +19,8 @@ namespace subsystems
 {
 namespace chassis
 {
-class ChassisSubsystem : public tap::control::Subsystem
+// class ChassisSubsystem : public tap::control::Subsystem
+class ChassisSubsystem : public UTSubsystem
 {
 public:
     ChassisSubsystem(src::Drivers* drivers);
@@ -31,6 +33,8 @@ class ChassisSubsystem : public tap::control::Subsystem
 
     void runHardwareTests() override;
 
+    bool hardwareOk() override;
+
     /// @brief Update robot motion based on simple input controls. Inputs are scaled and corrected
     /// to avoid over-driving motors. This logic can be adjusted to create various input schemes.
     /// @param move Linear movement (magnitude should be within [0,1])

Original file line number	Diff line number	Diff line change
`@@ -31,9 +31,10 @@ void ChassisSubsystem::initialize()`
`31`	`31`
`32`	`32`	`void ChassisSubsystem::refresh()`
`33`	`33`	`{`
	`34`	`+ setAmputated(hardwareOk());`
`34`	`35`	`for (int8_t i = 0; i < WHEELS; i++)`
`35`	`36`	`{`
`36`		`- wheels[i].setActive(!drivers->isKillSwitched());`
	`37`	`+ wheels[i].setActive(!drivers->isKillSwitched() && !isAmputated());`
`37`	`38`	`wheels[i].updateVelocity(targetWheelVels[i] / M_TWOPI); // rad/s to rev/s`
`38`	`39`	`}`
`39`	`40`
`@@ -71,6 +72,19 @@ void ChassisSubsystem::runHardwareTests()`
`71`	`72`	`// TODO`
`72`	`73`	`}`
`73`	`74`
	`75`	`+bool ChassisSubsystem::hardwareOk()`
	`76`	`+{`
	`77`	`+ for (int8_t i = 0; i < WHEELS; i++)`
	`78`	`+ {`
	`79`	`+ if (!wheels[i].isOnline())`
	`80`	`+ {`
	`81`	`+ return false;`
	`82`	`+ }`
	`83`	`+ }`
	`84`	`+`
	`85`	`+ return true;`
	`86`	`+}`
	`87`	`+`
`74`	`88`	`void ChassisSubsystem::input(Vector2f move, float spin)`
`75`	`89`	`{`
`76`	`90`	`Vector2f v = move * MAX_LINEAR_VEL;`