[examples] Add constants to java gearsbot example (#5248)

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Constants.java

@@ -0,0 +1,94 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.gearsbot;
+
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotorPort1 = 0;
+    public static final int kLeftMotorPort2 = 1;
+
+    public static final int kRightMotorPort1 = 2;
+    public static final int kRightMotorPort2 = 3;
+
+    public static final int[] kLeftEncoderPorts = {0, 1};
+    public static final int[] kRightEncoderPorts = {2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = false;
+
+    public static final int kRangeFinderPort = 6;
+    public static final int kAnalogGyroPort = 1;
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterInches = 6;
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR;
+  }
+
+  public static final class ClawConstants {
+    public static final int kMotorPort = 7;
+    public static final int kContactPort = 5;
+  }
+
+  public static final class WristConstants {
+    public static final int kMotorPort = 6;
+
+    // these pid constants are not real, and will need to be tuned
+    public static final double kP = 0.1;
+    public static final double kI = 0.0;
+    public static final double kD = 0.0;
+
+    public static final double kTolerance = 2.5;
+
+    public static final int kPotentiometerPort = 3;
+  }
+
+  public static final class ElevatorConstants {
+    public static final int kMotorPort = 5;
+    public static final int kPotentiometerPort = 2;
+
+    // these pid constants are not real, and will need to be tuned
+    public static final double kP_real = 4;
+    public static final double kI_real = 0.007;
+
+    public static final double kP_simulation = 18;
+    public static final double kI_simulation = 0.2;
+
+    public static final double kD = 0.0;
+
+    public static final double kTolerance = 0.005;
+  }
+
+  public static final class AutoConstants {
+    public static final double kDistToBox1 = 0.10;
+    public static final double kDistToBox2 = 0.60;
+
+    public static final double kWristSetpoint = -45.0;
+  }
+
+  public static final class DriveStraightConstants {
+    // these pid constants are not real, and will need to be tuned
+    public static final double kP = 4.0;
+    public static final double kI = 0.0;
+    public static final double kD = 0.0;
+  }
+
+  public static final class Positions {
+    public static final class Pickup {
+      public static final double kWristSetpoint = -45.0;
+      public static final double kElevatorSetpoint = 0.25;
+    }
+
+    public static final class Place {
+      public static final double kWristSetpoint = 0.0;
+      public static final double kElevatorSetpoint = 0.25;
+    }
+
+    public static final class PrepareToPickup {
+      public static final double kWristSetpoint = 0.0;
+      public static final double kElevatorSetpoint = 0.0;
+    }
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
+import edu.wpi.first.wpilibj.examples.gearsbot.Constants.AutoConstants;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
@@ -18,11 +19,12 @@ public class Autonomous extends SequentialCommandGroup {
     addCommands(
         new PrepareToPickup(claw, wrist, elevator),
         new Pickup(claw, wrist, elevator),
-        new SetDistanceToBox(0.10, drive),
+        new SetDistanceToBox(AutoConstants.kDistToBox1, drive),
         // new DriveStraight(4), // Use encoders if ultrasonic is broken
         new Place(claw, wrist, elevator),
-        new SetDistanceToBox(0.60, drive),
+        new SetDistanceToBox(AutoConstants.kDistToBox2, drive),
         // new DriveStraight(-2), // Use Encoders if ultrasonic is broken
-        Commands.parallel(new SetWristSetpoint(-45, wrist), new CloseClaw(claw)));
+        Commands.parallel(
+            new SetWristSetpoint(AutoConstants.kWristSetpoint, wrist), new CloseClaw(claw)));
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java

@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.examples.gearsbot.Constants.DriveStraightConstants;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
 
@@ -23,7 +24,11 @@ public class DriveStraight extends PIDCommand {
    */
   public DriveStraight(double distance, Drivetrain drivetrain) {
     super(
-        new PIDController(4, 0, 0), drivetrain::getDistance, distance, d -> drivetrain.drive(d, d));
+        new PIDController(
+            DriveStraightConstants.kP, DriveStraightConstants.kI, DriveStraightConstants.kD),
+        drivetrain::getDistance,
+        distance,
+        d -> drivetrain.drive(d, d));
 
     m_drivetrain = drivetrain;
     addRequirements(m_drivetrain);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
+import edu.wpi.first.wpilibj.examples.gearsbot.Constants.Positions;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
@@ -25,6 +26,7 @@ public class Pickup extends SequentialCommandGroup {
     addCommands(
         new CloseClaw(claw),
         Commands.parallel(
-            new SetWristSetpoint(-45, wrist), new SetElevatorSetpoint(0.25, elevator)));
+            new SetWristSetpoint(Positions.Pickup.kWristSetpoint, wrist),
+            new SetElevatorSetpoint(Positions.Pickup.kElevatorSetpoint, elevator)));
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
+import edu.wpi.first.wpilibj.examples.gearsbot.Constants.Positions;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
@@ -20,8 +21,8 @@ public class Place extends SequentialCommandGroup {
    */
   public Place(Claw claw, Wrist wrist, Elevator elevator) {
     addCommands(
-        new SetElevatorSetpoint(0.25, elevator),
-        new SetWristSetpoint(0, wrist),
+        new SetElevatorSetpoint(Positions.Place.kElevatorSetpoint, elevator),
+        new SetWristSetpoint(Positions.Place.kWristSetpoint, wrist),
         new OpenClaw(claw));
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
+import edu.wpi.first.wpilibj.examples.gearsbot.Constants.Positions;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
@@ -22,6 +23,8 @@ public class PrepareToPickup extends SequentialCommandGroup {
   public PrepareToPickup(Claw claw, Wrist wrist, Elevator elevator) {
     addCommands(
         new OpenClaw(claw),
-        Commands.parallel(new SetWristSetpoint(0, wrist), new SetElevatorSetpoint(0, elevator)));
+        Commands.parallel(
+            new SetWristSetpoint(Positions.PrepareToPickup.kWristSetpoint, wrist),
+            new SetElevatorSetpoint(Positions.PrepareToPickup.kElevatorSetpoint, elevator)));
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java

@@ -5,6 +5,7 @@
 package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
 
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.examples.gearsbot.Constants.ClawConstants;
 import edu.wpi.first.wpilibj.motorcontrol.Victor;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -14,8 +15,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
  * motors, you should probably use a sensor so that the motors don't stall.
  */
 public class Claw extends SubsystemBase {
-  private final Victor m_motor = new Victor(7);
-  private final DigitalInput m_contact = new DigitalInput(5);
+  private final Victor m_motor = new Victor(ClawConstants.kMotorPort);
+  private final DigitalInput m_contact = new DigitalInput(ClawConstants.kContactPort);
 
   /** Create a new claw subsystem. */
   public Claw() {

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java

@@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.gearsbot.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
@@ -21,17 +22,29 @@ public class Drivetrain extends SubsystemBase {
    * These include four drive motors, a left and right encoder and a gyro.
    */
   private final MotorController m_leftMotor =
-      new MotorControllerGroup(new PWMSparkMax(0), new PWMSparkMax(1));
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kLeftMotorPort1),
+          new PWMSparkMax(DriveConstants.kLeftMotorPort1));
 
   private final MotorController m_rightMotor =
-      new MotorControllerGroup(new PWMSparkMax(2), new PWMSparkMax(3));
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kRightMotorPort2),
+          new PWMSparkMax(DriveConstants.kLeftMotorPort2));
 
   private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotor, m_rightMotor);
 
-  private final Encoder m_leftEncoder = new Encoder(1, 2);
-  private final Encoder m_rightEncoder = new Encoder(3, 4);
-  private final AnalogInput m_rangefinder = new AnalogInput(6);
-  private final AnalogGyro m_gyro = new AnalogGyro(1);
+  private final Encoder m_leftEncoder =
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
+  private final Encoder m_rightEncoder =
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
+  private final AnalogInput m_rangefinder = new AnalogInput(DriveConstants.kRangeFinderPort);
+  private final AnalogGyro m_gyro = new AnalogGyro(DriveConstants.kAnalogGyroPort);
 
   /** Create a new drivetrain subsystem. */
   public Drivetrain() {
@@ -48,8 +61,8 @@ public class Drivetrain extends SubsystemBase {
     // simulate 360 tick encoders. This if statement allows for the
     // real robot to handle this difference in devices.
     if (Robot.isReal()) {
-      m_leftEncoder.setDistancePerPulse(0.042);
-      m_rightEncoder.setDistancePerPulse(0.042);
+      m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+      m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     } else {
       // Circumference = diameter in feet * pi. 360 tick simulated encoders.
       m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.examples.gearsbot.Constants.ElevatorConstants;
 import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
 import edu.wpi.first.wpilibj.motorcontrol.Victor;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -16,30 +17,32 @@ import edu.wpi.first.wpilibj2.command.PIDSubsystem;
  * values for simulation are different than in the real world do to minor differences.
  */
 public class Elevator extends PIDSubsystem {
-  private final Victor m_motor;
+  private final Victor m_motor = new Victor(ElevatorConstants.kMotorPort);
   private final AnalogPotentiometer m_pot;
 
-  private static final double kP_real = 4;
-  private static final double kI_real = 0.07;
-  private static final double kP_simulation = 18;
-  private static final double kI_simulation = 0.2;
-
   /** Create a new elevator subsystem. */
   public Elevator() {
-    super(new PIDController(kP_real, kI_real, 0));
-    if (Robot.isSimulation()) { // Check for simulation and update PID values
-      getController().setPID(kP_simulation, kI_simulation, 0);
-    }
-    getController().setTolerance(0.005);
+    super(
+        new PIDController(
+            ElevatorConstants.kP_real, ElevatorConstants.kI_real, ElevatorConstants.kD));
 
-    m_motor = new Victor(5);
+    if (Robot.isSimulation()) { // Check for simulation and update PID values
+      getController()
+          .setPID(
+              ElevatorConstants.kP_simulation,
+              ElevatorConstants.kI_simulation,
+              ElevatorConstants.kD);
+    }
+    getController().setTolerance(ElevatorConstants.kTolerance);
 
     // Conversion value of potentiometer varies between the real world and
     // simulation
+
     if (Robot.isReal()) {
-      m_pot = new AnalogPotentiometer(2, -2.0 / 5);
+      m_pot = new AnalogPotentiometer(ElevatorConstants.kPotentiometerPort, -2.0 / 5);
     } else {
-      m_pot = new AnalogPotentiometer(2); // Defaults to meters
+      // Defaults to meters
+      m_pot = new AnalogPotentiometer(ElevatorConstants.kPotentiometerPort);
     }
 
     // Let's name everything on the LiveWindow

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.examples.gearsbot.Constants.WristConstants;
 import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
 import edu.wpi.first.wpilibj.motorcontrol.Victor;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -15,24 +16,21 @@ import edu.wpi.first.wpilibj2.command.PIDSubsystem;
  * The wrist subsystem is like the elevator, but with a rotational joint instead of a linear joint.
  */
 public class Wrist extends PIDSubsystem {
-  private final Victor m_motor;
+  private final Victor m_motor = new Victor(WristConstants.kMotorPort);
   private final AnalogPotentiometer m_pot;
 
-  private static final double kP = 1;
-
   /** Create a new wrist subsystem. */
   public Wrist() {
-    super(new PIDController(kP, 0, 0));
-    getController().setTolerance(2.5);
-
-    m_motor = new Victor(6);
+    super(new PIDController(WristConstants.kP, WristConstants.kI, WristConstants.kD));
+    getController().setTolerance(WristConstants.kTolerance);
 
     // Conversion value of potentiometer varies between the real world and
     // simulation
     if (Robot.isReal()) {
-      m_pot = new AnalogPotentiometer(3, -270.0 / 5);
+      m_pot = new AnalogPotentiometer(WristConstants.kPotentiometerPort, -270.0 / 5);
     } else {
-      m_pot = new AnalogPotentiometer(3); // Defaults to degrees
+      // Defaults to degrees
+      m_pot = new AnalogPotentiometer(WristConstants.kPotentiometerPort);
     }
 
     // Let's name everything on the LiveWindow