diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java index 64f44d641a..5558f25ec8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java @@ -14,11 +14,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem; import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem; import static edu.wpi.first.wpilibj.XboxController.Button; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants.kDriverControllerPort; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -32,7 +32,7 @@ public class RobotContainer { private final ArmSubsystem m_robotArm = new ArmSubsystem(); // The driver's controller - XboxController m_driverController = new XboxController(kDriverControllerPort); + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); /** * The container for the robot. Contains subsystems, OI devices, and commands. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java index 85aafeade2..d9fd206c22 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java @@ -14,39 +14,28 @@ import edu.wpi.first.wpilibj.controller.ProfiledPIDController; import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kAVoltSecondSquaredPerRad; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kArmOffsetRads; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kCosVolts; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kEncoderPorts; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMaxAccelerationRadPerSecSquared; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMaxVelocityRadPerSecond; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kMotorPort; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kP; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kSVolts; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants.kVVoltSecondPerRad; +import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants; /** * A robot arm subsystem that moves with a motion profile. */ public class ArmSubsystem extends ProfiledPIDSubsystem { - private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort); - private final Encoder m_encoder = new Encoder(kEncoderPorts[0], kEncoderPorts[1]); + private final PWMVictorSPX m_motor = new PWMVictorSPX(ArmConstants.kMotorPort); + private final Encoder m_encoder = + new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]); private final ArmFeedforward m_feedforward = - new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad); + new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts, + ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad); /** * Create a new ArmSubsystem. */ public ArmSubsystem() { - super(new ProfiledPIDController(kP, 0, 0, - new TrapezoidProfile.Constraints( - kMaxVelocityRadPerSecond, - kMaxAccelerationRadPerSecSquared)), - 0); - m_encoder.setDistancePerPulse(kEncoderDistancePerPulse); + super(new ProfiledPIDController(ArmConstants.kP, 0, 0, new TrapezoidProfile.Constraints( + ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared)), 0); + m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse); // Start arm at rest in neutral position - setGoal(kArmOffsetRads); + setGoal(ArmConstants.kArmOffsetRads); } @Override @@ -59,6 +48,6 @@ public class ArmSubsystem extends ProfiledPIDSubsystem { @Override public double getMeasurement() { - return m_encoder.getDistance() + kArmOffsetRads; + return m_encoder.getDistance() + ArmConstants.kArmOffsetRads; } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java index a22079e63b..940401361f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java @@ -13,45 +13,39 @@ import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftEncoderPorts; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftEncoderReversed; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftMotor1Port; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kLeftMotor2Port; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightEncoderPorts; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightEncoderReversed; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightMotor1Port; -import static edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants.kRightMotor2Port; +import edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants; public class DriveSubsystem extends SubsystemBase { // The motors on the left side of the drive. private final SpeedControllerGroup m_leftMotors = - new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), - new PWMVictorSPX(kLeftMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port), + new PWMVictorSPX(DriveConstants.kLeftMotor2Port)); // The motors on the right side of the drive. private final SpeedControllerGroup m_rightMotors = - new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), - new PWMVictorSPX(kRightMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port), + new PWMVictorSPX(DriveConstants.kRightMotor2Port)); // The robot's drive private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); // The left-side drive encoder private final Encoder m_leftEncoder = - new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1], + DriveConstants.kLeftEncoderReversed); // The right-side drive encoder private final Encoder m_rightEncoder = - new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1], + DriveConstants.kRightEncoderReversed); /** * Creates a new DriveSubsystem. */ public DriveSubsystem() { // Sets the distance per pulse for the encoders - m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java index 285bd58746..6bca84aeff 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java @@ -14,11 +14,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem; import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem; import static edu.wpi.first.wpilibj.XboxController.Button; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants.kDriverControllerPort; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -32,7 +32,7 @@ public class RobotContainer { private final ArmSubsystem m_robotArm = new ArmSubsystem(); // The driver's controller - XboxController m_driverController = new XboxController(kDriverControllerPort); + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); /** * The container for the robot. Contains subsystems, OI devices, and commands. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java index 2d2698cb91..92fd43c925 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java @@ -11,34 +11,27 @@ import edu.wpi.first.wpilibj.controller.ArmFeedforward; import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; +import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants; import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kAVoltSecondSquaredPerRad; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kArmOffsetRads; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kCosVolts; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMaxAccelerationRadPerSecSquared; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMaxVelocityRadPerSecond; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kMotorPort; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kP; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kSVolts; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants.kVVoltSecondPerRad; - /** * A robot arm subsystem that moves with a motion profile. */ public class ArmSubsystem extends TrapezoidProfileSubsystem { - private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(kMotorPort); + private final ExampleSmartMotorController m_motor = + new ExampleSmartMotorController(ArmConstants.kMotorPort); private final ArmFeedforward m_feedforward = - new ArmFeedforward(kSVolts, kCosVolts, kVVoltSecondPerRad, kAVoltSecondSquaredPerRad); + new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts, + ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad); /** * Create a new ArmSubsystem. */ public ArmSubsystem() { - super(new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond, - kMaxAccelerationRadPerSecSquared), - kArmOffsetRads); - m_motor.setPID(kP, 0, 0); + super(new TrapezoidProfile.Constraints(ArmConstants.kMaxVelocityRadPerSecond, + ArmConstants.kMaxAccelerationRadPerSecSquared), + ArmConstants.kArmOffsetRads); + m_motor.setPID(ArmConstants.kP, 0, 0); } @Override @@ -46,8 +39,7 @@ public class ArmSubsystem extends TrapezoidProfileSubsystem { // Calculate the feedforward from the sepoint double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); // Add the feedforward to the PID output to get the motor output - m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, - setpoint.position, + m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java index 974f51467a..433dba4dad 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java @@ -13,45 +13,39 @@ import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftEncoderPorts; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftEncoderReversed; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftMotor1Port; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kLeftMotor2Port; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightEncoderPorts; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightEncoderReversed; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightMotor1Port; -import static edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants.kRightMotor2Port; +import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants; public class DriveSubsystem extends SubsystemBase { // The motors on the left side of the drive. private final SpeedControllerGroup m_leftMotors = - new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), - new PWMVictorSPX(kLeftMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port), + new PWMVictorSPX(DriveConstants.kLeftMotor2Port)); // The motors on the right side of the drive. private final SpeedControllerGroup m_rightMotors = - new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), - new PWMVictorSPX(kRightMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port), + new PWMVictorSPX(DriveConstants.kRightMotor2Port)); // The robot's drive private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); // The left-side drive encoder private final Encoder m_leftEncoder = - new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1], + DriveConstants.kLeftEncoderReversed); // The right-side drive encoder private final Encoder m_rightEncoder = - new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1], + DriveConstants.kRightEncoderReversed); /** * Creates a new DriveSubsystem. */ public DriveSubsystem() { // Sets the distance per pulse for the encoders - m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java index 10df8b6f39..f7ff493036 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java @@ -11,9 +11,6 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; -import static edu.wpi.first.wpilibj.examples.differentialdrivebot.Drivetrain.kMaxAngularSpeed; -import static edu.wpi.first.wpilibj.examples.differentialdrivebot.Drivetrain.kMaxSpeed; - public class Robot extends TimedRobot { private final XboxController m_controller = new XboxController(0); private final Drivetrain m_drive = new Drivetrain(); @@ -28,13 +25,13 @@ public class Robot extends TimedRobot { public void teleopPeriodic() { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. - final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed; + final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * 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. - final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed; + final var rot = -m_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed; m_drive.drive(xSpeed, rot); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java index a0eb86af3d..f618b9d5b5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java @@ -16,13 +16,12 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants; +import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled; import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem; import static edu.wpi.first.wpilibj.XboxController.Button; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxSpeedMetersPerSecond; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants.kDriverControllerPort; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -35,7 +34,7 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); // The driver's controller - XboxController m_driverController = new XboxController(kDriverControllerPort); + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -72,8 +71,9 @@ public class RobotContainer { new TrapezoidProfileCommand( new TrapezoidProfile( // Limit the max acceleration and velocity - new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond, - kMaxAccelerationMetersPerSecondSquared), + new TrapezoidProfile.Constraints( + DriveConstants.kMaxSpeedMetersPerSecond, + DriveConstants.kMaxAccelerationMetersPerSecondSquared), // End at desired position in meters; implicitly starts at 0 new TrapezoidProfile.State(3, 0)), // Pipe the profile state to the drive diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java index f89b36a9c9..280acd3bf5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java @@ -10,11 +10,9 @@ package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands; import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand; +import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants; import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kMaxSpeedMetersPerSecond; - /** * Drives a set distance using a motion profile. */ @@ -29,8 +27,8 @@ public class DriveDistanceProfiled extends TrapezoidProfileCommand { super( new TrapezoidProfile( // Limit the max acceleration and velocity - new TrapezoidProfile.Constraints(kMaxSpeedMetersPerSecond, - kMaxAccelerationMetersPerSecondSquared), + new TrapezoidProfile.Constraints(DriveConstants.kMaxSpeedMetersPerSecond, + DriveConstants.kMaxAccelerationMetersPerSecondSquared), // End at desired position in meters; implicitly starts at 0 new TrapezoidProfile.State(meters, 0)), // Pipe the profile state to the drive diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java index c8b98f7a0c..ac5510013d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java @@ -12,35 +12,27 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants; import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kLeftMotor1Port; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kLeftMotor2Port; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kRightMotor1Port; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kRightMotor2Port; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kaVoltSecondsSquaredPerMeter; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kp; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.ksVolts; -import static edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants.kvVoltSecondsPerMeter; - public class DriveSubsystem extends SubsystemBase { // The motors on the left side of the drive. private final ExampleSmartMotorController m_leftMaster = - new ExampleSmartMotorController(kLeftMotor1Port); + new ExampleSmartMotorController(DriveConstants.kLeftMotor1Port); private final ExampleSmartMotorController m_leftSlave = - new ExampleSmartMotorController(kLeftMotor2Port); + new ExampleSmartMotorController(DriveConstants.kLeftMotor2Port); private final ExampleSmartMotorController m_rightMaster = - new ExampleSmartMotorController(kRightMotor1Port); + new ExampleSmartMotorController(DriveConstants.kRightMotor1Port); private final ExampleSmartMotorController m_rightSlave = - new ExampleSmartMotorController(kRightMotor2Port); + new ExampleSmartMotorController(DriveConstants.kRightMotor2Port); private final SimpleMotorFeedforward m_feedforward = - new SimpleMotorFeedforward(ksVolts, - kvVoltSecondsPerMeter, - kaVoltSecondsSquaredPerMeter); + new SimpleMotorFeedforward(DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter); // The robot's drive private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMaster, m_rightMaster); @@ -52,8 +44,8 @@ public class DriveSubsystem extends SubsystemBase { m_leftSlave.follow(m_leftMaster); m_rightSlave.follow(m_rightMaster); - m_leftMaster.setPID(kp, 0, 0); - m_rightMaster.setPID(kp, 0, 0); + m_leftMaster.setPID(DriveConstants.kp, 0, 0); + m_rightMaster.setPID(DriveConstants.kp, 0, 0); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java index 69008ae8ef..957f11f02e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java @@ -17,13 +17,12 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants; +import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem; import static edu.wpi.first.wpilibj.XboxController.Button; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoShootTimeSeconds; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoTimeoutSeconds; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants.kDriverControllerPort; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -45,10 +44,10 @@ public class RobotContainer { // Start running the feeder new InstantCommand(m_shooter::runFeeder, m_shooter), // Shoot for the specified time - new WaitCommand(kAutoShootTimeSeconds)) + new WaitCommand(AutoConstants.kAutoShootTimeSeconds)) // Add a timeout (will end the command if, for instance, the shooter never gets up to // speed) - .withTimeout(kAutoTimeoutSeconds) + .withTimeout(AutoConstants.kAutoTimeoutSeconds) // When the command ends, turn off the shooter and the feeder .andThen(() -> { m_shooter.disable(); @@ -56,7 +55,7 @@ public class RobotContainer { }); // The driver's controller - XboxController m_driverController = new XboxController(kDriverControllerPort); + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -72,7 +71,7 @@ public class RobotContainer { // hand, and turning controlled by the right. new RunCommand(() -> m_robotDrive .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), - m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive)); + m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java index 192fa30e71..b82cb7548e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java @@ -13,45 +13,39 @@ import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderPorts; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderReversed; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor1Port; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor2Port; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderPorts; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderReversed; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor1Port; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor2Port; +import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants; public class DriveSubsystem extends SubsystemBase { // The motors on the left side of the drive. private final SpeedControllerGroup m_leftMotors = - new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), - new PWMVictorSPX(kLeftMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port), + new PWMVictorSPX(DriveConstants.kLeftMotor2Port)); // The motors on the right side of the drive. private final SpeedControllerGroup m_rightMotors = - new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), - new PWMVictorSPX(kRightMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port), + new PWMVictorSPX(DriveConstants.kRightMotor2Port)); // The robot's drive private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); // The left-side drive encoder private final Encoder m_leftEncoder = - new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1], + DriveConstants.kLeftEncoderReversed); // The right-side drive encoder private final Encoder m_rightEncoder = - new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1], + DriveConstants.kRightEncoderReversed); /** * Creates a new DriveSubsystem. */ public DriveSubsystem() { // Sets the distance per pulse for the encoders - m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java index c870114bb6..8b0f3cafd2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java @@ -13,36 +13,26 @@ import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.PIDSubsystem; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kD; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderPorts; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderReversed; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederMotorPort; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederSpeed; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kI; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kP; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSVolts; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterMotorPort; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterTargetRPS; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterToleranceRPS; -import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVVoltSecondsPerRotation; +import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants; public class ShooterSubsystem extends PIDSubsystem { - private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(kShooterMotorPort); - private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(kFeederMotorPort); + private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(ShooterConstants.kShooterMotorPort); + private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(ShooterConstants.kFeederMotorPort); private final Encoder m_shooterEncoder = - new Encoder(kEncoderPorts[0], kEncoderPorts[1], kEncoderReversed); - private final SimpleMotorFeedforward m_shooterFeedforward - = new SimpleMotorFeedforward(kSVolts, kVVoltSecondsPerRotation); + new Encoder(ShooterConstants.kEncoderPorts[0], ShooterConstants.kEncoderPorts[1], + ShooterConstants.kEncoderReversed); + private final SimpleMotorFeedforward m_shooterFeedforward = + new SimpleMotorFeedforward(ShooterConstants.kSVolts, + ShooterConstants.kVVoltSecondsPerRotation); /** * The shooter subsystem for the robot. */ public ShooterSubsystem() { - super(new PIDController(kP, kI, kD)); - getController().setTolerance(kShooterToleranceRPS); - m_shooterEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - setSetpoint(kShooterTargetRPS); + super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD)); + getController().setTolerance(ShooterConstants.kShooterToleranceRPS); + m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse); + setSetpoint(ShooterConstants.kShooterTargetRPS); } @Override @@ -60,7 +50,7 @@ public class ShooterSubsystem extends PIDSubsystem { } public void runFeeder() { - m_feederMotor.set(kFeederSpeed); + m_feederMotor.set(ShooterConstants.kFeederSpeed); } public void stopFeeder() { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java index 44b09b8cec..1b94feaf26 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java @@ -16,15 +16,13 @@ import edu.wpi.first.wpilibj2.command.PIDCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants; +import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle; import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngleProfiled; import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem; import static edu.wpi.first.wpilibj.XboxController.Button; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationD; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationI; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationP; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants.kDriverControllerPort; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -37,7 +35,7 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); // The driver's controller - XboxController m_driverController = new XboxController(kDriverControllerPort); + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -53,7 +51,7 @@ public class RobotContainer { // hand, and turning controlled by the right. new RunCommand(() -> m_robotDrive .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), - m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive)); + m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive)); } @@ -70,18 +68,17 @@ public class RobotContainer { .whenReleased(() -> m_robotDrive.setMaxOutput(1)); // Stabilize robot to drive straight with gyro when left bumper is held - new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld( - new PIDCommand( - new PIDController(kStabilizationP, kStabilizationI, kStabilizationD), - // Close the loop on the turn rate - m_robotDrive::getTurnRate, - // Setpoint is 0 - 0, - // Pipe the output to the turning controls - output -> m_robotDrive - .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output), - // Require the robot drive - m_robotDrive)); + new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(new PIDCommand( + new PIDController(DriveConstants.kStabilizationP, DriveConstants.kStabilizationI, + DriveConstants.kStabilizationD), + // Close the loop on the turn rate + m_robotDrive::getTurnRate, + // Setpoint is 0 + 0, + // Pipe the output to the turning controls + output -> m_robotDrive.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output), + // Require the robot drive + m_robotDrive)); // Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout new JoystickButton(m_driverController, Button.kX.value) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java index fe6c725999..6c8ea369cc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java @@ -10,14 +10,9 @@ package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands; import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj2.command.PIDCommand; +import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants; import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnD; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnI; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnP; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnRateToleranceDegPerS; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnToleranceDeg; - /** * A command that will turn the robot to the specified angle. */ @@ -29,7 +24,8 @@ public class TurnToAngle extends PIDCommand { * @param drive The drive subsystem to use */ public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) { - super(new PIDController(kTurnP, kTurnI, kTurnD), + super( + new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD), // Close loop on heading drive::getHeading, // Set reference to target @@ -43,7 +39,8 @@ public class TurnToAngle extends PIDCommand { getController().enableContinuousInput(-180, 180); // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the // setpoint before it is considered as having reached the reference - getController().setTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS); + getController() + .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java index 8fb4320da0..44f24d39cd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java @@ -11,16 +11,9 @@ import edu.wpi.first.wpilibj.controller.ProfiledPIDController; import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand; +import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants; import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kMaxTurnAccelerationDegPerSSquared; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kMaxTurnRateDegPerS; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnD; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnI; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnP; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnRateToleranceDegPerS; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnToleranceDeg; - /** * A command that will turn the robot to the specified angle using a motion profile. */ @@ -33,11 +26,10 @@ public class TurnToAngleProfiled extends ProfiledPIDCommand { */ public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) { super( - new ProfiledPIDController(kTurnP, kTurnI, kTurnD, - new TrapezoidProfile.Constraints( - kMaxTurnRateDegPerS, - kMaxTurnAccelerationDegPerSSquared - )), + new ProfiledPIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, + DriveConstants.kTurnD, new TrapezoidProfile.Constraints( + DriveConstants.kMaxTurnRateDegPerS, + DriveConstants.kMaxTurnAccelerationDegPerSSquared)), // Close loop on heading drive::getHeading, // Set reference to target @@ -51,7 +43,8 @@ public class TurnToAngleProfiled extends ProfiledPIDCommand { getController().enableContinuousInput(-180, 180); // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the // setpoint before it is considered as having reached the reference - getController().setTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS); + getController() + .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java index a4f001e902..7bdeb247bc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java @@ -15,38 +15,31 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kGyroReversed; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderPorts; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderReversed; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor1Port; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor2Port; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderPorts; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderReversed; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor1Port; -import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor2Port; +import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants; public class DriveSubsystem extends SubsystemBase { // The motors on the left side of the drive. private final SpeedControllerGroup m_leftMotors = - new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), - new PWMVictorSPX(kLeftMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port), + new PWMVictorSPX(DriveConstants.kLeftMotor2Port)); // The motors on the right side of the drive. private final SpeedControllerGroup m_rightMotors = - new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), - new PWMVictorSPX(kRightMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port), + new PWMVictorSPX(DriveConstants.kRightMotor2Port)); // The robot's drive private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); // The left-side drive encoder private final Encoder m_leftEncoder = - new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1], + DriveConstants.kLeftEncoderReversed); // The right-side drive encoder private final Encoder m_rightEncoder = - new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1], + DriveConstants.kRightEncoderReversed); // The gyro sensor private final Gyro m_gyro = new ADXRS450_Gyro(); @@ -56,8 +49,8 @@ public class DriveSubsystem extends SubsystemBase { */ public DriveSubsystem() { // Sets the distance per pulse for the encoders - m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); } /** @@ -127,7 +120,7 @@ public class DriveSubsystem extends SubsystemBase { * @return the robot's heading in degrees, from 180 to 180 */ public double getHeading() { - return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0); + return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0); } /** @@ -136,6 +129,6 @@ public class DriveSubsystem extends SubsystemBase { * @return The turn rate of the robot, in degrees per second */ public double getTurnRate() { - return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0); + return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java index a5c71295a0..225012357b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java @@ -17,14 +17,13 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants; +import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.ComplexAutoCommand; import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem; import static edu.wpi.first.wpilibj.XboxController.Button; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants.kDriverControllerPort; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -40,20 +39,18 @@ public class RobotContainer { // The autonomous routines // A simple auto routine that drives forward a specified distance, and then stops. - private final Command m_simpleAuto = - new StartEndCommand( - // Start driving forward at the start of the command - () -> m_robotDrive.arcadeDrive(kAutoDriveSpeed, 0), - // Stop driving at the end of the command - () -> m_robotDrive.arcadeDrive(0, 0), - // Requires the drive subsystem - m_robotDrive - ) - // Reset the encoders before starting - .beforeStarting(m_robotDrive::resetEncoders) - // End the command when the robot's driven distance exceeds the desired value - .withInterrupt(() -> m_robotDrive.getAverageEncoderDistance() - >= kAutoDriveDistanceInches); + private final Command m_simpleAuto = new StartEndCommand( + // Start driving forward at the start of the command + () -> m_robotDrive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0), + // Stop driving at the end of the command + () -> m_robotDrive.arcadeDrive(0, 0), + // Requires the drive subsystem + m_robotDrive) + // Reset the encoders before starting + .beforeStarting(m_robotDrive::resetEncoders) + // End the command when the robot's driven distance exceeds the desired value + .withInterrupt( + () -> m_robotDrive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches); // A complex auto routine that drives forward, drops a hatch, and then drives backward. private final Command m_complexAuto = new ComplexAutoCommand(m_robotDrive, m_hatchSubsystem); @@ -62,7 +59,7 @@ public class RobotContainer { SendableChooser m_chooser = new SendableChooser<>(); // The driver's controller - XboxController m_driverController = new XboxController(kDriverControllerPort); + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -76,11 +73,9 @@ public class RobotContainer { m_robotDrive.setDefaultCommand( // A split-stick arcade command, with forward/backward controlled by the left // hand, and turning controlled by the right. - new RunCommand(() -> m_robotDrive.arcadeDrive( - m_driverController.getY(GenericHID.Hand.kLeft), - m_driverController.getX(GenericHID.Hand.kRight)), - m_robotDrive) - ); + new RunCommand(() -> m_robotDrive + .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), + m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive)); // Add commands to the autonomous command chooser m_chooser.addOption("Simple Auto", m_simpleAuto); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java index f46bcf2548..45674da7e4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java @@ -11,13 +11,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants; import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoBackupDistanceInches; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed; - /** * A complex auto command that drives forward, releases a hatch, and then drives backward. */ @@ -33,29 +30,26 @@ public class ComplexAutoCommand extends SequentialCommandGroup { // Drive forward up to the front of the cargo ship new StartEndCommand( // Start driving forward at the start of the command - () -> driveSubsystem.arcadeDrive(kAutoDriveSpeed, 0), + () -> driveSubsystem.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0), // Stop driving at the end of the command - () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem - ) + () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem) // Reset the encoders before starting .beforeStarting(driveSubsystem::resetEncoders) // End the command when the robot's driven distance exceeds the desired value - .withInterrupt( - () -> driveSubsystem.getAverageEncoderDistance() >= kAutoDriveDistanceInches), + .withInterrupt(() -> driveSubsystem.getAverageEncoderDistance() + >= AutoConstants.kAutoDriveDistanceInches), // Release the hatch new InstantCommand(hatchSubsystem::releaseHatch, hatchSubsystem), // Drive backward the specified distance new StartEndCommand( - () -> driveSubsystem.arcadeDrive(-kAutoDriveSpeed, 0), - () -> driveSubsystem.arcadeDrive(0, 0), - driveSubsystem - ) + () -> driveSubsystem.arcadeDrive(-AutoConstants.kAutoDriveSpeed, 0), + () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem) .beforeStarting(driveSubsystem::resetEncoders) .withInterrupt( - () -> driveSubsystem.getAverageEncoderDistance() <= -kAutoBackupDistanceInches) - ); + () -> driveSubsystem.getAverageEncoderDistance() + <= -AutoConstants.kAutoBackupDistanceInches)); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java index 5361e87d71..5d16a445ab 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java @@ -13,45 +13,39 @@ import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderPorts; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderReversed; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor1Port; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor2Port; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderPorts; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderReversed; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor1Port; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor2Port; +import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants; public class DriveSubsystem extends SubsystemBase { // The motors on the left side of the drive. private final SpeedControllerGroup m_leftMotors = - new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), - new PWMVictorSPX(kLeftMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port), + new PWMVictorSPX(DriveConstants.kLeftMotor2Port)); // The motors on the right side of the drive. private final SpeedControllerGroup m_rightMotors = - new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), - new PWMVictorSPX(kRightMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port), + new PWMVictorSPX(DriveConstants.kRightMotor2Port)); // The robot's drive private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); // The left-side drive encoder private final Encoder m_leftEncoder = - new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1], + DriveConstants.kLeftEncoderReversed); // The right-side drive encoder private final Encoder m_rightEncoder = - new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1], + DriveConstants.kRightEncoderReversed); /** * Creates a new DriveSubsystem. */ public DriveSubsystem() { // Sets the distance per pulse for the encoders - m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java index 6dce9e1cd5..92e4979976 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java @@ -10,17 +10,18 @@ package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants; + import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidModule; -import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidPorts; /** * A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}. */ public class HatchSubsystem extends SubsystemBase { private final DoubleSolenoid m_hatchSolenoid = - new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]); + new DoubleSolenoid(HatchConstants.kHatchSolenoidModule, HatchConstants.kHatchSolenoidPorts[0], + HatchConstants.kHatchSolenoidPorts[1]); /** * Grabs the hatch. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java index f839768c0f..3e2dc825d3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java @@ -14,6 +14,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ComplexAuto; import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DefaultDrive; import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DriveDistance; @@ -24,9 +26,6 @@ import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsys import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem; import static edu.wpi.first.wpilibj.XboxController.Button; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants.kDriverControllerPort; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -43,7 +42,8 @@ public class RobotContainer { // A simple auto routine that drives forward a specified distance, and then stops. private final Command m_simpleAuto = - new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, m_robotDrive); + new DriveDistance(AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed, + m_robotDrive); // A complex auto routine that drives forward, drops a hatch, and then drives backward. private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem); @@ -52,7 +52,7 @@ public class RobotContainer { SendableChooser m_chooser = new SendableChooser<>(); // The driver's controller - XboxController m_driverController = new XboxController(kDriverControllerPort); + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -69,8 +69,7 @@ public class RobotContainer { new DefaultDrive( m_robotDrive, () -> m_driverController.getY(GenericHID.Hand.kLeft), - () -> m_driverController.getX(GenericHID.Hand.kRight)) - ); + () -> m_driverController.getX(GenericHID.Hand.kRight))); // Add commands to the autonomous command chooser m_chooser.addOption("Simple Auto", m_simpleAuto); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java index 9614922353..5f776a514a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java @@ -9,13 +9,10 @@ package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants; import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoBackupDistanceInches; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed; - /** * A complex auto command that drives forward, releases a hatch, and then drives backward. */ @@ -29,14 +26,15 @@ public class ComplexAuto extends SequentialCommandGroup { public ComplexAuto(DriveSubsystem drive, HatchSubsystem hatch) { addCommands( // Drive forward the specified distance - new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, drive), + new DriveDistance(AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed, + drive), // Release the hatch new ReleaseHatch(hatch), // Drive backward the specified distance - new DriveDistance(kAutoBackupDistanceInches, -kAutoDriveSpeed, drive) - ); + new DriveDistance(AutoConstants.kAutoBackupDistanceInches, -AutoConstants.kAutoDriveSpeed, + drive)); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java index a068b56f13..f49abb95b1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java @@ -13,45 +13,39 @@ import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftEncoderPorts; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftEncoderReversed; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftMotor1Port; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftMotor2Port; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightEncoderPorts; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightEncoderReversed; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightMotor1Port; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightMotor2Port; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants; public class DriveSubsystem extends SubsystemBase { // The motors on the left side of the drive. private final SpeedControllerGroup m_leftMotors = - new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), - new PWMVictorSPX(kLeftMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port), + new PWMVictorSPX(DriveConstants.kLeftMotor2Port)); // The motors on the right side of the drive. private final SpeedControllerGroup m_rightMotors = - new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), - new PWMVictorSPX(kRightMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port), + new PWMVictorSPX(DriveConstants.kRightMotor2Port)); // The robot's drive private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); // The left-side drive encoder private final Encoder m_leftEncoder = - new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1], + DriveConstants.kLeftEncoderReversed); // The right-side drive encoder private final Encoder m_rightEncoder = - new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1], + DriveConstants.kRightEncoderReversed); /** * Creates a new DriveSubsystem. */ public DriveSubsystem() { // Sets the distance per pulse for the encoders - m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java index e93fea46f3..47da829a23 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java @@ -10,17 +10,18 @@ package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants; + import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants.kHatchSolenoidModule; -import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants.kHatchSolenoidPorts; /** * A hatch mechanism actuated by a single {@link DoubleSolenoid}. */ public class HatchSubsystem extends SubsystemBase { private final DoubleSolenoid m_hatchSolenoid = - new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]); + new DoubleSolenoid(HatchConstants.kHatchSolenoidModule, HatchConstants.kHatchSolenoidPorts[0], + HatchConstants.kHatchSolenoidPorts[1]); /** * Grabs the hatch. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java index b7540d3c15..aeb4681c66 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java @@ -11,9 +11,6 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; -import static edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed; -import static edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed; - public class Robot extends TimedRobot { private final XboxController m_controller = new XboxController(0); private final Drivetrain m_mecanum = new Drivetrain(); @@ -32,18 +29,18 @@ public class Robot extends TimedRobot { private void driveWithJoystick(boolean fieldRelative) { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. - final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed; + final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * 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. - final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * kMaxSpeed; + final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * 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. - final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed; + final var rot = -m_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed; m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java index 54707e37fc..1ec1fc9ca8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java @@ -25,22 +25,11 @@ import edu.wpi.first.wpilibj2.command.MecanumControllerCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants; +import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants; +import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPThetaController; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPXController; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kPYController; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants.kThetaControllerConstraints; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kDriveKinematics; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFeedforward; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPFrontLeftVel; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPFrontRightVel; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPRearLeftVel; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kPRearRightVel; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants.kDriverControllerPort; - /* * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -53,7 +42,7 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); // The driver's controller - XboxController m_driverController = new XboxController(kDriverControllerPort); + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -96,9 +85,10 @@ public class RobotContainer { public Command getAutonomousCommand() { // Create config for trajectory TrajectoryConfig config = - new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared) + new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) // Add kinematics to ensure max speed is actually obeyed - .setKinematics(kDriveKinematics); + .setKinematics(DriveConstants.kDriveKinematics); // An example trajectory to follow. All units in meters. Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( @@ -107,7 +97,7 @@ public class RobotContainer { // Pass through these two interior waypoints, making an 's' curve path List.of( new Translation2d(1, 1), - new Translation2d(2, - 1) + new Translation2d(2, -1) ), // End 3 meters straight ahead of where we started, facing forward new Pose2d(3, 0, new Rotation2d(0)), @@ -118,22 +108,23 @@ public class RobotContainer { exampleTrajectory, m_robotDrive::getPose, - kFeedforward, - kDriveKinematics, + DriveConstants.kFeedforward, + DriveConstants.kDriveKinematics, //Position contollers - new PIDController(kPXController, 0, 0), - new PIDController(kPYController, 0, 0), - new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints), + new PIDController(AutoConstants.kPXController, 0, 0), + new PIDController(AutoConstants.kPYController, 0, 0), + new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, + AutoConstants.kThetaControllerConstraints), //Needed for normalizing wheel speeds - kMaxSpeedMetersPerSecond, + AutoConstants.kMaxSpeedMetersPerSecond, //Velocity PID's - new PIDController(kPFrontLeftVel, 0, 0), - new PIDController(kPRearLeftVel, 0, 0), - new PIDController(kPFrontRightVel, 0, 0), - new PIDController(kPRearRightVel, 0, 0), + new PIDController(DriveConstants.kPFrontLeftVel, 0, 0), + new PIDController(DriveConstants.kPRearLeftVel, 0, 0), + new PIDController(DriveConstants.kPFrontRightVel, 0, 0), + new PIDController(DriveConstants.kPRearRightVel, 0, 0), m_robotDrive::getCurrentWheelSpeeds, diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java index b68f770558..7aaf114ab3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java @@ -19,70 +19,60 @@ import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry; import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kDriveKinematics; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftEncoderPorts; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftEncoderReversed; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontLeftMotorPort; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightEncoderPorts; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightEncoderReversed; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kFrontRightMotorPort; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kGyroReversed; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftEncoderPorts; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftEncoderReversed; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearLeftMotorPort; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightEncoderPorts; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightEncoderReversed; -import static edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants.kRearRightMotorPort; +import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants; public class DriveSubsystem extends SubsystemBase { - private final PWMVictorSPX m_frontLeft = new PWMVictorSPX(kFrontLeftMotorPort); - private final PWMVictorSPX m_rearLeft = new PWMVictorSPX(kRearLeftMotorPort); - private final PWMVictorSPX m_frontRight = new PWMVictorSPX(kFrontRightMotorPort); - private final PWMVictorSPX m_rearRight = new PWMVictorSPX(kRearRightMotorPort); + private final PWMVictorSPX m_frontLeft = new PWMVictorSPX(DriveConstants.kFrontLeftMotorPort); + private final PWMVictorSPX m_rearLeft = new PWMVictorSPX(DriveConstants.kRearLeftMotorPort); + private final PWMVictorSPX m_frontRight = new PWMVictorSPX(DriveConstants.kFrontRightMotorPort); + private final PWMVictorSPX m_rearRight = new PWMVictorSPX(DriveConstants.kRearRightMotorPort); private final MecanumDrive m_drive = new MecanumDrive( - m_frontLeft, - m_rearLeft, - m_frontRight, - m_rearRight); + m_frontLeft, + m_rearLeft, + m_frontRight, + m_rearRight); // The front-left-side drive encoder private final Encoder m_frontLeftEncoder = - new Encoder(kFrontLeftEncoderPorts[0], kFrontLeftEncoderPorts[1], - kFrontLeftEncoderReversed); + new Encoder(DriveConstants.kFrontLeftEncoderPorts[0], + DriveConstants.kFrontLeftEncoderPorts[1], + DriveConstants.kFrontLeftEncoderReversed); // The rear-left-side drive encoder private final Encoder m_rearLeftEncoder = - new Encoder(kRearLeftEncoderPorts[0], kRearLeftEncoderPorts[1], - kRearLeftEncoderReversed); + new Encoder(DriveConstants.kRearLeftEncoderPorts[0], + DriveConstants.kRearLeftEncoderPorts[1], + DriveConstants.kRearLeftEncoderReversed); // The front-right--side drive encoder private final Encoder m_frontRightEncoder = - new Encoder(kFrontRightEncoderPorts[0], kFrontRightEncoderPorts[1], - kFrontRightEncoderReversed); + new Encoder(DriveConstants.kFrontRightEncoderPorts[0], + DriveConstants.kFrontRightEncoderPorts[1], + DriveConstants.kFrontRightEncoderReversed); // The rear-right-side drive encoder private final Encoder m_rearRightEncoder = - new Encoder(kRearRightEncoderPorts[0], kRearRightEncoderPorts[1], - kRearRightEncoderReversed); + new Encoder(DriveConstants.kRearRightEncoderPorts[0], + DriveConstants.kRearRightEncoderPorts[1], + DriveConstants.kRearRightEncoderReversed); // The gyro sensor private final Gyro m_gyro = new ADXRS450_Gyro(); // Odometry class for tracking robot pose MecanumDriveOdometry m_odometry = - new MecanumDriveOdometry(kDriveKinematics, getAngle()); + new MecanumDriveOdometry(DriveConstants.kDriveKinematics, getAngle()); /** * Creates a new DriveSubsystem. */ public DriveSubsystem() { // Sets the distance per pulse for the encoders - m_frontLeftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - m_rearLeftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - m_frontRightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - m_rearRightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_frontLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rearLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_frontRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rearRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); } /** @@ -92,18 +82,18 @@ public class DriveSubsystem extends SubsystemBase { */ public Rotation2d getAngle() { // Negating the angle because WPILib gyros are CW positive. - return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0)); + return Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? 1.0 : -1.0)); } @Override public void periodic() { // Update the odometry in the periodic block m_odometry.update(getAngle(), - new MecanumDriveWheelSpeeds( - m_frontLeftEncoder.getRate(), - m_rearLeftEncoder.getRate(), - m_frontRightEncoder.getRate(), - m_rearRightEncoder.getRate())); + new MecanumDriveWheelSpeeds( + m_frontLeftEncoder.getRate(), + m_rearLeftEncoder.getRate(), + m_frontRightEncoder.getRate(), + m_rearRightEncoder.getRate())); } /** @@ -135,7 +125,7 @@ public class DriveSubsystem extends SubsystemBase { */ @SuppressWarnings("ParameterName") public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - if ( fieldRelative ) { + if (fieldRelative) { m_drive.driveCartesian(ySpeed, xSpeed, rot, -m_gyro.getAngle()); } else { m_drive.driveCartesian(ySpeed, xSpeed, rot); @@ -144,8 +134,8 @@ public class DriveSubsystem extends SubsystemBase { } /** - * Sets the front left drive SpeedController to a voltage. - */ + * Sets the front left drive SpeedController to a voltage. + */ public void setDriveSpeedControllersVolts(MecanumDriveMotorVoltages volts) { m_frontLeft.setVoltage(volts.frontLeftVoltage); m_rearLeft.setVoltage(volts.rearLeftVoltage); @@ -193,6 +183,7 @@ public class DriveSubsystem extends SubsystemBase { public Encoder getFrontRightEncoder() { return m_frontRightEncoder; } + /** * Gets the rear right drive encoder. * @@ -211,9 +202,9 @@ public class DriveSubsystem extends SubsystemBase { public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() { return new MecanumDriveWheelSpeeds(m_frontLeftEncoder.getRate(), - m_rearLeftEncoder.getRate(), - m_frontRightEncoder.getRate(), - m_rearRightEncoder.getRate()); + m_rearLeftEncoder.getRate(), + m_frontRightEncoder.getRate(), + m_rearRightEncoder.getRate()); } @@ -239,7 +230,7 @@ public class DriveSubsystem extends SubsystemBase { * @return the robot's heading in degrees, from 180 to 180 */ public double getHeading() { - return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0); + return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0); } /** @@ -248,6 +239,6 @@ public class DriveSubsystem extends SubsystemBase { * @return The turn rate of the robot, in degrees per second */ public double getTurnRate() { - return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0); + return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java index c575e29d2f..8bb4e1d1ae 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java @@ -26,19 +26,12 @@ import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants; +import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants; +import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems.DriveSubsystem; import static edu.wpi.first.wpilibj.XboxController.Button; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kRamseteB; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants.kRamseteZeta; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kPDriveVel; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kaVoltSecondsSquaredPerMeter; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.ksVolts; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kvVoltSecondsPerMeter; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants.kDriverControllerPort; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -51,7 +44,7 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); // The driver's controller - XboxController m_driverController = new XboxController(kDriverControllerPort); + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -96,17 +89,18 @@ public class RobotContainer { // Create a voltage constraint to ensure we don't accelerate too fast var autoVoltageConstraint = new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward(ksVolts, - kvVoltSecondsPerMeter, - kaVoltSecondsSquaredPerMeter), - kDriveKinematics, + new SimpleMotorFeedforward(DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, 10); // Create config for trajectory TrajectoryConfig config = - new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared) + new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) // Add kinematics to ensure max speed is actually obeyed - .setKinematics(kDriveKinematics) + .setKinematics(DriveConstants.kDriveKinematics) // Apply the voltage constraint .addConstraint(autoVoltageConstraint); @@ -128,12 +122,14 @@ public class RobotContainer { RamseteCommand ramseteCommand = new RamseteCommand( exampleTrajectory, m_robotDrive::getPose, - new RamseteController(kRamseteB, kRamseteZeta), - new SimpleMotorFeedforward(ksVolts, kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter), - kDriveKinematics, + new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta), + new SimpleMotorFeedforward(DriveConstants.ksVolts, + DriveConstants.kvVoltSecondsPerMeter, + DriveConstants.kaVoltSecondsSquaredPerMeter), + DriveConstants.kDriveKinematics, m_robotDrive::getWheelSpeeds, - new PIDController(kPDriveVel, 0, 0), - new PIDController(kPDriveVel, 0, 0), + new PIDController(DriveConstants.kPDriveVel, 0, 0), + new PIDController(DriveConstants.kPDriveVel, 0, 0), // RamseteCommand passes volts to the callback m_robotDrive::tankDriveVolts, m_robotDrive diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java index ecb640c19c..7b5c5e50e2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java @@ -19,38 +19,31 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kGyroReversed; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderPorts; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderReversed; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftMotor1Port; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftMotor2Port; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightEncoderPorts; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightEncoderReversed; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightMotor1Port; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kRightMotor2Port; +import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants; public class DriveSubsystem extends SubsystemBase { // The motors on the left side of the drive. private final SpeedControllerGroup m_leftMotors = - new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), - new PWMVictorSPX(kLeftMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port), + new PWMVictorSPX(DriveConstants.kLeftMotor2Port)); // The motors on the right side of the drive. private final SpeedControllerGroup m_rightMotors = - new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), - new PWMVictorSPX(kRightMotor2Port)); + new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port), + new PWMVictorSPX(DriveConstants.kRightMotor2Port)); // The robot's drive private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); // The left-side drive encoder private final Encoder m_leftEncoder = - new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1], + DriveConstants.kLeftEncoderReversed); // The right-side drive encoder private final Encoder m_rightEncoder = - new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1], + DriveConstants.kRightEncoderReversed); // The gyro sensor private final Gyro m_gyro = new ADXRS450_Gyro(); @@ -63,8 +56,8 @@ public class DriveSubsystem extends SubsystemBase { */ public DriveSubsystem() { // Sets the distance per pulse for the encoders - m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); - m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); resetEncoders(); m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading())); @@ -74,7 +67,7 @@ public class DriveSubsystem extends SubsystemBase { public void periodic() { // Update the odometry in the periodic block m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(), - m_rightEncoder.getDistance()); + m_rightEncoder.getDistance()); } /** @@ -183,7 +176,7 @@ public class DriveSubsystem extends SubsystemBase { * @return the robot's heading in degrees, from 180 to 180 */ public double getHeading() { - return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0); + return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0); } /** @@ -192,6 +185,6 @@ public class DriveSubsystem extends SubsystemBase { * @return The turn rate of the robot, in degrees per second */ public double getTurnRate() { - return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0); + return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java index 922b8ff8d0..570a0ad6a9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java @@ -18,8 +18,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants; + import static edu.wpi.first.wpilibj.XboxController.Button; -import static edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants.kDriverControllerPort; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -29,7 +30,8 @@ import static edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIC */ public class RobotContainer { // The driver's controller - private final XboxController m_driverController = new XboxController(kDriverControllerPort); + private final XboxController m_driverController = + new XboxController(OIConstants.kDriverControllerPort); // A few commands that do nothing, but will demonstrate the scheduler functionality private final CommandBase m_instantCommand1 = new InstantCommand(); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java index ff9f28b26d..35afdf3d06 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java @@ -11,9 +11,6 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; -import static edu.wpi.first.wpilibj.examples.swervebot.Drivetrain.kMaxAngularSpeed; -import static edu.wpi.first.wpilibj.examples.swervebot.Drivetrain.kMaxSpeed; - public class Robot extends TimedRobot { private final XboxController m_controller = new XboxController(0); private final Drivetrain m_swerve = new Drivetrain(); @@ -32,18 +29,18 @@ public class Robot extends TimedRobot { private void driveWithJoystick(boolean fieldRelative) { // Get the x speed. We are inverting this because Xbox controllers return // negative values when we push forward. - final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed; + final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * 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. - final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * kMaxSpeed; + final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * 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. - final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed; + final var rot = -m_controller.getX(GenericHID.Hand.kRight) * Drivetrain.kMaxAngularSpeed; m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java index 19fc601050..706b15e84e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java @@ -74,6 +74,7 @@ public final class Constants { public static final double kvVoltSecondsPerMeter = 0.8; public static final double kaVoltSecondsSquaredPerMeter = 0.15; + public static final double kMaxSpeedMetersPerSecond = 3; } public static final class ModuleConstants { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java index d0fcc71c32..503cede410 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java @@ -24,17 +24,11 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants; +import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants; +import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPThetaController; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPXController; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kPYController; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kThetaControllerConstraints; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kDriveKinematics; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants.kDriverControllerPort; - /* * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -46,7 +40,7 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); // The driver's controller - XboxController m_driverController = new XboxController(kDriverControllerPort); + XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -85,9 +79,10 @@ public class RobotContainer { public Command getAutonomousCommand() { // Create config for trajectory TrajectoryConfig config = - new TrajectoryConfig(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared) + new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond, + AutoConstants.kMaxAccelerationMetersPerSecondSquared) // Add kinematics to ensure max speed is actually obeyed - .setKinematics(kDriveKinematics); + .setKinematics(DriveConstants.kDriveKinematics); // An example trajectory to follow. All units in meters. Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( @@ -96,7 +91,7 @@ public class RobotContainer { // Pass through these two interior waypoints, making an 's' curve path List.of( new Translation2d(1, 1), - new Translation2d(2, - 1) + new Translation2d(2, -1) ), // End 3 meters straight ahead of where we started, facing forward new Pose2d(3, 0, new Rotation2d(0)), @@ -106,12 +101,13 @@ public class RobotContainer { SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( exampleTrajectory, m_robotDrive::getPose, //Functional interface to feed supplier - kDriveKinematics, + DriveConstants.kDriveKinematics, //Position controllers - new PIDController(kPXController, 0, 0), - new PIDController(kPYController, 0, 0), - new ProfiledPIDController(kPThetaController, 0, 0, kThetaControllerConstraints), + new PIDController(AutoConstants.kPXController, 0, 0), + new PIDController(AutoConstants.kPYController, 0, 0), + new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, + AutoConstants.kThetaControllerConstraints), m_robotDrive::setModuleStates, diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index ba80a15fe6..5d9cf56357 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -17,77 +17,56 @@ import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry; import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants.kMaxSpeedMetersPerSecond; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kDriveKinematics; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveEncoderPorts; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveEncoderReversed; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftDriveMotorPort; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningEncoderPorts; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningEncoderReversed; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontLeftTurningMotorPort; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveEncoderPorts; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveEncoderReversed; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightDriveMotorPort; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningEncoderPorts; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningEncoderReversed; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kFrontRightTurningMotorPort; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kGyroReversed; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveEncoderPorts; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveEncoderReversed; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftDriveMotorPort; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningEncoderPorts; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningEncoderReversed; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearLeftTurningMotorPort; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveEncoderPorts; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveEncoderReversed; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightDriveMotorPort; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningEncoderPorts; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningEncoderReversed; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants.kRearRightTurningMotorPort; +import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants; @SuppressWarnings("PMD.ExcessiveImports") public class DriveSubsystem extends SubsystemBase { //Robot swerve modules - private final SwerveModule m_frontLeft = new SwerveModule(kFrontLeftDriveMotorPort, - kFrontLeftTurningMotorPort, - kFrontLeftDriveEncoderPorts, - kFrontLeftTurningEncoderPorts, - kFrontLeftDriveEncoderReversed, - kFrontLeftTurningEncoderReversed); + private final SwerveModule m_frontLeft + = new SwerveModule(DriveConstants.kFrontLeftDriveMotorPort, + DriveConstants.kFrontLeftTurningMotorPort, + DriveConstants.kFrontLeftDriveEncoderPorts, + DriveConstants.kFrontLeftTurningEncoderPorts, + DriveConstants.kFrontLeftDriveEncoderReversed, + DriveConstants.kFrontLeftTurningEncoderReversed); - private final SwerveModule m_rearLeft = new SwerveModule(kRearLeftDriveMotorPort, - kRearLeftTurningMotorPort, - kRearLeftDriveEncoderPorts, - kRearLeftTurningEncoderPorts, - kRearLeftDriveEncoderReversed, - kRearLeftTurningEncoderReversed); + private final SwerveModule m_rearLeft = + new SwerveModule(DriveConstants.kRearLeftDriveMotorPort, + DriveConstants.kRearLeftTurningMotorPort, + DriveConstants.kRearLeftDriveEncoderPorts, + DriveConstants.kRearLeftTurningEncoderPorts, + DriveConstants.kRearLeftDriveEncoderReversed, + DriveConstants.kRearLeftTurningEncoderReversed); - private final SwerveModule m_frontRight = new SwerveModule(kFrontRightDriveMotorPort, - kFrontRightTurningMotorPort, - kFrontRightDriveEncoderPorts, - kFrontRightTurningEncoderPorts, - kFrontRightDriveEncoderReversed, - kFrontRightTurningEncoderReversed); + private final SwerveModule m_frontRight = + new SwerveModule(DriveConstants.kFrontRightDriveMotorPort, + DriveConstants.kFrontRightTurningMotorPort, + DriveConstants.kFrontRightDriveEncoderPorts, + DriveConstants.kFrontRightTurningEncoderPorts, + DriveConstants.kFrontRightDriveEncoderReversed, + DriveConstants.kFrontRightTurningEncoderReversed); - private final SwerveModule m_rearRight = new SwerveModule(kRearRightDriveMotorPort, - kRearRightTurningMotorPort, - kRearRightDriveEncoderPorts, - kRearRightTurningEncoderPorts, - kRearRightDriveEncoderReversed, - kRearRightTurningEncoderReversed); + private final SwerveModule m_rearRight = + new SwerveModule(DriveConstants.kRearRightDriveMotorPort, + DriveConstants.kRearRightTurningMotorPort, + DriveConstants.kRearRightDriveEncoderPorts, + DriveConstants.kRearRightTurningEncoderPorts, + DriveConstants.kRearRightDriveEncoderReversed, + DriveConstants.kRearRightTurningEncoderReversed); // The gyro sensor private final Gyro m_gyro = new ADXRS450_Gyro(); // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = - new SwerveDriveOdometry(kDriveKinematics, getAngle()); + new SwerveDriveOdometry(DriveConstants.kDriveKinematics, getAngle()); /** * Creates a new DriveSubsystem. */ - public DriveSubsystem() {} + public DriveSubsystem() { + } /** * Returns the angle of the robot as a Rotation2d. @@ -96,7 +75,7 @@ public class DriveSubsystem extends SubsystemBase { */ public Rotation2d getAngle() { // Negating the angle because WPILib gyros are CW positive. - return Rotation2d.fromDegrees(m_gyro.getAngle() * (kGyroReversed ? 1.0 : -1.0)); + return Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? 1.0 : -1.0)); } @Override @@ -138,12 +117,13 @@ public class DriveSubsystem extends SubsystemBase { */ @SuppressWarnings("ParameterName") public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { - var swerveModuleStates = kDriveKinematics.toSwerveModuleStates( + var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( xSpeed, ySpeed, rot, getAngle()) : new ChassisSpeeds(xSpeed, ySpeed, rot) ); - SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeedMetersPerSecond); + SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, + DriveConstants.kMaxSpeedMetersPerSecond); m_frontLeft.setDesiredState(swerveModuleStates[0]); m_frontRight.setDesiredState(swerveModuleStates[1]); m_rearLeft.setDesiredState(swerveModuleStates[2]); @@ -151,12 +131,13 @@ public class DriveSubsystem extends SubsystemBase { } /** - * Sets the swerve ModuleStates. - * - * @param desiredStates The desired SwerveModule states. - */ + * Sets the swerve ModuleStates. + * + * @param desiredStates The desired SwerveModule states. + */ public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.normalizeWheelSpeeds(desiredStates, kMaxSpeedMetersPerSecond); + SwerveDriveKinematics.normalizeWheelSpeeds(desiredStates, + DriveConstants.kMaxSpeedMetersPerSecond); m_frontLeft.setDesiredState(desiredStates[0]); m_frontRight.setDesiredState(desiredStates[1]); m_rearLeft.setDesiredState(desiredStates[2]); @@ -186,7 +167,7 @@ public class DriveSubsystem extends SubsystemBase { * @return the robot's heading in degrees, from 180 to 180 */ public double getHeading() { - return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1.0 : 1.0); + return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0); } /** @@ -195,6 +176,6 @@ public class DriveSubsystem extends SubsystemBase { * @return The turn rate of the robot, in degrees per second */ public double getTurnRate() { - return m_gyro.getRate() * (kGyroReversed ? -1.0 : 1.0); + return m_gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java index bf33288157..e70c6ba8cd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java @@ -15,12 +15,7 @@ import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kDriveEncoderDistancePerPulse; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kPModuleDriveController; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kPModuleTurningController; -import static edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants.kTurningEncoderDistancePerPulse; +import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants; public class SwerveModule { private final Spark m_driveMotor; @@ -30,13 +25,15 @@ public class SwerveModule { private final Encoder m_turningEncoder; private final PIDController m_drivePIDController = - new PIDController(kPModuleDriveController, 0, 0); + new PIDController(ModuleConstants.kPModuleDriveController, 0, 0); //Using a TrapezoidProfile PIDController to allow for smooth turning private final ProfiledPIDController m_turningPIDController - = new ProfiledPIDController(kPModuleTurningController, 0, 0, - new TrapezoidProfile.Constraints(kMaxModuleAngularSpeedRadiansPerSecond, - kMaxModuleAngularAccelerationRadiansPerSecondSquared)); + = new ProfiledPIDController( + ModuleConstants.kPModuleTurningController, 0, 0, + new TrapezoidProfile.Constraints( + ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond, + ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared)); /** * Constructs a SwerveModule. @@ -45,11 +42,11 @@ public class SwerveModule { * @param turningMotorChannel ID for the turning motor. */ public SwerveModule(int driveMotorChannel, - int turningMotorChannel, - int[] driveEncoderPorts, - int[] turningEncoderPorts, - boolean driveEncoderReversed, - boolean turningEncoderReversed) { + int turningMotorChannel, + int[] driveEncoderPorts, + int[] turningEncoderPorts, + boolean driveEncoderReversed, + boolean turningEncoderReversed) { m_driveMotor = new Spark(driveMotorChannel); m_turningMotor = new Spark(turningMotorChannel); @@ -61,7 +58,7 @@ public class SwerveModule { // Set the distance per pulse for the drive encoder. We can simply use the // distance traveled for one rotation of the wheel divided by the encoder // resolution. - m_driveEncoder.setDistancePerPulse(kDriveEncoderDistancePerPulse); + m_driveEncoder.setDistancePerPulse(ModuleConstants.kDriveEncoderDistancePerPulse); //Set whether drive encoder should be reversed or not m_driveEncoder.setReverseDirection(driveEncoderReversed); @@ -69,7 +66,7 @@ public class SwerveModule { // Set the distance (in this case, angle) per pulse for the turning encoder. // This is the the angle through an entire rotation (2 * wpi::math::pi) // divided by the encoder resolution. - m_turningEncoder.setDistancePerPulse(kTurningEncoderDistancePerPulse); + m_turningEncoder.setDistancePerPulse(ModuleConstants.kTurningEncoderDistancePerPulse); //Set whether turning encoder should be reversed or not m_turningEncoder.setReverseDirection(turningEncoderReversed);