From a63d06ff77e41a66e92ae9a4cd9dc68acf2ba0e8 Mon Sep 17 00:00:00 2001 From: Josiah Hamm <93680829+bakedPotatoLord@users.noreply.github.com> Date: Fri, 28 Apr 2023 21:56:14 -0600 Subject: [PATCH] [examples] Add constants to java gearsbot example (#5248) --- .../wpilibj/examples/gearsbot/Constants.java | 94 +++++++++++++++++++ .../gearsbot/commands/Autonomous.java | 8 +- .../gearsbot/commands/DriveStraight.java | 7 +- .../examples/gearsbot/commands/Pickup.java | 4 +- .../examples/gearsbot/commands/Place.java | 5 +- .../gearsbot/commands/PrepareToPickup.java | 5 +- .../examples/gearsbot/subsystems/Claw.java | 5 +- .../gearsbot/subsystems/Drivetrain.java | 29 ++++-- .../gearsbot/subsystems/Elevator.java | 31 +++--- .../examples/gearsbot/subsystems/Wrist.java | 16 ++-- 10 files changed, 163 insertions(+), 41 deletions(-) create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Constants.java diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Constants.java new file mode 100644 index 0000000000..6f80067769 --- /dev/null +++ b/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; + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java index 44a7ce7d60..63b55f680f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java +++ b/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))); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java index ed47cc1c90..2430b61c3c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java +++ b/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); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java index 4e6eafd4ca..78f887956d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java +++ b/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))); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java index 442744c4f8..fad6de5a4d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java +++ b/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)); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java index 3c0661de24..d9ae53ca61 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java +++ b/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))); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java index d34d12a26e..06c627e480 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java +++ b/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() { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java index 44f455d062..ffde63d402 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java +++ b/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); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java index e174595a60..b3eba1fda7 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java +++ b/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 diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java index e11e2d748f..d13df6419e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java +++ b/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