Merge branch 'develop' into suhas/sentry-kill-switch

.github/workflows/super-linter.yml

@@ -25,7 +25,7 @@ jobs:
           fetch-depth: 0
 
       - name: Lint
-        uses: super-linter/super-linter/slim@v6.3.0
+        uses: super-linter/super-linter/slim@v7
         env:
           VALIDATE_CLANG_FORMAT: true
           VALIDATE_ALL_CODEBASE: false

.gitignore

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

ut-robomaster/project.xml

@@ -29,6 +29,22 @@
     <option name="taproot:board:analog_in_pins"></option>
     <option name="taproot:board:pwm_pins">C1,C2,C3,C4,C5,C6,C7,Buzzer,ImuHeater</option>
 
+    <option name="taproot:modm-project.xml:modm_hal_modules">
+      modm:platform:i2c:2
+      modm:platform:rtt
+    </option>
+
+    <option name="taproot:modm-project.xml:modm_hal_options">
+      modm:platform:rtt:buffer.rx 0,00,000,0000
+      modm:platform:rtt:buffer.tx 1024,0b10000000000,0o2000,0x400
+      modm:platform:uart:1:buffer.rx 256
+      modm:platform:uart:1:buffer.tx 256
+      modm:platform:uart:3:buffer.tx 256
+      modm:platform:uart:3:buffer.rx 256
+      modm:platform:uart:6:buffer.rx 256
+      modm:platform:uart:6:buffer.tx 256
+    </option>
+
   </options>
   <modules>
     <module>taproot:core</module>

ut-robomaster/src/main.cpp

@@ -42,6 +42,12 @@ static void updateIo(src::Drivers *drivers)
     drivers->remote.read();
 }
 
+static void updateImu(src::Drivers *drivers)
+{
+    drivers->bmi088.read();
+    drivers->bmi088.periodicIMUUpdate();
+}
+
 src::Drivers drivers;
 RobotControl control{&drivers};
 
@@ -61,7 +67,7 @@ int main()
 
         if (refreshTimer.execute())
         {
-            PROFILE(drivers.profiler, drivers.bmi088.periodicIMUUpdate, ());
+            PROFILE(drivers.profiler, updateImu, (&drivers));
             PROFILE(drivers.profiler, drivers.commandScheduler.run, ());
             PROFILE(drivers.profiler, drivers.djiMotorTxHandler.encodeAndSendCanData, ());
             PROFILE(drivers.profiler, drivers.terminalSerial.update, ());

ut-robomaster/src/robots/common/common_control_manual.hpp

@@ -5,13 +5,11 @@
 #include "tap/control/toggle_command_mapping.hpp"
 
 #include "robots/common/common_control.hpp"
-#include "subsystems/chassis/command_move_chassis_joystick.hpp"
-#include "subsystems/chassis/command_move_chassis_keyboard.hpp"
+#include "subsystems/chassis/command_move_chassis.hpp"
 #include "subsystems/flywheel/command_flywheel_off.hpp"
 #include "subsystems/flywheel/command_rotate_flywheel.hpp"
+#include "subsystems/turret/command_move_turret.hpp"
 #include "subsystems/turret/command_move_turret_aimbot.hpp"
-#include "subsystems/turret/command_move_turret_joystick.hpp"
-#include "subsystems/turret/command_move_turret_mouse.hpp"
 
 class CommonControlManual : protected CommonControl
 {
@@ -27,56 +25,31 @@ class CommonControlManual : protected CommonControl
         drivers->commandMapper.addMap(&keyGToggled);
 
         // Controller
-        drivers->commandMapper.addMap(&rightSwitchUp);
-        drivers->commandMapper.addMap(&rightSwitchMid);
-        // drivers->commandMapper.addMap(&rightSwitchDown);
         drivers->commandMapper.addMap(&leftSwitchMid);
 
-        chassis.setDefaultCommand(&move_Keyboard);
-        turret.setDefaultCommand(&look_Mouse);
+        chassis.setDefaultCommand(&move);
+        turret.setDefaultCommand(&look);
     }
 
     // Commands
-    CommandMoveChassisJoystick moveChassisRelative_Joystick{drivers, &chassis, &turret};
-    CommandMoveChassisJoystick moveTurretRelative_Joystick{drivers, &chassis, &turret, true};
-    CommandMoveChassisKeyboard move_Keyboard{drivers, &chassis, &turret};
-    CommandMoveChassisKeyboard moveAndBeyblade_Keyboard{drivers, &chassis, &turret, true};
+    CommandMoveChassis move{drivers, &chassis, &turret, true, false};
+    CommandMoveChassis moveBeyblade{drivers, &chassis, &turret, true, true};
 
     CommandRotateFlywheel rotateFlywheel_Keyboard{drivers, &flywheel};
     CommandRotateFlywheel rotateFlywheel_SwitchUp{drivers, &flywheel};
     CommandRotateFlywheel rotateFlywheel_SwitchMid{drivers, &flywheel};
 
-    CommandMoveTurretJoystick look_Joystick_SwitchMid{drivers, &turret};
-    CommandMoveTurretJoystick look_Joystick_SwitchUp{drivers, &turret};
-    CommandMoveTurretMouse look_Mouse{drivers, &turret};
+    CommandMoveTurret look{drivers, &turret};
 
     // Keyboard mappings
-    ToggleCommandMapping keyRToggled{
-        drivers,
-        {&moveAndBeyblade_Keyboard},
-        RemoteMapState({Remote::Key::R})};
+    ToggleCommandMapping keyRToggled{drivers, {&moveBeyblade}, RemoteMapState({Remote::Key::R})};
 
     ToggleCommandMapping keyGToggled{
         drivers,
         {&rotateFlywheel_Keyboard},
         RemoteMapState({Remote::Key::G})};
 
     // Controller mappings
-    HoldCommandMapping rightSwitchUp{
-        drivers,
-        {&moveTurretRelative_Joystick, &look_Joystick_SwitchUp},
-        RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::UP)};
-
-    HoldCommandMapping rightSwitchMid{
-        drivers,
-        {&moveChassisRelative_Joystick, &look_Joystick_SwitchMid},
-        RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::MID)};
-
-    // HoldCommandMapping rightSwitchDown{
-    //     drivers,
-    //     {&move_Keyboard, &look_Mouse},
-    //     RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::DOWN)};
-
     HoldCommandMapping leftSwitchMid{
         drivers,
         {&rotateFlywheel_SwitchUp},

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

@@ -1,14 +1,12 @@
 #include "agitator_subsystem.hpp"
 
+#include "subsystems/subsystem.hpp"
+
 namespace subsystems::agitator
 {
-
 using tap::algorithms::compareFloatClose;
 using tap::arch::clock::getTimeMilliseconds;
 
-/**
- * AgitatorSubsystem class instantiation
- */
 #if defined(TARGET_STANDARD) || defined(TARGET_SENTRY)
 AgitatorSubsystem::AgitatorSubsystem(
     src::Drivers* drivers,
@@ -48,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);
@@ -70,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,26 +5,21 @@
 
 #include "robots/robot_constants.hpp"
 #include "subsystems/flywheel/flywheel_subsystem.hpp"
+#include "subsystems/subsystem.hpp"
 #include "utils/motors/motor_controller.hpp"
 
 #include "drivers.hpp"
 
-namespace subsystems
-{
-namespace agitator
+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);
-
-    ~AgitatorSubsystem() = default;
-
     void initialize() override;
-
     void refresh() override;
 
     float getShapedVelocity(float time, float a, float phi, float ballsPerSecond);
@@ -33,6 +28,8 @@ class AgitatorSubsystem : public tap::control::Subsystem
     float getPosition();
     float getVelocity();
 
+    bool hardwareOk() override;
+
 private:
     src::Drivers *drivers;
     FlywheelSubsystem *flywheel;
@@ -44,6 +41,4 @@ class AgitatorSubsystem : public tap::control::Subsystem
     MotorController feeder;
 #endif
 };
-
-}  // namespace agitator
-}  // namespace subsystems
+}  // namespace subsystems::agitator

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

@@ -3,15 +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;
 
-namespace subsystems
-{
-namespace chassis
-{
 ChassisSubsystem::ChassisSubsystem(src::Drivers* drivers)
-    : tap::control::Subsystem(drivers),
+    : Subsystem(drivers),
       drivers(drivers),
       powerLimiter(drivers, ENERGY_BUFFER_LIMIT_THRESHOLD, ENERGY_BUFFER_CRIT_THRESHOLD),
       wheels{
@@ -31,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
     }
 
@@ -66,9 +66,17 @@ void ChassisSubsystem::limitChassisPower()
     }
 }
 
-void ChassisSubsystem::runHardwareTests()
+bool ChassisSubsystem::hardwareOk()
 {
-    // TODO
+    for (int8_t i = 0; i < WHEELS; i++)
+    {
+        if (!wheels[i].isOnline())
+        {
+            return false;
+        }
+    }
+
+    return true;
 }
 
 void ChassisSubsystem::input(Vector2f move, float spin)
@@ -124,5 +132,4 @@ Vector3f ChassisSubsystem::measureVelocity()
     // Rotated -90 deg to match our reference frame
     return Vector3f(ya, -xa, wa) * WHEEL_RADIUS / 4.0f * M_TWOPI;
 }
-}  // namespace chassis
-}  // namespace subsystems
+}  // namespace subsystems::chassis

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

@@ -1,36 +1,31 @@
-#ifndef CHASSIS_SUBSYSTEM_HPP_
-#define CHASSIS_SUBSYSTEM_HPP_
+#pragma once
 
 #include "tap/control/subsystem.hpp"
 
 #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"
 
 #include "drivers.hpp"
 
+namespace subsystems::chassis
+{
 using namespace tap::communication::sensors::imu;
 using namespace modm;
 using motors::MotorController;
 
-namespace subsystems
-{
-namespace chassis
-{
-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();
 
-    void runHardwareTests() 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])
@@ -41,8 +36,6 @@ class ChassisSubsystem : public tap::control::Subsystem
     /// @return x,y is linear velocity (m/s) and z is angular velocity (rad/s)
     Vector3f measureVelocity();
 
-    const char* getName() override { return "Chassis subsystem"; }
-
 private:
     src::Drivers* drivers;
     power_limiter::PowerLimiter powerLimiter;
@@ -64,7 +57,4 @@ class ChassisSubsystem : public tap::control::Subsystem
     static constexpr float ENERGY_BUFFER_CRIT_THRESHOLD = 30.0f;
 #endif
 };
-}  // namespace chassis
-}  // namespace subsystems
-
-#endif
+}  // namespace subsystems::chassis