Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Java examples: use non-static imports for constants (#2191)

Browse Source
This commit is contained in:
Oblarg
2019-12-29 15:55:49 -05:00
committed by Peter Johnson
parent c7a1dfc0bc
commit 44bcf7fb4d
36 changed files with 380 additions and 543 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
View File

@@ -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.

33
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
View File

@@ -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;
}
}

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
View File

@@ -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);
}
/**

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
View File

@@ -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.

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
View File

@@ -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);
}
}

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
View File

@@ -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);
}
/**

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
View File

@@ -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);
}

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
View File

@@ -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

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
View File

@@ -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

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
View File

@@ -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);
}
/**

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
View File

@@ -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));
}

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
View File

@@ -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);
}
/**

36
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
View File

@@ -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() {

33
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
View File

@@ -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)

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
View File

@@ -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

21
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
View File

@@ -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

33
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
View File

@@ -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);
}
}

41
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
View File

@@ -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<Command> 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);

24
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
View File

@@ -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));
}
}

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
View File

@@ -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);
}
/**

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
View File

@@ -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.

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
View File

@@ -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<Command> 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);

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
View File

@@ -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));
}
}

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
View File

@@ -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);
}
/**

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
View File

@@ -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.

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
View File

@@ -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);
}

47
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
View File

@@ -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,

91
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -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);
}
}

40
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
View File

@@ -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

35
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
View File

@@ -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);
}
}

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
View File

@@ -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();

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
View File

@@ -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);
}

1
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
View File

@@ -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 {

30
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
View File

@@ -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,

107
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -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);
}
}

31
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
View File

@@ -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);