-
Notifications
You must be signed in to change notification settings - Fork 5
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Unified joystick and keyboard controls: UP = Joystick and keyboard turret relative MID = Joystick chassis relative and keyboard turret relative DOWN = Joystick and keyboard chassis relative Beyblade toggle sets turret relative to both
- Loading branch information
1 parent
98eaf43
commit 546b913
Showing
5 changed files
with
277 additions
and
20 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
105 changes: 105 additions & 0 deletions
105
ut-robomaster/src/subsystems/chassis/command_move_chassis.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,105 @@ | ||
| #include "command_move_chassis.hpp" | ||
|
|
||
| namespace commands | ||
| { | ||
| void CommandMoveChassis::initialize() { inputMove = Vector2f(0.0f); } | ||
|
|
||
| void CommandMoveChassis::execute() | ||
| { | ||
| Remote* remote = &drivers->remote; | ||
| // Keyboard | ||
| 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); | ||
| } | ||
| } | ||
|
|
||
| float inputSpin = 0.0f; | ||
| if (keyboardTurretRelative) | ||
| { | ||
| float yawAngle = turret->getTargetLocalYaw(); | ||
|
|
||
| if (beyblade) | ||
| { | ||
| inputSpin = 1.0f; | ||
| } | ||
| else if (inputMove.getLengthSquared() > 0.0f) // auto-align | ||
| { | ||
| inputSpin = calculateAutoAlignCorrection(yawAngle, CHASSIS_AUTOALIGN_ANGLE) * | ||
| CHASSIS_AUTOALIGN_FACTOR; | ||
| } | ||
| chassis->input(Vector2f(inputMove).rotate(yawAngle), inputSpin); | ||
| } | ||
| else | ||
| { | ||
| chassis->input(inputMove, inputSpin); | ||
| } | ||
|
|
||
| // Only take joystick input when no keyboard input and not beyblading | ||
| if (inputMove.getLengthSquared() > 0.0f && inputSpin == 0.0f) | ||
| { | ||
| 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); | ||
|
|
||
| if (joystickTurretRelative) | ||
| { | ||
| float yawAngle = turret->getTargetLocalYaw(); | ||
|
|
||
| // auto-align to turret when moving | ||
| if (inputMove.getLengthSquared() > 0.0f && inputSpin == 0.0f) | ||
| { | ||
| inputSpin = calculateAutoAlignCorrection(yawAngle, CHASSIS_AUTOALIGN_ANGLE) * | ||
| CHASSIS_AUTOALIGN_FACTOR; | ||
| } | ||
|
|
||
| chassis->input(Vector2f(inputMove).rotate(yawAngle), inputSpin); | ||
| } | ||
| else | ||
| { | ||
| chassis->input(inputMove, inputSpin); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| void CommandMoveChassis::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); } | ||
|
|
||
| bool CommandMoveChassis::isFinished() const { return false; } | ||
| } // namespace commands |
59 changes: 59 additions & 0 deletions
59
ut-robomaster/src/subsystems/chassis/command_move_chassis.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 joystickTurretRelative = false, | ||
| bool keyboardTurretRelative = true, | ||
| bool beyblade = false) | ||
| : drivers(drivers), | ||
| chassis(chassis), | ||
| turret(turret), | ||
| joystickTurretRelative(joystickTurretRelative), | ||
| keyboardTurretRelative(keyboardTurretRelative), | ||
| 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 inputMove = Vector2f(0.0f); | ||
| const bool joystickTurretRelative = false; | ||
| const bool keyboardTurretRelative = true; | ||
| const bool beyblade = false; | ||
| }; | ||
| } // namespace commands |
51 changes: 51 additions & 0 deletions
51
ut-robomaster/src/subsystems/turret/command_move_turret.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
45 changes: 45 additions & 0 deletions
45
ut-robomaster/src/subsystems/turret/command_move_turret.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |