From 27c8204651eda5817504d6f6e4cf5e77bfe22cc2 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 5 Jan 2024 11:59:01 -0800 Subject: [PATCH] feat: everything works :partyparrot: --- .../team1540/advantagekitdemo/Constants.java | 9 ++- .../org/team1540/advantagekitdemo/Robot.java | 2 + .../advantagekitdemo/RobotContainer.java | 25 ++++++--- .../commands/IntakeCommand.java | 36 ++++++++++++ .../subsystems/climber/Climber.java | 5 +- .../subsystems/climber/ClimberIO.java | 4 +- .../subsystems/climber/ClimberIOSim.java | 12 ++-- .../subsystems/elevator/ElevatorIOReal.java | 55 +++++++++++++++++++ .../subsystems/intake/Intake.java | 7 ++- .../util/SuperstructureVisualizer.java | 19 +++++-- 10 files changed, 150 insertions(+), 24 deletions(-) create mode 100644 src/main/java/org/team1540/advantagekitdemo/commands/IntakeCommand.java create mode 100644 src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIOReal.java diff --git a/src/main/java/org/team1540/advantagekitdemo/Constants.java b/src/main/java/org/team1540/advantagekitdemo/Constants.java index 2c6e26a..40714f0 100644 --- a/src/main/java/org/team1540/advantagekitdemo/Constants.java +++ b/src/main/java/org/team1540/advantagekitdemo/Constants.java @@ -30,6 +30,10 @@ public enum SimulationMode { public static final double DEADZONE_RADIUS = 0.1; + public static final class SimConstants { + public static final double OFFSET_METERS = 0.022225; + } + public static final class DrivetrainConstants { public static final int FL_ID = 1; public static final int BL_ID = 2; @@ -59,6 +63,9 @@ public static final class DrivetrainConstants { } public static final class ElevatorConstants { + public static final int LEADER_ID = 5; + public static final int FOLLOWER_ID = 6; + public static final double GEAR_RATIO = 4.21; public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(0.75); public static final double MAX_HEIGHT_METERS = Units.inchesToMeters(80); @@ -82,7 +89,7 @@ public static final class IntakeConstants { public static final double INTAKE_MASS_KG = Units.lbsToKilograms(8); public static final double INTAKE_MOI = INTAKE_MASS_KG * INTAKE_LENGTH_METERS * INTAKE_LENGTH_METERS / 3; - public static final double WRIST_KP = 1; + public static final double WRIST_KP = 50; public static final double WRIST_KI = 0; public static final double WRIST_KD = 0; public static final TrapezoidProfile.Constraints MOTION_CONSTRAINTS = diff --git a/src/main/java/org/team1540/advantagekitdemo/Robot.java b/src/main/java/org/team1540/advantagekitdemo/Robot.java index 1fc00aa..9f63ca8 100644 --- a/src/main/java/org/team1540/advantagekitdemo/Robot.java +++ b/src/main/java/org/team1540/advantagekitdemo/Robot.java @@ -13,6 +13,7 @@ package org.team1540.advantagekitdemo; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.littletonrobotics.junction.LogFileUtil; @@ -83,6 +84,7 @@ public void robotInit() { // Start AdvantageKit logger Logger.start(); + DriverStation.silenceJoystickConnectionWarning(true); // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. diff --git a/src/main/java/org/team1540/advantagekitdemo/RobotContainer.java b/src/main/java/org/team1540/advantagekitdemo/RobotContainer.java index 023615f..aa31525 100644 --- a/src/main/java/org/team1540/advantagekitdemo/RobotContainer.java +++ b/src/main/java/org/team1540/advantagekitdemo/RobotContainer.java @@ -13,21 +13,20 @@ package org.team1540.advantagekitdemo; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import org.team1540.advantagekitdemo.commands.ArcadeDriveCommand; -import org.team1540.advantagekitdemo.commands.ElevatorManualCommand; -import org.team1540.advantagekitdemo.commands.ElevatorSetpointCommand; -import org.team1540.advantagekitdemo.commands.WristManualCommand; +import org.team1540.advantagekitdemo.commands.*; import org.team1540.advantagekitdemo.commands.auto.TestAuto; import org.team1540.advantagekitdemo.subsystems.drivetrain.Drivetrain; import org.team1540.advantagekitdemo.subsystems.drivetrain.DrivetrainIOReal; import org.team1540.advantagekitdemo.subsystems.drivetrain.DrivetrainIOSim; import org.team1540.advantagekitdemo.subsystems.elevator.Elevator; -import org.team1540.advantagekitdemo.subsystems.elevator.ElevatorIO; +import org.team1540.advantagekitdemo.subsystems.elevator.ElevatorIOReal; import org.team1540.advantagekitdemo.subsystems.elevator.ElevatorIOSim; import org.team1540.advantagekitdemo.subsystems.intake.Intake; import org.team1540.advantagekitdemo.subsystems.intake.IntakeIO; @@ -56,7 +55,7 @@ public class RobotContainer { public RobotContainer() { if (Robot.isReal()) { drivetrain = new Drivetrain(new DrivetrainIOReal()); - elevator = new Elevator(new ElevatorIO() {}); + elevator = new Elevator(new ElevatorIOReal()); intake = new Intake(new IntakeIO() {}); climber = new Climber(new ClimberIO() {}); } else { @@ -76,8 +75,8 @@ public RobotContainer() { */ private void configureButtonBindings() { drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver)); - intake.setDefaultCommand(new WristManualCommand(intake, copilot)); - elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); +// intake.setDefaultCommand(new WristManualCommand(intake, copilot)); +// elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot)); copilot.rightBumper().onTrue(new ElevatorSetpointCommand(elevator, 1.5)); copilot.leftBumper().onTrue(new ElevatorSetpointCommand(elevator, 0)); @@ -94,6 +93,14 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new TestAuto(drivetrain); + return Commands.parallel( + new TestAuto(drivetrain), + new IntakeCommand(intake, Rotation2d.fromDegrees(180), 0), + new ElevatorSetpointCommand(elevator, 1.8) + ).andThen(new InstantCommand(() -> { + System.out.println("setting climbers"); + climber.setForksStowed(false); + climber.setHangerStowed(false); + })); } } diff --git a/src/main/java/org/team1540/advantagekitdemo/commands/IntakeCommand.java b/src/main/java/org/team1540/advantagekitdemo/commands/IntakeCommand.java new file mode 100644 index 0000000..921ad35 --- /dev/null +++ b/src/main/java/org/team1540/advantagekitdemo/commands/IntakeCommand.java @@ -0,0 +1,36 @@ +package org.team1540.advantagekitdemo.commands; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import org.team1540.advantagekitdemo.subsystems.intake.Intake; + +import static org.team1540.advantagekitdemo.Constants.IntakeConstants.*; + +public class IntakeCommand extends Command { + private final Intake intake; + private final Rotation2d wristSetpoint; + private final double intakePercent; + + private final PIDController pid = new PIDController(WRIST_KP, WRIST_KI, WRIST_KD); + private final ArmFeedforward ff = new ArmFeedforward(WRIST_KS, WRIST_KG, WRIST_KV); + + public IntakeCommand(Intake intake, Rotation2d wristSetpoint, double intakePercent) { + this.intake = intake; + this.wristSetpoint = wristSetpoint; + this.intakePercent = intakePercent; + addRequirements(intake); + } + + @Override + public void initialize() { + intake.setWristPosition(wristSetpoint); + intake.setIntakePercent(intakePercent); + } + + @Override + public boolean isFinished() { + return intake.getProfilePosition() == wristSetpoint.getRotations(); + } +} diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/Climber.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/Climber.java index bd8901c..3bf8847 100644 --- a/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/Climber.java +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/Climber.java @@ -20,16 +20,17 @@ public void periodic() { } public void setHangerStowed(boolean hangerStowed){ - io.setPosition(hangerStowed, getForksStowed()); + io.setHanger(hangerStowed); } public void setForksStowed(boolean forksStowed){ - io.setPosition(getHangerStowed(), forksStowed); + io.setForks(forksStowed); } public boolean getHangerStowed(){ return inputs.hangerStowed; } + public boolean getForksStowed(){ return inputs.forksStowed; } diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/ClimberIO.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/ClimberIO.java index 535da35..ec2bc23 100644 --- a/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/ClimberIO.java +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/ClimberIO.java @@ -11,5 +11,7 @@ class ClimberIOInputs{ default void updateInputs(ClimberIOInputs inputs) {} - default void setPosition(boolean hangerStowed, boolean forksStowed){} + + default void setForks(boolean stowed) {} + default void setHanger(boolean stowed) {} } diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/ClimberIOSim.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/ClimberIOSim.java index 618ce03..dbf6a99 100644 --- a/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/climber/ClimberIOSim.java @@ -1,7 +1,7 @@ package org.team1540.advantagekitdemo.subsystems.climber; public class ClimberIOSim implements ClimberIO{ - private boolean hangerStowed = false; + private boolean hangerStowed = true; private boolean forksStowed = true; @Override @@ -11,8 +11,12 @@ public void updateInputs(ClimberIOInputs inputs) { } @Override - public void setPosition(boolean hangerStowed, boolean forksStowed) { - this.hangerStowed = hangerStowed; - this.forksStowed = forksStowed; + public void setForks(boolean stowed) { + forksStowed = stowed; + } + + @Override + public void setHanger(boolean stowed) { + hangerStowed = stowed; } } diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIOReal.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIOReal.java new file mode 100644 index 0000000..f92446c --- /dev/null +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/elevator/ElevatorIOReal.java @@ -0,0 +1,55 @@ +package org.team1540.advantagekitdemo.subsystems.elevator; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.ForwardLimitValue; +import com.ctre.phoenix6.signals.ReverseLimitValue; +import org.team1540.advantagekitdemo.util.Conversions; + +import static org.team1540.advantagekitdemo.Constants.ElevatorConstants.*; + +public class ElevatorIOReal implements ElevatorIO { + private final TalonFX leader = new TalonFX(LEADER_ID); + private final TalonFX follower = new TalonFX(FOLLOWER_ID); + + private final StatusSignal position = leader.getPosition(); + private final StatusSignal voltage = leader.getMotorVoltage(); + private final StatusSignal current = leader.getSupplyCurrent(); + private final StatusSignal lowerLimit = leader.getReverseLimit(); + private final StatusSignal upperLimit = leader.getForwardLimit(); + + public ElevatorIOReal() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.CurrentLimits.SupplyCurrentLimit = 40; + config.CurrentLimits.SupplyCurrentThreshold = 60; + config.CurrentLimits.SupplyTimeThreshold = 0.1; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + + config.HardwareLimitSwitch.ForwardLimitEnable = true; + config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = 0; + config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable = true; + config.HardwareLimitSwitch.ReverseLimitEnable = true; + + follower.setControl(new Follower(LEADER_ID, true)); + } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + BaseStatusSignal.refreshAll(position, voltage, current); + + inputs.positionMeters = Conversions.motorRotsToMeters(position.getValue(), DRUM_RADIUS_METERS * 2, GEAR_RATIO); + inputs.appliedVolts = voltage.getValue(); + inputs.currentAmps = voltage.getValue(); + inputs.lowerLimit = lowerLimit.getValue() == ReverseLimitValue.ClosedToGround; + inputs.upperLimit = upperLimit.getValue() == ForwardLimitValue.ClosedToGround; + } + + @Override + public void setVoltage(double volts) { + leader.setControl(new VoltageOut(volts)); + } +} diff --git a/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/Intake.java b/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/Intake.java index 1379080..41f8143 100644 --- a/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/Intake.java +++ b/src/main/java/org/team1540/advantagekitdemo/subsystems/intake/Intake.java @@ -54,13 +54,18 @@ public void stopIntake() { } public void stopWrist() { - setWristPercent(0); + if (RobotState.isDisabled()) setWristPercent(0); + else setWristPosition(getWristPosition()); } public Rotation2d getWristPosition() { return inputs.wristPosition; } + public double getProfilePosition() { + return getController().getSetpoint().position; + } + @Override protected void useOutput(double output, TrapezoidProfile.State setpoint) { setWristVoltage(output + wristFeedforward.calculate(getWristPosition().getRadians(), setpoint.velocity)); diff --git a/src/main/java/org/team1540/advantagekitdemo/util/SuperstructureVisualizer.java b/src/main/java/org/team1540/advantagekitdemo/util/SuperstructureVisualizer.java index 163919b..9b5592d 100644 --- a/src/main/java/org/team1540/advantagekitdemo/util/SuperstructureVisualizer.java +++ b/src/main/java/org/team1540/advantagekitdemo/util/SuperstructureVisualizer.java @@ -3,10 +3,11 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; import org.littletonrobotics.junction.Logger; import org.team1540.advantagekitdemo.Constants.*; +import static org.team1540.advantagekitdemo.Constants.SimConstants.OFFSET_METERS; + public class SuperstructureVisualizer { private static Pose3d elevatorStage1 = new Pose3d(); private static Pose3d elevatorCarriage = new Pose3d(); @@ -22,13 +23,19 @@ public static void periodic() { public static void setElevatorPosition(double positionMeters) { if (positionMeters <= ElevatorConstants.STAGE_1_HEIGHT_METERS) { - elevatorStage1 = new Pose3d(); + elevatorStage1 = + new Pose3d( + 0.0, + 0.0, + OFFSET_METERS, + new Rotation3d() + ); } else { elevatorStage1 = new Pose3d( 0.0, 0.0, - positionMeters - ElevatorConstants.STAGE_1_HEIGHT_METERS, + positionMeters - ElevatorConstants.STAGE_1_HEIGHT_METERS + OFFSET_METERS, new Rotation3d() ); } @@ -40,7 +47,7 @@ public static void setWristPosition(Rotation2d wristPosition) { wrist = new Pose3d( 0.0 + 0.228600, 0.0, - elevatorPosition + 0.193675, + elevatorPosition + 0.193675 + OFFSET_METERS, new Rotation3d(0, wristPosition.getRadians(), 0) ); } @@ -49,12 +56,12 @@ public static void setClimberPosition(boolean hangerStowed, boolean forksStowed) forks = new Pose3d( 0 - 0.215900, 0, - 0 + 0.155575, + 0 + 0.155575 + OFFSET_METERS, new Rotation3d(0, Rotation2d.fromDegrees(forksStowed?0:-90).getRadians(), 0)); hanger = new Pose3d( 0.0 - 0.292100, 0.0, - (elevatorPosition <= ElevatorConstants.STAGE_1_HEIGHT_METERS ? 0 : (elevatorPosition - ElevatorConstants.STAGE_1_HEIGHT_METERS)) + 1.374775, + (elevatorPosition <= ElevatorConstants.STAGE_1_HEIGHT_METERS ? OFFSET_METERS : (elevatorPosition - ElevatorConstants.STAGE_1_HEIGHT_METERS)) + 1.374775, new Rotation3d(0, Rotation2d.fromDegrees(hangerStowed?0:90).getRadians(), 0)); }