diff --git a/ut-robomaster/src/robots/standard/standard_control.hpp b/ut-robomaster/src/robots/standard/standard_control.hpp index d9aa5d20..1b79210a 100644 --- a/ut-robomaster/src/robots/standard/standard_control.hpp +++ b/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})}; }; \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/client_display_subsystem.cpp b/ut-robomaster/src/subsystems/hud/client_display_subsystem.cpp new file mode 100644 index 00000000..0b001ed0 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/client_display_subsystem.hpp b/ut-robomaster/src/subsystems/hud/client_display_subsystem.hpp new file mode 100644 index 00000000..5eed8c31 --- /dev/null +++ b/ut-robomaster/src/subsystems/hud/client_display_subsystem.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include + +#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 \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/command_client_display.cpp b/ut-robomaster/src/subsystems/hud/command_client_display.cpp new file mode 100644 index 00000000..f0eca023 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/command_client_display.hpp b/ut-robomaster/src/subsystems/hud/command_client_display.hpp new file mode 100644 index 00000000..f9e569ea --- /dev/null +++ b/ut-robomaster/src/subsystems/hud/command_client_display.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include + +#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 \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/command_client_display_reset.cpp b/ut-robomaster/src/subsystems/hud/command_client_display_reset.cpp new file mode 100644 index 00000000..e988e95c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/command_client_display_reset.hpp b/ut-robomaster/src/subsystems/hud/command_client_display_reset.hpp new file mode 100644 index 00000000..0235622c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/graphics/graphic_abstract.hpp b/ut-robomaster/src/subsystems/hud/graphics/graphic_abstract.hpp new file mode 100644 index 00000000..aaae5d6b --- /dev/null +++ b/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 initialize() = 0; + virtual ResumableResult 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 \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/graphics/graphic_circle.hpp b/ut-robomaster/src/subsystems/hud/graphics/graphic_circle.hpp new file mode 100644 index 00000000..9b4aaa5f --- /dev/null +++ b/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 initialize() override + { + RF_BEGIN(); + RF_CALL(refSerialTransmitter.sendGraphic(&msg)); + RF_END_RETURN(); + }; + + modm::ResumableResult 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 \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/graphics/graphic_flywheel_on.hpp b/ut-robomaster/src/subsystems/hud/graphics/graphic_flywheel_on.hpp new file mode 100644 index 00000000..51029e81 --- /dev/null +++ b/ut-robomaster/src/subsystems/hud/graphics/graphic_flywheel_on.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include "subsystems/flywheel/flywheel_subsystem.hpp" + +#include "graphic_abstract.hpp" + +using subsystems::flywheel::FlywheelSubsystem; +using tap::communication::serial::RefSerialData; +namespace graphic +{ + +class graphic_flywheel_on : public graphic_abstract +{ +public: + graphic_flywheel_on(src::Drivers *drivers, FlywheelSubsystem *flywheel) + : graphic_abstract(drivers), + flywheel(flywheel) + { + graphicId[0] = 2; + + RefSerialTransmitter::configGraphicGenerics( + &word_msg.graphicData, + graphicId, + RefSerialData::Tx::GRAPHIC_ADD, + 0, + RefSerialData::Tx::GraphicColor::ORANGE); + + // RESOLUTION HAS TO BE 1920x1080 OR HUD WILL NOT WORK PROPERLY + const char *message = "Flywheel on"; + RefSerialTransmitter::configCharacterMsg( + FONT_SIZE, + WIDTH, + CENTER_X, + CENTER_Y, + message, + &word_msg); + }; + + modm::ResumableResult initialize() override + { + RF_BEGIN(); + RF_CALL(refSerialTransmitter.sendGraphic(&word_msg)); + RF_END_RETURN(); + }; + + modm::ResumableResult run() override + { + RF_BEGIN(); + + // msg.graphicData.lineWidth = 5.0f + 25.0f * t; + // msg.graphicData.radius = 100.0f + 300.0f * t; + // DROP_DISTANCE = this.turret.getBulletDropReticle(); + // msg.graphicData.startY = CENTER_Y - turret->getBulletDropReticle(); + // startY is centerY + + word_msg.graphicData.operation = RefSerialData::Tx::GRAPHIC_MODIFY; + + if (flywheel->isActive()) + { + RefSerialTransmitter::configCharacterMsg( + FONT_SIZE, + WIDTH, + CENTER_X, + CENTER_Y, + "Flywheel on", + &word_msg); + } + else + { + RefSerialTransmitter::configCharacterMsg( + FONT_SIZE, + WIDTH, + CENTER_X, + CENTER_Y, + "Flywheel off", + &word_msg); + } + + RF_CALL(refSerialTransmitter.sendGraphic(&word_msg)); + RF_END_RETURN(true); + }; + +private: + RefSerialData::Tx::GraphicCharacterMessage word_msg; + + static constexpr uint16_t FONT_SIZE = 30; + static constexpr uint16_t WIDTH = 3; + static constexpr uint16_t CENTER_X = 50; + static constexpr uint16_t CENTER_Y = 850; + FlywheelSubsystem *flywheel; +}; + +} // namespace graphic \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/graphics/graphic_orientation.hpp b/ut-robomaster/src/subsystems/hud/graphics/graphic_orientation.hpp new file mode 100644 index 00000000..64cef3b2 --- /dev/null +++ b/ut-robomaster/src/subsystems/hud/graphics/graphic_orientation.hpp @@ -0,0 +1,109 @@ +#pragma once +#include "subsystems/turret/turret_subsystem.hpp" + +#include "graphic_abstract.hpp" +using subsystems::turret::TurretSubsystem; + +namespace graphic +{ + +class graphic_orientation : public graphic_abstract +{ +public: + graphic_orientation(src::Drivers *drivers, TurretSubsystem *turret) + : graphic_abstract(drivers), + turret(turret) + { + graphicId[0] = 4; + + RefSerialTransmitter::configGraphicGenerics( + &multiMsg.graphicData[0], + graphicId, + RefSerialData::Tx::GRAPHIC_ADD, + 0, + RefSerialData::Tx::GraphicColor::ORANGE); + + graphicId[0] = 5; + + RefSerialTransmitter::configGraphicGenerics( + &multiMsg.graphicData[1], + graphicId, + RefSerialData::Tx::GRAPHIC_ADD, + 0, + RefSerialData::Tx::GraphicColor::GREEN); + + graphicId[0] = 6; + + RefSerialTransmitter::configGraphicGenerics( + &multiMsg.graphicData[2], + graphicId, + RefSerialData::Tx::GRAPHIC_ADD, + 0, + RefSerialData::Tx::GraphicColor::ORANGE); + + graphicId[0] = 7; + + RefSerialTransmitter::configGraphicGenerics( + &multiMsg.graphicData[3], + graphicId, + RefSerialData::Tx::GRAPHIC_ADD, + 0, + RefSerialData::Tx::GraphicColor::ORANGE); + + RefSerialTransmitter::configLine(10, 0, 0, 1920, 1080, &multiMsg.graphicData[0]); + RefSerialTransmitter::configLine(10, 0, 0, 1920, 1080, &multiMsg.graphicData[1]); + RefSerialTransmitter::configLine(10, 0, 0, 1920, 1080, &multiMsg.graphicData[2]); + RefSerialTransmitter::configLine(10, 0, 0, 1920, 1080, &multiMsg.graphicData[3]); + }; + + modm::ResumableResult initialize() override + { + RF_BEGIN(); + RF_CALL(refSerialTransmitter.sendGraphic(&multiMsg)); + RF_END_RETURN(); + }; + + modm::ResumableResult run() override + { + float a = -turret->getCurrentLocalYaw(); + float r = 100.0f; + + float mx = 150.0f; + float my = 700.0f; + + float ax = mx + cosf(a - M_PI_4) * r; + float ay = my + sinf(a - M_PI_4) * r; + float bx = mx + cosf(a + M_PI_4) * r; + float by = my + sinf(a + M_PI_4) * r; + float cx = mx + cosf(a - M_PI_4 + M_PI) * r; + float cy = my + sinf(a - M_PI_4 + M_PI) * r; + float dx = mx + cosf(a + M_PI_4 + M_PI) * r; + float dy = my + sinf(a + M_PI_4 + M_PI) * r; + + RF_BEGIN(); + + multiMsg.graphicData[0].operation = RefSerialData::Tx::GRAPHIC_MODIFY; + multiMsg.graphicData[1].operation = RefSerialData::Tx::GRAPHIC_MODIFY; + multiMsg.graphicData[2].operation = RefSerialData::Tx::GRAPHIC_MODIFY; + multiMsg.graphicData[3].operation = RefSerialData::Tx::GRAPHIC_MODIFY; + + RefSerialTransmitter::configLine(10, ax, ay, bx, by, &multiMsg.graphicData[0]); + RefSerialTransmitter::configLine(10, bx, by, cx, cy, &multiMsg.graphicData[1]); + RefSerialTransmitter::configLine(10, cx, cy, dx, dy, &multiMsg.graphicData[2]); + RefSerialTransmitter::configLine(10, dx, dy, ax, ay, &multiMsg.graphicData[3]); + + // msg.graphicData[0].value = turret->getChassisYaw() * 1000; + // msg.graphicData[1].value = turret->getCurrentLocalYaw() * 1000 + PI; // turret yaw + + // modify existing graphic based on the ID (GRAPHIC_MODIFY operation) + RF_CALL(refSerialTransmitter.sendGraphic(&multiMsg)); + + RF_END_RETURN(true); + }; + +private: + TurretSubsystem *turret; + RefSerialData::Tx::Graphic5Message multiMsg; +}; + +} // namespace graphic \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/hud/graphics/graphic_reticle.hpp b/ut-robomaster/src/subsystems/hud/graphics/graphic_reticle.hpp new file mode 100644 index 00000000..2d46d0fc --- /dev/null +++ b/ut-robomaster/src/subsystems/hud/graphics/graphic_reticle.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include "subsystems/turret/turret_subsystem.hpp" + +#include "graphic_abstract.hpp" + +using subsystems::turret::TurretSubsystem; +namespace graphic +{ + +class graphic_reticle : public graphic_abstract +{ +public: + graphic_reticle(src::Drivers *drivers, TurretSubsystem *turret) + : graphic_abstract(drivers), + turret(turret) + { + graphicId[0] = 1; + + RefSerialTransmitter::configGraphicGenerics( + &msg.graphicData, + graphicId, + RefSerialData::Tx::GRAPHIC_ADD, + 0, + RefSerialData::Tx::GraphicColor::CYAN); + + // RESOLUTION HAS TO BE 1920x1080 OR HUD WILL NOT WORK PROPERLY + RefSerialTransmitter::configCircle(10, CENTER_X, CENTER_Y, CIRCLE_SIZE, &msg.graphicData); + }; + + modm::ResumableResult initialize() override + { + RF_BEGIN(); + RF_CALL(refSerialTransmitter.sendGraphic(&msg)); + RF_END_RETURN(); + }; + + modm::ResumableResult run() override + { + // float t = sinf(tap::arch::clock::getTimeMilliseconds() / 1000.0f * 3.0f) * 0.5f + 0.5f; + RF_BEGIN(); + + // // msg.graphicData.lineWidth = 5.0f + 25.0f * t; + // // msg.graphicData.radius = 100.0f + 300.0f * t; + // // DROP_DISTANCE = this.turret.getBulletDropReticle(); + // msg.graphicData.startY = CENTER_Y - turret->getBulletDropReticle(); + // // startY is centerY + + // // modify existing graphic based on the ID (GRAPHIC_MODIFY operation) + // RF_CALL(refSerialTransmitter.sendGraphic(&msg)); + + // msg.graphicData.operation = RefSerialData::Tx::GRAPHIC_MODIFY; + // msg.graphicData.lineWidth = 5.0f + 25.0f * t; + // msg.graphicData.radius = 100.0f + 300.0f * t; + + msg.graphicData.operation = RefSerialData::Tx::GRAPHIC_MODIFY; + msg.graphicData.startY = turret->getBulletDropReticle(); + + // modify existing graphic based on the ID (GRAPHIC_MODIFY operation) + RF_CALL(refSerialTransmitter.sendGraphic(&msg)); + RF_END_RETURN(true); + }; + +private: + RefSerialData::Tx::Graphic1Message msg; + static constexpr uint16_t CENTER_X = 1920 / 2; + static constexpr uint16_t CENTER_Y = 1080 / 2; + static constexpr uint16_t CIRCLE_SIZE = 100; + TurretSubsystem *turret; +}; + +} // namespace graphic \ No newline at end of file diff --git a/ut-robomaster/src/subsystems/turret/command_move_turret_aimbot.cpp b/ut-robomaster/src/subsystems/turret/command_move_turret_aimbot.cpp index 18befc57..3c8792fe 100644 --- a/ut-robomaster/src/subsystems/turret/command_move_turret_aimbot.cpp +++ b/ut-robomaster/src/subsystems/turret/command_move_turret_aimbot.cpp @@ -30,7 +30,7 @@ void CommandMoveTurretAimbot::execute() Vector3f targetVel(data.xVel, data.zVel, data.yVel); Vector3f targetAcc(data.xAcc, data.zAcc, data.yAcc); - if (USE_BALLISTICS) + if (USE_BALLISTICS) // use ballistics to adjust turret pitch { // Rotate to world relative pitch float a = turret->getCurrentLocalPitch(); @@ -61,8 +61,9 @@ void CommandMoveTurretAimbot::execute() turret->setTargetWorldAngles(currentWorldYaw + turretYaw, turretPitch); } } - else - { + else // auto aim + { + // find the angle error based on target position from vision float deltaYaw = -atan(targetPos.x / targetPos.y); // yaw is opposite to camera X float deltaPitch = atan(targetPos.z / targetPos.y); float scale = 0.006f; diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp index 9d770935..83c64594 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp @@ -61,11 +61,13 @@ void TurretSubsystem::refresh() } } -void TurretSubsystem::inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration) +void TurretSubsystem::inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration, float distance) { targetPosition = position; targetVelocity = velocity; targetAcceleration = acceleration; + if(distance<5.0) centerDistance = distance; + else centerDistance = 0.0f; } void TurretSubsystem::setTargetWorldAngles(float yaw, float pitch) @@ -90,6 +92,58 @@ float TurretSubsystem::getCurrentLocalPitch() return !isCalibrated ? 0.0f : pitch.getAngle() / PITCH_REDUCTION - basePitch; } +float TurretSubsystem::getBulletDropReticle(){ + if (centerDistance <= 0.1f) return 0; + /* bullet drop based on target position + Vector3f targetPos( + data.xPos + CAMERA_X_OFFSET, + data.zPos + CAMERA_TO_PITCH, + data.yPos + CAMERA_TO_BARRELS); + Vector3f targetVel(data.xVel, data.zVel, data.yVel); + Vector3f targetAcc(data.xAcc, data.zAcc, data.yAcc); + + // Rotate to world relative pitch + float a = turret->getCurrentLocalPitch(); + const float matData[9] = {1.0f, 0, 0, 0, cos(a), -sin(a), 0, sin(a), cos(a)}; + modm::Matrix3f rotMat(matData); + targetPos = rotMat * targetPos; + targetVel = rotMat * targetVel; + targetAcc = rotMat * targetAcc; + + MeasuredKinematicState kinState{targetPos, targetVel, targetAcc}; + + float turretPitch = 0.0f; + float turretYaw = 0.0f; + float travelTime = 0.0f; + + bool projectileTime = computeTravelTime( + kinState, + TARGET_PROJECTILE_VELOCITY, + BALLISTIC_ITERATIONS, + &turretPitch, + &turretYaw, + &travelTime, + -NOZZLE_TO_PITCH); + */ + + // physical distance = 0.5*9.8*powf(travelTime, 2) + // angle from turret = atan(hdrop/d) + float travelTime = centerDistance/TARGET_PROJECTILE_VELOCITY; + float dropDistance = 0.5*9.8*powf(travelTime, 2); + // float targetDistance = hypot(targetPos.x, targetPos.y) - NOZZLE_TO_PITCH; + float reticleDistance = atan(dropDistance/centerDistance); + // linear interpolation. simple, but it is less effective the more distorted the camera is + // 80, guess for vertical angle range. 720, number of pixels in vertical direction + float cameraAnglePixelScale = 80/720; // idk what the actual camera fov is, software didn't know and we can change it + + //number of pixels below current camera position + return reticleDistance*cameraAnglePixelScale+NOZZLE_TO_PITCH; // target pitch - current pitch = bullet drop over distance? + // } // i'm assuming that the camera is pointed above the turret + + // return 0; +} + + bool TurretSubsystem::getIsCalibrated() { return isCalibrated; } void TurretSubsystem::runHardwareTests() @@ -97,4 +151,4 @@ void TurretSubsystem::runHardwareTests() // TODO } } // namespace turret -} // namespace subsystems \ No newline at end of file +} // namespace subsystems diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp index e793836f..9270912a 100644 --- a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp +++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp @@ -28,7 +28,7 @@ class TurretSubsystem : public tap::control::Subsystem void initialize() override; /// @brief Input target data from CV (relative to camera) - void inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration); + void inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration, float centerDistance); void setTargetWorldAngles(float yaw, float pitch); @@ -46,6 +46,8 @@ class TurretSubsystem : public tap::control::Subsystem float getCurrentLocalPitch(); + float getBulletDropReticle(); + bool getIsCalibrated(); void refresh() override; @@ -64,6 +66,8 @@ class TurretSubsystem : public tap::control::Subsystem Vector3f targetVelocity = Vector3f(0.0f); Vector3f targetAcceleration = Vector3f(0.0f); + float centerDistance = 0.0f; + #if defined(TARGET_STANDARD) || defined(TARGET_HERO) As5600 yawEncoder; DoubleYawMotor yaw; diff --git a/ut-robomaster/taproot/src/tap/control/command.hpp b/ut-robomaster/taproot/src/tap/control/command.hpp index c8b84055..34e3b18a 100644 --- a/ut-robomaster/taproot/src/tap/control/command.hpp +++ b/ut-robomaster/taproot/src/tap/control/command.hpp @@ -1,3 +1,4 @@ +#include /*****************************************************************************/ /********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/ /*****************************************************************************/