Add new features to HUD

ut-robomaster/src/robots/standard/standard_control.hpp

@@ -1,5 +1,12 @@
 #include "robots/common/common_control_manual.hpp"
 #include "subsystems/agitator/command_agitator_continuous.hpp"
+#include "subsystems/hud/client_display_subsystem.hpp"
+#include "subsystems/hud/command_client_display.hpp"
+#include "subsystems/hud/command_client_display_reset.hpp"
+
+using commands::CommandClientDisplay;
+using commands::CommandClientDisplayReset;
+using subsystems::hud::ClientDisplaySubsystem;
 
 class StandardControl : CommonControlManual
 {
@@ -12,14 +19,18 @@ class StandardControl : CommonControlManual
 
         agitator.initialize();
         drivers->commandScheduler.registerSubsystem(&agitator);
+        drivers->commandScheduler.registerSubsystem(&client);
+        client.setDefaultCommand(&hudDisplay);
 
         drivers->commandMapper.addMap(&leftMouseDown);
         drivers->commandMapper.addMap(&leftSwitchUp);
+        drivers->commandMapper.addMap(&hudResetKey);
     }
 
 private:
     // Subsystems
     AgitatorSubsystem agitator{drivers, &flywheel, AGITATOR};
+    ClientDisplaySubsystem client{drivers, &turret, &flywheel};
 
     // Commands
     CommandAgitatorContinuous rotateAgitator_LeftMouse{drivers, &agitator, BarrelId::STANDARD1};
@@ -29,6 +40,9 @@ class StandardControl : CommonControlManual
         BarrelId::STANDARD1,
         true};
 
+    commands::CommandClientDisplay hudDisplay{drivers, &client};
+    commands::CommandClientDisplayReset hudReset{drivers, &client};
+
     // Mappings
     HoldCommandMapping leftMouseDown{
         drivers,
@@ -39,4 +53,6 @@ class StandardControl : CommonControlManual
         drivers,
         {&rotateAgitator_SwitchUp, &rotateFlywheel_SwitchMid},
         RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP)};
+
+    ToggleCommandMapping hudResetKey{drivers, {&hudReset}, RemoteMapState({Remote::Key::Z})};
 };

ut-robomaster/src/subsystems/hud/client_display_subsystem.cpp

@@ -0,0 +1,29 @@
+#include "client_display_subsystem.hpp"
+
+#include "tap/communication/sensors/buzzer/buzzer.hpp"
+
+namespace subsystems::hud
+{
+ClientDisplaySubsystem::ClientDisplaySubsystem(
+    src::Drivers* drivers,
+    TurretSubsystem* turret,
+    FlywheelSubsystem* flywheel)
+    : Subsystem(drivers),
+      drivers(drivers),
+      turret(turret),
+      flywheel(flywheel),
+      circle(drivers),
+      reticle(drivers, ClientDisplaySubsystem::turret),
+      flywheel_on(drivers, ClientDisplaySubsystem::flywheel),
+      orientation(drivers, ClientDisplaySubsystem::turret)
+
+{
+}
+
+void ClientDisplaySubsystem::initialize() { *numGraphics = 4; }
+
+void ClientDisplaySubsystem::refresh() {}
+
+void ClientDisplaySubsystem::runHardwareTests() {}
+
+}  // namespace subsystems::hud

ut-robomaster/src/subsystems/hud/client_display_subsystem.hpp

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <vector>
+
+#include "tap/communication/sensors/buzzer/buzzer.hpp"
+#include "tap/control/command.hpp"
+#include "tap/control/subsystem.hpp"
+
+#include "graphics/graphic_abstract.hpp"
+#include "graphics/graphic_circle.hpp"
+#include "graphics/graphic_flywheel_on.hpp"
+#include "graphics/graphic_orientation.hpp"
+#include "graphics/graphic_reticle.hpp"
+#include "subsystems/flywheel/flywheel_subsystem.hpp"
+#include "subsystems/turret/turret_subsystem.hpp"
+
+#include "drivers.hpp"
+
+using subsystems::flywheel::FlywheelSubsystem;
+using subsystems::turret::TurretSubsystem;
+
+namespace subsystems::hud
+{
+class ClientDisplaySubsystem : public tap::control::Subsystem
+{
+public:
+    ClientDisplaySubsystem(src::Drivers*, TurretSubsystem* turret, FlywheelSubsystem* flywheel);
+
+    // add graphics desired here
+    void initialize() override;
+
+    void refresh() override;
+
+    graphic::graphic_abstract* getCircle() { return &circle; };
+    graphic::graphic_abstract* getReticle() { return &reticle; };
+    graphic::graphic_abstract* getFlywheelOn() { return &flywheel_on; };
+    graphic::graphic_abstract* getOrientation() { return &orientation; };
+
+    void runHardwareTests() override;
+
+    const char* getName() override { return "Client Display subsystem"; }
+
+private:
+    uint8_t* numGraphics;
+    src::Drivers* drivers;
+    TurretSubsystem* turret;
+    FlywheelSubsystem* flywheel;
+
+    graphic::graphic_circle circle{drivers};
+    graphic::graphic_reticle reticle{drivers, turret};
+    graphic::graphic_flywheel_on flywheel_on{drivers, flywheel};
+    graphic::graphic_orientation orientation{drivers, turret};
+};
+}  // namespace subsystems::hud

ut-robomaster/src/subsystems/hud/command_client_display.cpp

@@ -0,0 +1,49 @@
+#include "command_client_display.hpp"
+
+namespace commands
+{
+
+void CommandClientDisplay::initialize()
+{
+    // tap::buzzer::playNote(&drivers->pwm, 440);
+
+    circle = client->getCircle();
+    reticle = client->getReticle();
+    flywheel_on = client->getFlywheelOn();
+    orientation = client->getOrientation();
+    restart();
+}
+
+void CommandClientDisplay::execute() { run(); }
+
+void CommandClientDisplay::end(bool) { /*tap::buzzer::silenceBuzzer(&drivers->pwm);*/ }
+
+bool CommandClientDisplay::isFinished() const { return false; }
+
+bool CommandClientDisplay::run()
+{
+    PT_BEGIN();
+
+    // Initialize
+    PT_WAIT_UNTIL(drivers->refSerial.getRefSerialReceivingData());
+    // PT_CALL(reticle->initialize());
+    PT_CALL(circle->initialize());
+    PT_CALL(flywheel_on->initialize());
+    PT_CALL(orientation->initialize());
+
+    // Update
+    while (true)
+    {
+        // PT_CALL(reticle->run());
+        // PT_WAIT_UNTIL(hudTimer.execute());
+        PT_CALL(circle->run());
+        PT_WAIT_UNTIL(hudTimer.execute());
+        PT_CALL(flywheel_on->run());
+        PT_WAIT_UNTIL(hudTimer.execute());
+        PT_CALL(orientation->run());
+        PT_WAIT_UNTIL(hudTimer.execute());
+    }
+
+    PT_END();
+}
+}  // namespace commands

ut-robomaster/src/subsystems/hud/command_client_display.hpp

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <vector>
+
+#include "tap/communication/sensors/buzzer/buzzer.hpp"
+#include "tap/communication/serial/ref_serial_data.hpp"
+#include "tap/communication/serial/ref_serial_transmitter.hpp"
+#include "tap/control/command.hpp"
+
+#include "graphics/graphic_abstract.hpp"
+#include "modm/processing/protothread.hpp"
+#include "modm/processing/resumable.hpp"
+#include "subsystems/flywheel/flywheel_subsystem.hpp"
+
+#include "client_display_subsystem.hpp"
+#include "drivers.hpp"
+
+using namespace tap::control;
+using namespace tap::communication::serial;
+
+using subsystems::flywheel::FlywheelSubsystem;
+using subsystems::hud::ClientDisplaySubsystem;
+
+namespace commands
+{
+class CommandClientDisplay : public Command, modm::pt::Protothread
+{
+public:
+    CommandClientDisplay(src::Drivers* drivers, ClientDisplaySubsystem* client)
+        : Command(),
+          drivers(drivers),
+          client(client)
+    {
+        addSubsystemRequirement(client);
+    }
+
+    bool run();
+
+    void initialize() override;
+    void execute() override;
+    void end(bool) override;
+    bool isFinished() const override;
+    const char* getName() const override { return "client display"; }
+
+private:
+    src::Drivers* drivers;
+    ClientDisplaySubsystem* client;
+    graphic::graphic_abstract* circle;
+    graphic::graphic_abstract* reticle;
+    graphic::graphic_abstract* flywheel_on;
+    graphic::graphic_abstract* orientation;
+    tap::arch::PeriodicMilliTimer hudTimer{50};
+};
+}  // namespace commands

ut-robomaster/src/subsystems/hud/command_client_display_reset.cpp

@@ -0,0 +1,14 @@
+#include "command_client_display_reset.hpp"
+
+#include "tap/communication/sensors/buzzer/buzzer.hpp"
+
+namespace commands
+{
+void CommandClientDisplayReset::initialize() { tap::buzzer::playNote(&drivers->pwm, 440); }
+
+void CommandClientDisplayReset::execute() {}
+
+void CommandClientDisplayReset::end(bool) { tap::buzzer::silenceBuzzer(&drivers->pwm); }
+
+bool CommandClientDisplayReset::isFinished() const { return false; }
+}  // namespace commands

ut-robomaster/src/subsystems/hud/command_client_display_reset.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "tap/control/command.hpp"
+
+#include "client_display_subsystem.hpp"
+#include "drivers.hpp"
+
+using namespace tap::control;
+using namespace tap::communication::serial;
+
+using subsystems::hud::ClientDisplaySubsystem;
+
+namespace commands
+{
+class CommandClientDisplayReset : public Command
+{
+public:
+    CommandClientDisplayReset(src::Drivers* drivers, ClientDisplaySubsystem* client)
+        : Command(),
+          drivers(drivers),
+          client(client)
+    {
+        addSubsystemRequirement(client);
+    }
+
+    void initialize() override;
+    void execute() override;
+    void end(bool) override;
+    bool isFinished() const override;
+    const char* getName() const override { return "client display reset"; }
+
+private:
+    src::Drivers* drivers;
+    ClientDisplaySubsystem* client;
+};
+}  // namespace commands

ut-robomaster/src/subsystems/hud/graphics/graphic_abstract.hpp

@@ -0,0 +1,34 @@
+#pragma once
+#include "tap/communication/serial/ref_serial_data.hpp"
+#include "tap/communication/serial/ref_serial_transmitter.hpp"
+#include "tap/control/command.hpp"
+
+#include "modm/processing/protothread.hpp"
+#include "modm/processing/resumable.hpp"
+
+#include "drivers.hpp"
+
+using namespace tap::communication::serial;
+using modm::NestedResumable;
+using modm::ResumableResult;
+
+namespace graphic
+{
+
+class graphic_abstract : protected NestedResumable<3>
+{
+public:
+    graphic_abstract(src::Drivers* drivers) : drivers(drivers), refSerialTransmitter(drivers) {};
+    virtual ResumableResult<void> initialize() = 0;
+    virtual ResumableResult<bool> run() = 0;
+
+    // virtual ~graphic_abstract();
+
+protected:
+    // bottom left is (0, 0) top right is (1920, 1080)
+    src::Drivers* drivers;
+    RefSerialTransmitter refSerialTransmitter;
+    uint8_t graphicId[3];  // 3 byte identifier for this graphic element
+};
+
+}  // namespace graphic

ut-robomaster/src/subsystems/hud/graphics/graphic_circle.hpp

@@ -0,0 +1,51 @@
+#pragma once
+#include "graphic_abstract.hpp"
+
+namespace graphic
+{
+
+class graphic_circle : public graphic_abstract
+{
+public:
+    graphic_circle(src::Drivers *drivers) : graphic_abstract(drivers)
+    {
+        graphicId[0] = 0;
+
+        RefSerialTransmitter::configGraphicGenerics(
+            &msg.graphicData,
+            graphicId,
+            RefSerialData::Tx::GRAPHIC_ADD,
+            0,
+            RefSerialData::Tx::GraphicColor::PINK);
+
+        RefSerialTransmitter::configCircle(10, 1920 / 2, 1080 / 2, 100, &msg.graphicData);
+    };
+
+    modm::ResumableResult<void> initialize() override
+    {
+        RF_BEGIN();
+        RF_CALL(refSerialTransmitter.sendGraphic(&msg));
+        RF_END_RETURN();
+    };
+
+    modm::ResumableResult<bool> run() override
+    {
+        float t = sinf(tap::arch::clock::getTimeMilliseconds() / 1000.0f * 4.0f) * 0.5f + 0.5f;
+        RF_BEGIN();
+
+        msg.graphicData.operation = RefSerialData::Tx::GRAPHIC_MODIFY;
+        msg.graphicData.lineWidth = 5.0f + 25.0f * t;
+        msg.graphicData.radius = 100.0f + 300.0f * t;
+
+        // modify existing graphic based on the ID (GRAPHIC_MODIFY operation)
+        RF_CALL(refSerialTransmitter.sendGraphic(&msg));
+
+        RF_END_RETURN(true);
+    };
+
+private:
+    RefSerialData::Tx::Graphic1Message msg;
+    float t = 10.0;
+};
+
+}  // namespace graphic