mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpilib] Add PS4Controller, remove Hand from GenericHID/XboxController (#3345)
- GenericHID is now concrete, and has only getRawAxis/Button(int) functionality - getXxx() has been moved into Joystick as that's the only place where it makes sense - Hand (and therefore getXxx(Hand)) has been removed, replaced by specific getLeft/RightXxx() methods in XboxController and the new PS4Controller class - C++ ::Button:: and ::Axis:: enums have been converted to identically-namespaced static constexpr ints
This commit is contained in:
@@ -2,7 +2,6 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <frc/GenericHID.h>
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc/drive/DifferentialDrive.h>
|
||||
@@ -23,9 +22,8 @@ class Robot : public frc::TimedRobot {
|
||||
// Drive with split arcade style
|
||||
// That means that the Y axis of the left stick moves forward
|
||||
// and backward, and the X of the right stick turns left and right.
|
||||
m_robotDrive.ArcadeDrive(
|
||||
m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
|
||||
m_driverController.GetX(frc::GenericHID::JoystickHand::kRightHand));
|
||||
m_robotDrive.ArcadeDrive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightX());
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -17,9 +17,8 @@ RobotContainer::RobotContainer() {
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(
|
||||
m_driverController.GetY(frc::GenericHID::kLeftHand),
|
||||
m_driverController.GetX(frc::GenericHID::kRightHand));
|
||||
m_drive.ArcadeDrive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
@@ -28,7 +27,7 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
|
||||
frc2::JoystickButton(&m_driverController, 1)
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
|
||||
.WhenPressed(
|
||||
[this] {
|
||||
m_arm.SetGoal(2_rad);
|
||||
@@ -37,7 +36,7 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
{&m_arm});
|
||||
|
||||
// Move the arm to neutral position when the 'B' button is pressed.
|
||||
frc2::JoystickButton(&m_driverController, 2)
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
|
||||
.WhenPressed(
|
||||
[this] {
|
||||
m_arm.SetGoal(ArmConstants::kArmOffset);
|
||||
@@ -46,11 +45,12 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
{&m_arm});
|
||||
|
||||
// Disable the arm controller when Y is pressed.
|
||||
frc2::JoystickButton(&m_driverController, 4)
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kY)
|
||||
.WhenPressed([this] { m_arm.Disable(); }, {&m_arm});
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, 6)
|
||||
frc2::JoystickButton(&m_driverController,
|
||||
frc::XboxController::Button::kRightBumper)
|
||||
.WhenPressed([this] { m_drive.SetMaxOutput(0.5); })
|
||||
.WhenReleased([this] { m_drive.SetMaxOutput(1); });
|
||||
}
|
||||
|
||||
@@ -17,9 +17,8 @@ RobotContainer::RobotContainer() {
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(
|
||||
m_driverController.GetY(frc::GenericHID::kLeftHand),
|
||||
m_driverController.GetX(frc::GenericHID::kRightHand));
|
||||
m_drive.ArcadeDrive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
@@ -28,16 +27,17 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// Move the arm to 2 radians above horizontal when the 'A' button is pressed.
|
||||
frc2::JoystickButton(&m_driverController, 1)
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
|
||||
.WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
|
||||
|
||||
// Move the arm to neutral position when the 'B' button is pressed.
|
||||
frc2::JoystickButton(&m_driverController, 1)
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
|
||||
.WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
|
||||
{&m_arm});
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, 6)
|
||||
frc2::JoystickButton(&m_driverController,
|
||||
frc::XboxController::Button::kRightBumper)
|
||||
.WhenPressed([this] { m_drive.SetMaxOutput(0.5); })
|
||||
.WhenReleased([this] { m_drive.SetMaxOutput(1); });
|
||||
}
|
||||
|
||||
@@ -3,7 +3,6 @@
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/GenericHID.h>
|
||||
#include <frc/Joystick.h>
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/StateSpaceUtil.h>
|
||||
|
||||
@@ -18,16 +18,14 @@ class Robot : public frc::TimedRobot {
|
||||
void TeleopPeriodic() override {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
const auto xSpeed = -m_speedLimiter.Calculate(
|
||||
m_controller.GetY(frc::GenericHID::kLeftHand)) *
|
||||
const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
const auto rot = -m_rotLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kRightHand)) *
|
||||
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
|
||||
Drivetrain::kMaxAngularSpeed;
|
||||
|
||||
m_drive.Drive(xSpeed, rot);
|
||||
|
||||
@@ -18,16 +18,14 @@ class Robot : public frc::TimedRobot {
|
||||
void TeleopPeriodic() override {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
const auto xSpeed = -m_speedLimiter.Calculate(
|
||||
m_controller.GetY(frc::GenericHID::kLeftHand)) *
|
||||
const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
const auto rot = -m_rotLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kRightHand)) *
|
||||
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
|
||||
Drivetrain::kMaxAngularSpeed;
|
||||
|
||||
m_drive.Drive(xSpeed, rot);
|
||||
|
||||
@@ -18,9 +18,8 @@ RobotContainer::RobotContainer() {
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(
|
||||
m_driverController.GetY(frc::GenericHID::kLeftHand),
|
||||
m_driverController.GetX(frc::GenericHID::kRightHand));
|
||||
m_drive.ArcadeDrive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
@@ -29,18 +28,19 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, 6)
|
||||
frc2::JoystickButton(&m_driverController,
|
||||
frc::XboxController::Button::kRightBumper)
|
||||
.WhenPressed(&m_driveHalfSpeed)
|
||||
.WhenReleased(&m_driveFullSpeed);
|
||||
|
||||
// Drive forward by 3 meters when the 'A' button is pressed, with a timeout of
|
||||
// 10 seconds
|
||||
frc2::JoystickButton(&m_driverController, 1)
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
|
||||
.WhenPressed(DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));
|
||||
|
||||
// Do the same thing as above when the 'B' button is pressed, but defined
|
||||
// inline
|
||||
frc2::JoystickButton(&m_driverController, 2)
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
|
||||
.WhenPressed(
|
||||
frc2::TrapezoidProfileCommand<units::meters>(
|
||||
frc::TrapezoidProfile<units::meters>(
|
||||
|
||||
@@ -3,7 +3,6 @@
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/GenericHID.h>
|
||||
#include <frc/Joystick.h>
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/StateSpaceUtil.h>
|
||||
|
||||
@@ -16,9 +16,8 @@ RobotContainer::RobotContainer() {
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(
|
||||
m_driverController.GetY(frc::GenericHID::kLeftHand),
|
||||
m_driverController.GetX(frc::GenericHID::kRightHand));
|
||||
m_drive.ArcadeDrive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
@@ -27,18 +26,21 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// Spin up the shooter when the 'A' button is pressed
|
||||
frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_spinUpShooter);
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
|
||||
.WhenPressed(&m_spinUpShooter);
|
||||
|
||||
// Turn off the shooter when the 'B' button is pressed
|
||||
frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_stopShooter);
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
|
||||
.WhenPressed(&m_stopShooter);
|
||||
|
||||
// Shoot when the 'X' button is held
|
||||
frc2::JoystickButton(&m_driverController, 3)
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kX)
|
||||
.WhenPressed(&m_shoot)
|
||||
.WhenReleased(&m_stopFeeder);
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, 6)
|
||||
frc2::JoystickButton(&m_driverController,
|
||||
frc::XboxController::Button::kRightBumper)
|
||||
.WhenPressed(&m_driveHalfSpeed)
|
||||
.WhenReleased(&m_driveFullSpeed);
|
||||
}
|
||||
|
||||
@@ -22,10 +22,9 @@ RobotContainer::RobotContainer()
|
||||
frc::SmartDashboard::PutData(&m_wrist);
|
||||
frc::SmartDashboard::PutData(&m_claw);
|
||||
|
||||
m_drivetrain.SetDefaultCommand(TankDrive(
|
||||
[this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kLeftHand); },
|
||||
[this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kRightHand); },
|
||||
&m_drivetrain));
|
||||
m_drivetrain.SetDefaultCommand(TankDrive([this] { return m_joy.GetLeftY(); },
|
||||
[this] { return m_joy.GetRightY(); },
|
||||
&m_drivetrain));
|
||||
|
||||
// Configure the button bindings
|
||||
ConfigureButtonBindings();
|
||||
@@ -33,24 +32,20 @@ RobotContainer::RobotContainer()
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
frc2::JoystickButton m_dUp{&m_joy, 5};
|
||||
frc2::JoystickButton m_dRight{&m_joy, 6};
|
||||
frc2::JoystickButton m_dDown{&m_joy, 7};
|
||||
frc2::JoystickButton m_dLeft{&m_joy, 8};
|
||||
frc2::JoystickButton m_l2{&m_joy, 9};
|
||||
frc2::JoystickButton m_r2{&m_joy, 10};
|
||||
frc2::JoystickButton m_l1{&m_joy, 11};
|
||||
frc2::JoystickButton m_r1{&m_joy, 12};
|
||||
|
||||
m_dUp.WhenPressed(SetElevatorSetpoint(0.25, &m_elevator));
|
||||
m_dDown.WhenPressed(SetElevatorSetpoint(0.0, &m_elevator));
|
||||
m_dRight.WhenPressed(CloseClaw(&m_claw));
|
||||
m_dLeft.WhenPressed(OpenClaw(&m_claw));
|
||||
|
||||
m_r1.WhenPressed(PrepareToPickup(&m_claw, &m_wrist, &m_elevator));
|
||||
m_r2.WhenPressed(Pickup(&m_claw, &m_wrist, &m_elevator));
|
||||
m_l1.WhenPressed(Place(&m_claw, &m_wrist, &m_elevator));
|
||||
m_l2.WhenPressed(Autonomous(&m_claw, &m_wrist, &m_elevator, &m_drivetrain));
|
||||
frc2::JoystickButton(&m_joy, 5).WhenPressed(
|
||||
SetElevatorSetpoint(0.25, &m_elevator));
|
||||
frc2::JoystickButton(&m_joy, 6).WhenPressed(CloseClaw(&m_claw));
|
||||
frc2::JoystickButton(&m_joy, 7).WhenPressed(
|
||||
SetElevatorSetpoint(0.0, &m_elevator));
|
||||
frc2::JoystickButton(&m_joy, 8).WhenPressed(OpenClaw(&m_claw));
|
||||
frc2::JoystickButton(&m_joy, 9).WhenPressed(
|
||||
Autonomous(&m_claw, &m_wrist, &m_elevator, &m_drivetrain));
|
||||
frc2::JoystickButton(&m_joy, 10)
|
||||
.WhenPressed(Pickup(&m_claw, &m_wrist, &m_elevator));
|
||||
frc2::JoystickButton(&m_joy, 11)
|
||||
.WhenPressed(Place(&m_claw, &m_wrist, &m_elevator));
|
||||
frc2::JoystickButton(&m_joy, 12)
|
||||
.WhenPressed(PrepareToPickup(&m_claw, &m_wrist, &m_elevator));
|
||||
}
|
||||
|
||||
frc2::Command* RobotContainer::GetAutonomousCommand() {
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/Joystick.h>
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc2/command/Command.h>
|
||||
|
||||
#include "commands/Autonomous.h"
|
||||
@@ -28,7 +28,7 @@ class RobotContainer {
|
||||
|
||||
private:
|
||||
// The robot's subsystems and commands are defined here...
|
||||
frc::Joystick m_joy{0};
|
||||
frc::XboxController m_joy{0};
|
||||
|
||||
Claw m_claw;
|
||||
Wrist m_wrist;
|
||||
|
||||
@@ -19,9 +19,8 @@ RobotContainer::RobotContainer() {
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(
|
||||
m_driverController.GetY(frc::GenericHID::kLeftHand),
|
||||
m_driverController.GetX(frc::GenericHID::kRightHand));
|
||||
m_drive.ArcadeDrive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
@@ -31,8 +30,8 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
|
||||
// Assorted commands to be bound to buttons
|
||||
|
||||
// Stabilize robot to drive straight with gyro when left bumper is held
|
||||
frc2::JoystickButton(&m_driverController, 5)
|
||||
// Stabilize robot to drive straight with gyro when L1 is held
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kL1)
|
||||
.WhenHeld(frc2::PIDCommand{
|
||||
frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
|
||||
dc::kStabilizationD},
|
||||
@@ -42,24 +41,22 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
0,
|
||||
// Pipe the output to the turning controls
|
||||
[this](double output) {
|
||||
m_drive.ArcadeDrive(m_driverController.GetY(
|
||||
frc::GenericHID::JoystickHand::kLeftHand),
|
||||
output);
|
||||
m_drive.ArcadeDrive(m_driverController.GetLeftY(), output);
|
||||
},
|
||||
// Require the robot drive
|
||||
{&m_drive}});
|
||||
|
||||
// Turn to 90 degrees when the 'X' button is pressed
|
||||
frc2::JoystickButton(&m_driverController, 3)
|
||||
// Turn to 90 degrees when the 'Cross' button is pressed
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kCross)
|
||||
.WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
|
||||
|
||||
// Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5
|
||||
// second timeout
|
||||
frc2::JoystickButton(&m_driverController, 1)
|
||||
// Turn to -90 degrees with a profile when the 'Square' button is pressed,
|
||||
// with a 5 second timeout
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kSquare)
|
||||
.WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, 6)
|
||||
// While holding R1, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kR1)
|
||||
.WhenPressed(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(0.5); }})
|
||||
.WhenReleased(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(1); }});
|
||||
}
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc/PS4Controller.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/Command.h>
|
||||
@@ -32,7 +32,7 @@ class RobotContainer {
|
||||
|
||||
private:
|
||||
// The driver's controller
|
||||
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
|
||||
frc::PS4Controller m_driverController{OIConstants::kDriverControllerPort};
|
||||
|
||||
// The robot's subsystems and commands are defined here...
|
||||
|
||||
|
||||
@@ -23,9 +23,8 @@ RobotContainer::RobotContainer() {
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(
|
||||
m_driverController.GetY(frc::GenericHID::kLeftHand),
|
||||
m_driverController.GetX(frc::GenericHID::kRightHand));
|
||||
m_drive.ArcadeDrive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
@@ -33,12 +32,14 @@ RobotContainer::RobotContainer() {
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// Grab the hatch when the 'A' button is pressed.
|
||||
frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_grabHatch);
|
||||
// Release the hatch when the 'B' button is pressed.
|
||||
frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_releaseHatch);
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, 6)
|
||||
// Grab the hatch when the 'Circle' button is pressed.
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kCircle)
|
||||
.WhenPressed(&m_grabHatch);
|
||||
// Release the hatch when the 'Square' button is pressed.
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kSquare)
|
||||
.WhenPressed(&m_releaseHatch);
|
||||
// While holding R1, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kR1)
|
||||
.WhenPressed(&m_driveHalfSpeed)
|
||||
.WhenReleased(&m_driveFullSpeed);
|
||||
}
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc/PS4Controller.h>
|
||||
#include <frc/smartdashboard/SendableChooser.h>
|
||||
#include <frc2/command/Command.h>
|
||||
#include <frc2/command/FunctionalCommand.h>
|
||||
@@ -35,7 +35,7 @@ class RobotContainer {
|
||||
|
||||
private:
|
||||
// The driver's controller
|
||||
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
|
||||
frc::PS4Controller m_driverController{OIConstants::kDriverControllerPort};
|
||||
|
||||
// The robot's subsystems and commands are defined here...
|
||||
|
||||
|
||||
@@ -27,9 +27,8 @@ RobotContainer::RobotContainer() {
|
||||
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(DefaultDrive(
|
||||
&m_drive,
|
||||
[this] { return m_driverController.GetY(frc::GenericHID::kLeftHand); },
|
||||
[this] { return m_driverController.GetX(frc::GenericHID::kRightHand); }));
|
||||
&m_drive, [this] { return m_driverController.GetLeftY(); },
|
||||
[this] { return m_driverController.GetRightX(); }));
|
||||
}
|
||||
|
||||
void RobotContainer::ConfigureButtonBindings() {
|
||||
@@ -41,13 +40,14 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
// stack-allocated and declared as members of RobotContainer.
|
||||
|
||||
// Grab the hatch when the 'A' button is pressed.
|
||||
frc2::JoystickButton(&m_driverController, 1)
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
|
||||
.WhenPressed(new GrabHatch(&m_hatch));
|
||||
// Release the hatch when the 'B' button is pressed.
|
||||
frc2::JoystickButton(&m_driverController, 2)
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
|
||||
.WhenPressed(new ReleaseHatch(&m_hatch));
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, 6)
|
||||
frc2::JoystickButton(&m_driverController,
|
||||
frc::XboxController::Button::kRightBumper)
|
||||
.WhenHeld(new HalveDriveSpeed(&m_drive));
|
||||
}
|
||||
|
||||
|
||||
@@ -2,7 +2,6 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <frc/GenericHID.h>
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
|
||||
@@ -24,7 +23,7 @@ class Robot : public frc::TimedRobot {
|
||||
}
|
||||
|
||||
private:
|
||||
frc::XboxController m_hid{0};
|
||||
frc::GenericHID m_hid{0};
|
||||
};
|
||||
|
||||
#ifndef RUNNING_FRC_TESTS
|
||||
|
||||
@@ -30,23 +30,20 @@ class Robot : public frc::TimedRobot {
|
||||
void DriveWithJoystick(bool fieldRelative) {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
const auto xSpeed = -m_xspeedLimiter.Calculate(
|
||||
m_controller.GetY(frc::GenericHID::kLeftHand)) *
|
||||
const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the y speed or sideways/strafe speed. We are inverting this because
|
||||
// we want a positive value when we pull to the left. Xbox controllers
|
||||
// return positive values when you pull to the right by default.
|
||||
const auto ySpeed = -m_yspeedLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kLeftHand)) *
|
||||
const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
const auto rot = -m_rotLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kRightHand)) *
|
||||
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
|
||||
Drivetrain::kMaxAngularSpeed;
|
||||
|
||||
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);
|
||||
|
||||
@@ -30,10 +30,9 @@ RobotContainer::RobotContainer() {
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.Drive(m_driverController.GetY(frc::GenericHID::kLeftHand),
|
||||
m_driverController.GetX(frc::GenericHID::kRightHand),
|
||||
m_driverController.GetX(frc::GenericHID::kLeftHand),
|
||||
false);
|
||||
m_drive.Drive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightX(),
|
||||
m_driverController.GetLeftX(), false);
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
@@ -42,7 +41,8 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, 6)
|
||||
frc2::JoystickButton(&m_driverController,
|
||||
frc::XboxController::Button::kRightBumper)
|
||||
.WhenPressed(&m_driveHalfSpeed)
|
||||
.WhenReleased(&m_driveFullSpeed);
|
||||
}
|
||||
|
||||
@@ -30,23 +30,20 @@ class Robot : public frc::TimedRobot {
|
||||
void DriveWithJoystick(bool fieldRelative) {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
const auto xSpeed = -m_xspeedLimiter.Calculate(
|
||||
m_controller.GetY(frc::GenericHID::kLeftHand)) *
|
||||
const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the y speed or sideways/strafe speed. We are inverting this because
|
||||
// we want a positive value when we pull to the left. Xbox controllers
|
||||
// return positive values when you pull to the right by default.
|
||||
const auto ySpeed = -m_yspeedLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kLeftHand)) *
|
||||
const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
const auto rot = -m_rotLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kRightHand)) *
|
||||
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
|
||||
Drivetrain::kMaxAngularSpeed;
|
||||
|
||||
m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);
|
||||
|
||||
@@ -4,9 +4,9 @@
|
||||
|
||||
#include "triggers/DoubleButton.h"
|
||||
|
||||
#include <frc/Joystick.h>
|
||||
#include <frc/GenericHID.h>
|
||||
|
||||
DoubleButton::DoubleButton(frc::Joystick* joy, int button1, int button2)
|
||||
DoubleButton::DoubleButton(frc::GenericHID* joy, int button1, int button2)
|
||||
: m_joy(*joy) {
|
||||
m_button1 = button1;
|
||||
m_button2 = button2;
|
||||
|
||||
@@ -7,17 +7,17 @@
|
||||
#include <frc/buttons/Trigger.h>
|
||||
|
||||
namespace frc {
|
||||
class Joystick;
|
||||
class GenericHID;
|
||||
} // namespace frc
|
||||
|
||||
class DoubleButton : public frc::Trigger {
|
||||
public:
|
||||
DoubleButton(frc::Joystick* joy, int button1, int button2);
|
||||
DoubleButton(frc::GenericHID* joy, int button1, int button2);
|
||||
|
||||
bool Get() override;
|
||||
|
||||
private:
|
||||
frc::Joystick& m_joy;
|
||||
frc::GenericHID& m_joy;
|
||||
int m_button1;
|
||||
int m_button2;
|
||||
};
|
||||
|
||||
@@ -28,9 +28,8 @@ RobotContainer::RobotContainer() {
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(
|
||||
m_driverController.GetY(frc::GenericHID::kLeftHand),
|
||||
m_driverController.GetX(frc::GenericHID::kRightHand));
|
||||
m_drive.ArcadeDrive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
|
||||
@@ -54,16 +54,14 @@ class Robot : public frc::TimedRobot {
|
||||
void TeleopPeriodic() override {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
const auto xSpeed = -m_speedLimiter.Calculate(
|
||||
m_controller.GetY(frc::GenericHID::kLeftHand)) *
|
||||
const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
const auto rot = -m_rotLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kRightHand)) *
|
||||
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
|
||||
Drivetrain::kMaxAngularSpeed;
|
||||
|
||||
m_drive.Drive(xSpeed, rot);
|
||||
|
||||
@@ -45,12 +45,15 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// Run instant command 1 when the 'A' button is pressed
|
||||
frc2::JoystickButton(&m_driverController, 0).WhenPressed(&m_instantCommand1);
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
|
||||
.WhenPressed(&m_instantCommand1);
|
||||
// Run instant command 2 when the 'X' button is pressed
|
||||
frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_instantCommand2);
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kX)
|
||||
.WhenPressed(&m_instantCommand2);
|
||||
// Run instant command 3 when the 'Y' button is held; release early to
|
||||
// interrupt
|
||||
frc2::JoystickButton(&m_driverController, 4).WhenHeld(&m_waitCommand);
|
||||
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kY)
|
||||
.WhenHeld(&m_waitCommand);
|
||||
}
|
||||
|
||||
frc2::Command* RobotContainer::GetAutonomousCommand() {
|
||||
|
||||
@@ -41,16 +41,14 @@ class Robot : public frc::TimedRobot {
|
||||
void TeleopPeriodic() override {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
const auto xSpeed = -m_speedLimiter.Calculate(
|
||||
m_controller.GetY(frc::GenericHID::kLeftHand)) *
|
||||
const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
auto rot = -m_rotLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kRightHand)) *
|
||||
auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
|
||||
Drivetrain::kMaxAngularSpeed;
|
||||
|
||||
m_drive.Drive(xSpeed, rot);
|
||||
|
||||
@@ -3,7 +3,6 @@
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/GenericHID.h>
|
||||
#include <frc/StateSpaceUtil.h>
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
@@ -112,7 +111,7 @@ class Robot : public frc::TimedRobot {
|
||||
// Sets the target position of our arm. This is similar to setting the
|
||||
// setpoint of a PID controller.
|
||||
frc::TrapezoidProfile<units::radians>::State goal;
|
||||
if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
|
||||
if (m_joystick.GetRightBumper()) {
|
||||
// We pressed the bumper, so let's set our next reference
|
||||
goal = {kRaisedPosition, 0_rad_per_s};
|
||||
} else {
|
||||
|
||||
@@ -28,9 +28,8 @@ RobotContainer::RobotContainer() {
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.ArcadeDrive(
|
||||
m_driverController.GetY(frc::GenericHID::kLeftHand),
|
||||
m_driverController.GetX(frc::GenericHID::kRightHand));
|
||||
m_drive.ArcadeDrive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightX());
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
@@ -47,7 +46,8 @@ void RobotContainer::ConfigureButtonBindings() {
|
||||
// Configure your button bindings here
|
||||
|
||||
// While holding the shoulder button, drive at half speed
|
||||
frc2::JoystickButton(&m_driverController, 6)
|
||||
frc2::JoystickButton(&m_driverController,
|
||||
frc::XboxController::Button::kRightBumper)
|
||||
.WhenPressed(&m_driveHalfSpeed)
|
||||
.WhenReleased(&m_driveFullSpeed);
|
||||
}
|
||||
|
||||
@@ -3,7 +3,6 @@
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/GenericHID.h>
|
||||
#include <frc/StateSpaceUtil.h>
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
@@ -109,7 +108,7 @@ class Robot : public frc::TimedRobot {
|
||||
// Sets the target height of our elevator. This is similar to setting the
|
||||
// setpoint of a PID controller.
|
||||
frc::TrapezoidProfile<units::meters>::State goal;
|
||||
if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
|
||||
if (m_joystick.GetRightBumper()) {
|
||||
// We pressed the bumper, so let's set our next reference
|
||||
goal = {kRaisedPosition, 0_fps};
|
||||
} else {
|
||||
|
||||
@@ -4,7 +4,6 @@
|
||||
|
||||
#include <frc/DriverStation.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/GenericHID.h>
|
||||
#include <frc/StateSpaceUtil.h>
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
@@ -93,7 +92,7 @@ class Robot : public frc::TimedRobot {
|
||||
void TeleopPeriodic() override {
|
||||
// Sets the target speed of our flywheel. This is similar to setting the
|
||||
// setpoint of a PID controller.
|
||||
if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
|
||||
if (m_joystick.GetRightBumper()) {
|
||||
// We pressed the bumper, so let's set our next reference
|
||||
m_loop.SetNextR(frc::MakeMatrix<1, 1>(kSpinup.to<double>()));
|
||||
} else {
|
||||
|
||||
@@ -4,7 +4,6 @@
|
||||
|
||||
#include <frc/DriverStation.h>
|
||||
#include <frc/Encoder.h>
|
||||
#include <frc/GenericHID.h>
|
||||
#include <frc/StateSpaceUtil.h>
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
@@ -94,7 +93,7 @@ class Robot : public frc::TimedRobot {
|
||||
void TeleopPeriodic() override {
|
||||
// Sets the target speed of our flywheel. This is similar to setting the
|
||||
// setpoint of a PID controller.
|
||||
if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
|
||||
if (m_joystick.GetRightBumper()) {
|
||||
// We pressed the bumper, so let's set our next reference
|
||||
m_loop.SetNextR(frc::MakeMatrix<1, 1>(kSpinup.to<double>()));
|
||||
} else {
|
||||
|
||||
@@ -30,23 +30,20 @@ class Robot : public frc::TimedRobot {
|
||||
void DriveWithJoystick(bool fieldRelative) {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
const auto xSpeed = -m_xspeedLimiter.Calculate(
|
||||
m_controller.GetY(frc::GenericHID::kLeftHand)) *
|
||||
const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the y speed or sideways/strafe speed. We are inverting this because
|
||||
// we want a positive value when we pull to the left. Xbox controllers
|
||||
// return positive values when you pull to the right by default.
|
||||
const auto ySpeed = -m_yspeedLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kLeftHand)) *
|
||||
const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
const auto rot = -m_rotLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kRightHand)) *
|
||||
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
|
||||
Drivetrain::kMaxAngularSpeed;
|
||||
|
||||
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
|
||||
|
||||
@@ -32,13 +32,10 @@ RobotContainer::RobotContainer() {
|
||||
// Set up default drive command
|
||||
m_drive.SetDefaultCommand(frc2::RunCommand(
|
||||
[this] {
|
||||
m_drive.Drive(units::meters_per_second_t(
|
||||
m_driverController.GetY(frc::GenericHID::kLeftHand)),
|
||||
units::meters_per_second_t(
|
||||
m_driverController.GetY(frc::GenericHID::kRightHand)),
|
||||
units::radians_per_second_t(
|
||||
m_driverController.GetX(frc::GenericHID::kLeftHand)),
|
||||
false);
|
||||
m_drive.Drive(
|
||||
units::meters_per_second_t(m_driverController.GetLeftY()),
|
||||
units::meters_per_second_t(m_driverController.GetRightY()),
|
||||
units::radians_per_second_t(m_driverController.GetLeftX()), false);
|
||||
},
|
||||
{&m_drive}));
|
||||
}
|
||||
|
||||
@@ -30,23 +30,20 @@ class Robot : public frc::TimedRobot {
|
||||
void DriveWithJoystick(bool fieldRelative) {
|
||||
// Get the x speed. We are inverting this because Xbox controllers return
|
||||
// negative values when we push forward.
|
||||
const auto xSpeed = -m_xspeedLimiter.Calculate(
|
||||
m_controller.GetY(frc::GenericHID::kLeftHand)) *
|
||||
const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the y speed or sideways/strafe speed. We are inverting this because
|
||||
// we want a positive value when we pull to the left. Xbox controllers
|
||||
// return positive values when you pull to the right by default.
|
||||
const auto ySpeed = -m_yspeedLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kLeftHand)) *
|
||||
const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
|
||||
Drivetrain::kMaxSpeed;
|
||||
|
||||
// Get the rate of angular rotation. We are inverting this because we want a
|
||||
// positive value when we pull to the left (remember, CCW is positive in
|
||||
// mathematics). Xbox controllers return positive values when you pull to
|
||||
// the right by default.
|
||||
const auto rot = -m_rotLimiter.Calculate(
|
||||
m_controller.GetX(frc::GenericHID::kRightHand)) *
|
||||
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
|
||||
Drivetrain::kMaxAngularSpeed;
|
||||
|
||||
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
|
||||
|
||||
@@ -2,7 +2,6 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include <frc/GenericHID.h>
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc/drive/DifferentialDrive.h>
|
||||
@@ -28,9 +27,8 @@ class Robot : public frc::TimedRobot {
|
||||
|
||||
void TeleopPeriodic() override {
|
||||
// Drive with tank style
|
||||
m_robotDrive.TankDrive(
|
||||
m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
|
||||
m_driverController.GetY(frc::GenericHID::JoystickHand::kRightHand));
|
||||
m_robotDrive.TankDrive(m_driverController.GetLeftY(),
|
||||
m_driverController.GetRightY());
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user