Unified control modes

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

@@ -5,10 +5,12 @@
 #include "tap/control/toggle_command_mapping.hpp"
 
 #include "robots/common/common_control.hpp"
+#include "subsystems/chassis/command_move_chassis.hpp"
 #include "subsystems/chassis/command_move_chassis_joystick.hpp"
 #include "subsystems/chassis/command_move_chassis_keyboard.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"
@@ -27,56 +29,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/chassis/command_move_chassis.cpp

@@ -0,0 +1,123 @@
+#include "command_move_chassis.hpp"
+
+namespace commands
+{
+void CommandMoveChassis::initialize() { keyboardInputMove = Vector2f(0.0f); }
+
+void CommandMoveChassis::execute()
+{
+    float yawAngle = turret->getTargetLocalYaw();
+    Vector2f inputMove = Vector2f(0.0f);
+    float inputSpin = 0.0f;
+
+    // get keyboard input
+    float keyboardInputSpin;
+    bool hasKeyboardInput = applyKeyboardInput(keyboardInputMove, keyboardInputSpin);
+
+    // get joystick input
+    Vector2f joystickInputMove;
+    float joystickInputSpin;
+    bool hasJoystickInput = applyJoystickInput(joystickInputMove, joystickInputSpin);
+
+    // decide which input source to use (keyboard has inertia)
+    if (!hasKeyboardInput && hasJoystickInput)
+    {
+        inputMove = joystickInputMove;
+        inputSpin = joystickInputSpin;
+    }
+    else
+    {
+        inputMove = keyboardInputMove;
+        inputSpin = keyboardInputSpin;
+    }
+
+    // auto-align chassis to turret when moving
+    if (inputMove.getLengthSquared() > 0.0f && inputSpin == 0.0f)
+    {
+        inputSpin = calculateAutoAlignCorrection(yawAngle, CHASSIS_AUTOALIGN_ANGLE) *
+                    CHASSIS_AUTOALIGN_FACTOR;
+    }
+
+    // override spin input while beyblading
+    if (beyblade)
+    {
+        inputSpin = 1.0f;
+    }
+
+    // rotate movement vector relative to turret
+    if (turretRelative)
+    {
+        inputMove = inputMove.rotate(yawAngle);
+    }
+
+    chassis->input(inputMove, inputSpin);
+}
+
+void CommandMoveChassis::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); }
+
+bool CommandMoveChassis::isFinished() const { return false; }
+
+bool CommandMoveChassis::applyKeyboardInput(Vector2f &inputMove, float &inputSpin)
+{
+    Remote *remote = &drivers->remote;
+
+    inputSpin = 0.0f;  // no keyboard spin controls
+
+    Vector2f rawMoveInput = Vector2f(
+        remote->keyPressed(Remote::Key::D) - remote->keyPressed(Remote::Key::A),
+        remote->keyPressed(Remote::Key::W) - remote->keyPressed(Remote::Key::S));
+
+    float rawInputLen = rawMoveInput.getLength();
+
+    if (rawInputLen > 0.0f)
+    {
+        Vector2f moveDir = rawMoveInput / rawInputLen;  // normalize input
+        inputMove += moveDir * KEYBOARD_ACCEL * DT;     // incorporate input
+        inputMove /= max(1.0f, inputMove.getLength());  // clamp length
+    }
+    else
+    {
+        // decelerate when input stops
+        float len = inputMove.getLength();
+        if (len > 0.0f)
+        {
+            inputMove *= max(1.0f - KEYBOARD_DECEL * DT / len, 0.0f);
+        }
+    }
+
+    return rawMoveInput != Vector2f(0.0f);
+}
+
+bool CommandMoveChassis::applyJoystickInput(Vector2f &inputMove, float &inputSpin)
+{
+    Remote *remote = &drivers->remote;
+
+    inputMove = Vector2f(
+        remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL),
+        remote->getChannel(Remote::Channel::RIGHT_VERTICAL));
+
+    inputSpin = remote->getChannel(Remote::Channel::WHEEL);
+
+    float inputMoveLen = inputMove.getLength();
+    if (inputMoveLen < ANALOG_DEAD_ZONE)
+    {
+        inputMove = Vector2f(0.0f);
+    }
+    else
+    {
+        inputMove /= max(1.0f, inputMoveLen);  // clamp length
+    }
+
+    if (abs(inputSpin) < ANALOG_DEAD_ZONE)
+    {
+        inputSpin = 0.0f;
+    }
+
+    // apply quadratic input ramping
+    inputMove *= inputMove.getLength();
+    inputSpin *= abs(inputSpin);
+
+    return inputMove != Vector2f(0.0f) || inputSpin != 0.0f;
+}
+
+}  // namespace commands

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

@@ -0,0 +1,59 @@
+#pragma once
+
+#include "tap/control/command.hpp"
+
+#include "robots/robot_constants.hpp"
+#include "subsystems/chassis/chassis_subsystem.hpp"
+#include "subsystems/turret/turret_subsystem.hpp"
+#include "utils/chassis_auto_align.hpp"
+
+#include "drivers.hpp"
+
+namespace commands
+{
+using namespace tap::communication::serial;
+using namespace modm;
+using subsystems::chassis::ChassisSubsystem;
+using subsystems::turret::TurretSubsystem;
+
+class CommandMoveChassis : public tap::control::Command
+{
+public:
+    CommandMoveChassis(
+        src::Drivers *drivers,
+        ChassisSubsystem *chassis,
+        TurretSubsystem *turret,
+        bool turretRelative = false,
+        bool beyblade = false)
+        : drivers(drivers),
+          chassis(chassis),
+          turret(turret),
+          turretRelative(turretRelative),
+          beyblade(beyblade)
+    {
+        addSubsystemRequirement(chassis);
+    }
+
+    void initialize() override;
+
+    void execute() override;
+
+    void end(bool interrupted) override;
+
+    bool isFinished() const override;
+
+    const char *getName() const override { return "move chassis command"; }
+
+private:
+    src::Drivers *drivers;
+    ChassisSubsystem *chassis;
+    TurretSubsystem *turret;
+
+    Vector2f keyboardInputMove = Vector2f(0.0f);
+    const bool turretRelative = false;
+    const bool beyblade = false;
+
+    bool applyKeyboardInput(Vector2f &moveOut, float &spinOut);
+    bool applyJoystickInput(Vector2f &moveOut, float &spinOut);
+};
+}  // namespace commands

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

@@ -0,0 +1,51 @@
+#include "command_move_turret.hpp"
+
+namespace commands
+{
+void CommandMoveTurret::initialize() { isCalibrated = false; }
+
+void CommandMoveTurret::execute()
+{
+    if (!isCalibrated && turret->getIsCalibrated())
+    {
+        yaw = turret->getCurrentLocalYaw() + turret->getChassisYaw();
+        pitch = turret->getCurrentLocalPitch();
+        isCalibrated = true;
+    }
+
+    if (isCalibrated)
+    {
+        Remote* remote = &drivers->remote;
+        float yawInput = 0.0f;
+        float pitchInput = 0.0f;
+
+        // Mouse input
+        yawInput = remote->getMouseX() * MOUSE_SENS_YAW * DT;
+        pitchInput = -remote->getMouseY() * MOUSE_SENS_PITCH * DT;
+
+        // If no mouse input, get joystick input
+        if (yawInput == 0.0f && pitchInput == 0.0f)
+        {
+            // Joystick input
+            float h = remote->getChannel(Remote::Channel::LEFT_HORIZONTAL);
+            float v = remote->getChannel(Remote::Channel::LEFT_VERTICAL);
+
+            if (abs(h) < ANALOG_DEAD_ZONE) h = 0.0f;
+            if (abs(v) < ANALOG_DEAD_ZONE) v = 0.0f;
+
+            yawInput = h * abs(h) * YAW_INPUT_SCALE * DT;      // quadratic input map
+            pitchInput = v * abs(v) * PITCH_INPUT_SCALE * DT;  // quadratic input map
+        }
+
+        yaw -= yawInput;
+        pitch += pitchInput;
+        pitch = modm::min(modm::max(pitch, PITCH_MIN), PITCH_MAX);
+
+        turret->setTargetWorldAngles(yaw, pitch);
+    }
+}
+
+void CommandMoveTurret::end(bool) {}
+
+bool CommandMoveTurret::isFinished(void) const { return false; }
+}  // namespace commands

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

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "tap/communication/serial/remote.hpp"
+#include "tap/control/command.hpp"
+
+#include "robots/robot_constants.hpp"
+#include "subsystems/turret/turret_subsystem.hpp"
+
+#include "drivers.hpp"
+
+namespace commands
+{
+using subsystems::turret::TurretSubsystem;
+using tap::communication::serial::Remote;
+
+class CommandMoveTurret : public tap::control::Command
+{
+public:
+    CommandMoveTurret(src::Drivers* drivers, TurretSubsystem* turret)
+        : drivers(drivers),
+          turret(turret)
+    {
+        addSubsystemRequirement(turret);
+    }
+
+    void initialize() override;
+
+    void execute() override;
+
+    void end(bool interrupted) override;
+
+    bool isFinished() const override;
+
+    const char* getName() const override { return "move turret command"; }
+
+private:
+    src::Drivers* drivers;
+    TurretSubsystem* turret;
+
+    bool isCalibrated = false;
+
+    float yaw = 0.0f;
+    float pitch = 0.0f;
+};
+}  // namespace commands