diff --git a/build.gradle b/build.gradle index 1e81e373..035e7648 100644 --- a/build.gradle +++ b/build.gradle @@ -112,7 +112,7 @@ task(generateDriveTrajectories, dependsOn: "classes", type: JavaExec) { mainClass = "org.littletonrobotics.frc2024.subsystems.drive.trajectory.GenerateTrajectories" classpath = sourceSets.main.runtimeClasspath } -compileJava.finalizedBy generateDriveTrajectories +//compileJava.finalizedBy generateDriveTrajectories // Check robot type when deploying task(checkRobot, dependsOn: "classes", type: JavaExec) { diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index f08dde90..a38755aa 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -19,7 +19,6 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization; -import org.littletonrobotics.frc2024.commands.auto.AutoCommands; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVision; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionConstants; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO; @@ -124,12 +123,19 @@ public RobotContainer() { new ModuleIOSim(DriveConstants.moduleConfigs[2]), new ModuleIOSim(DriveConstants.moduleConfigs[3])); flywheels = new Flywheels(new FlywheelsIOSim()); + + feeder = new Feeder(new FeederIOSim()); + indexer = new Indexer(new IndexerIOSim()); + intake = new Intake(new IntakeIOSim()); + rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {}); + arm = new Arm(new ArmIOSim()); + superstructure = new Superstructure(arm); } } } - // No implementation for replay + // No-op implementation for replay if (drive == null) { drive = new Drive( @@ -167,9 +173,6 @@ public RobotContainer() { } superstructure = new Superstructure(arm); - if (superstructure == null) { - superstructure = new Superstructure(arm); - } // Configure autos and buttons configureAutos(); configureButtonBindings(); @@ -177,8 +180,8 @@ public RobotContainer() { private void configureAutos() { autoChooser.addDefaultOption("Do Nothing", Commands.none()); - AutoCommands autoCommands = new AutoCommands(drive, superstructure); - autoChooser.addOption("Drive Straight", autoCommands.driveStraight()); + // AutoCommands autoCommands = new AutoCommands(drive, superstructure); + // autoChooser.addOption("Drive Straight", autoCommands.driveStraight()); // Set up feedforward characterization autoChooser.addOption( @@ -202,18 +205,20 @@ private void configureAutos() { private void configureButtonBindings() { // Drive Commands drive.setDefaultCommand( - drive.run( - () -> - drive.setTeleopDriveGoal( - -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX()))); + drive + .run( + () -> + drive.setTeleopDriveGoal( + -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX())) + .withName("Drive Teleop Input")); - // Aim + // Aim and Rev Flywheels controller .a() - .onTrue(Commands.runOnce(drive::setAutoAimGoal)) - .onFalse(Commands.runOnce(drive::clearAutoAimGoal)); - controller.a().whileTrue(Commands.parallel(flywheels.shoot(), superstructure.aim())); - + .whileTrue( + Commands.startEnd(drive::setAutoAimGoal, drive::clearAutoAimGoal) + .alongWith(superstructure.aim(), flywheels.shoot()) + .withName("Aim")); // Shoot Trigger readyToShoot = new Trigger( @@ -228,22 +233,26 @@ private void configureButtonBindings() { .whileFalse( Commands.run( () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0))); - controller.rightTrigger().and(readyToShoot).whileTrue(rollers.feedShooter()); - + controller + .rightTrigger() + .and(readyToShoot) + .onTrue(rollers.feedShooter().withTimeout(1.0).withName("Shoot")); // Intake Floor controller .leftTrigger() .whileTrue( - Commands.parallel( - superstructure.intake(), - Commands.waitUntil(superstructure::atGoal).andThen(rollers.floorIntake()))); + superstructure + .intake() + .alongWith(rollers.floorIntake().onlyIf(superstructure::atGoal)) + .withName("Floor Intake")); // Eject Floor controller .leftBumper() .whileTrue( - Commands.parallel( - superstructure.intake(), - Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor()))); + superstructure + .intake() + .alongWith(rollers.ejectFloor().onlyIf(superstructure::atGoal)) + .withName("Eject Floor")); controller .y() diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index 8cbbe6c2..883bc4e0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.DoubleSupplier; import lombok.Getter; @@ -52,7 +51,7 @@ public enum Goal { IDLING(idleLeftRpm, idleRightRpm), SHOOTING(shootingLeftRpm, shootingRightRpm), INTAKING(intakingRpm, intakingRpm), - EJECTing(ejectingRpm, ejectingRpm), + EJECTING(ejectingRpm, ejectingRpm), CHARACTERIZING(() -> 0.0, () -> 0.0); private final DoubleSupplier leftSetpoint; @@ -122,28 +121,25 @@ public boolean atGoal() { } public Command stop() { - return runOnce(() -> setGoal(Goal.STOP)).andThen(Commands.idle()).withName("Flywheels Stop"); + return runOnce(() -> setGoal(Goal.STOP)).withName("Flywheels Stop"); } public Command idle() { - return runOnce(() -> setGoal(Goal.IDLING)).andThen(Commands.idle()).withName("Flywheels Idle"); + return runOnce(() -> setGoal(Goal.IDLING)).withName("Flywheels Idle"); } public Command shoot() { return startEnd(() -> setGoal(Goal.SHOOTING), () -> setGoal(Goal.IDLING)) - .andThen(Commands.idle()) .withName("Flywheels Shooting"); } public Command intake() { return startEnd(() -> setGoal(Goal.INTAKING), () -> setGoal(Goal.IDLING)) - .andThen(Commands.idle()) .withName("Flywheels Intaking"); } public Command eject() { - return startEnd(() -> setGoal(Goal.EJECTing), () -> setGoal(Goal.IDLING)) - .andThen(Commands.idle()) + return startEnd(() -> setGoal(Goal.EJECTING), () -> setGoal(Goal.IDLING)) .withName("Flywheels Ejecting"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java index 7140e9e1..a483da9f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java @@ -1,3 +1,10 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + package org.littletonrobotics.frc2024.subsystems.rollers; import edu.wpi.first.math.MathUtil; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java index 9f07150a..af04a75f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -8,7 +8,6 @@ package org.littletonrobotics.frc2024.subsystems.rollers; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; import lombok.Setter; @@ -90,30 +89,26 @@ public void periodic() { } public Command idle() { - return runOnce(() -> setGoal(Goal.IDLING)).andThen(Commands.idle()).withName("Rollers Idle"); + return runOnce(() -> setGoal(Goal.IDLING)).withName("Rollers Idle"); } public Command floorIntake() { return startEnd(() -> setGoal(Goal.FLOOR_INTAKING), () -> setGoal(Goal.IDLING)) - .andThen(Commands.idle()) .withName("Rollers Floor Intake"); } public Command stationIntake() { return startEnd(() -> setGoal(Goal.STATION_INTAKING), () -> setGoal(Goal.IDLING)) - .andThen(Commands.idle()) .withName("Rollers Station Intake"); } public Command ejectFloor() { return startEnd(() -> setGoal(Goal.EJECTING_TO_FLOOR), () -> setGoal(Goal.IDLING)) - .andThen(Commands.idle()) .withName("Rollers Eject Floor"); } public Command feedShooter() { return startEnd(() -> setGoal(Goal.FEEDING_TO_SHOOTER), () -> setGoal(Goal.IDLING)) - .andThen(Commands.idle()) .withName("Rollers Feed Shooter"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOSim.java index fa7b69a7..26e4e82e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOSim.java @@ -1,3 +1,10 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + package org.littletonrobotics.frc2024.subsystems.rollers.feeder; import edu.wpi.first.math.system.plant.DCMotor; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java index bdda57d0..aec69a5e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java @@ -1,3 +1,10 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + package org.littletonrobotics.frc2024.subsystems.rollers.indexer; import java.util.function.DoubleSupplier; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSim.java index f3e1fb3f..441f1dbe 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSim.java @@ -1,3 +1,10 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + package org.littletonrobotics.frc2024.subsystems.rollers.indexer; import edu.wpi.first.math.system.plant.DCMotor; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOSim.java index d8a3dec0..ade16761 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOSim.java @@ -1,3 +1,10 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + package org.littletonrobotics.frc2024.subsystems.rollers.intake; import edu.wpi.first.math.system.plant.DCMotor; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index b1cfcfc8..c1c75d9b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -8,12 +8,10 @@ package org.littletonrobotics.frc2024.subsystems.superstructure; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; -import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -66,26 +64,21 @@ public boolean atGoal() { } public Command stow() { - return runOnce(() -> setGoal(SystemState.STOWING)) - .andThen(Commands.idle()) - .withName("Superstructure Stow"); + return runOnce(() -> setGoal(SystemState.STOWING)).withName("Superstructure Stow"); } public Command aim() { return startEnd(() -> setGoal(SystemState.AIMING), () -> setGoal(SystemState.STOWING)) - .andThen(Commands.idle()) .withName("Superstructure Aim"); } public Command intake() { return startEnd(() -> setGoal(SystemState.INTAKING), () -> setGoal(SystemState.STOWING)) - .andThen(Commands.idle()) .withName("Superstructure Intake"); } public Command stationIntake() { return startEnd(() -> setGoal(SystemState.STATION_INTAKING), () -> setGoal(SystemState.STOWING)) - .andThen(Commands.idle()) .withName("Superstructure Station Intake"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index 52dfb5e2..9067c960 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -1,14 +1,20 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + package org.littletonrobotics.frc2024.subsystems.superstructure.arm; import static org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants.*; -import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.DoubleSupplier; import lombok.Getter; @@ -34,14 +40,14 @@ public class Arm extends SubsystemBase { private static final LoggedTunableNumber armToleranceDegreees = new LoggedTunableNumber("Arm/ToleranceDegrees", positionTolerance.getDegrees()); private static final LoggedTunableNumber armLowerLimit = - new LoggedTunableNumber("Arm/LowerLimitDegrees", 15.0); + new LoggedTunableNumber("Arm/LowerLimitDegrees", minAngle.getDegrees()); private static final LoggedTunableNumber armUpperLimit = - new LoggedTunableNumber("Arm/UpperLimitDegrees", 90.0); - private static LoggedTunableNumber armStowDegrees = + new LoggedTunableNumber("Arm/UpperLimitDegrees", maxAngle.getDegrees()); + private static final LoggedTunableNumber armStowDegrees = new LoggedTunableNumber("Superstructure/ArmStowDegrees", 20.0); - private static LoggedTunableNumber armStationIntakeDegrees = + private static final LoggedTunableNumber armStationIntakeDegrees = new LoggedTunableNumber("Superstructure/ArmStationIntakeDegrees", 45.0); - private static LoggedTunableNumber armIntakeDegrees = + private static final LoggedTunableNumber armIntakeDegrees = new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0); @RequiredArgsConstructor @@ -53,7 +59,7 @@ public enum Goal { private final DoubleSupplier armSetpointSupplier; - private double getArmSetpointRads() { + private double getRads() { return armSetpointSupplier.getAsDouble(); } } @@ -64,16 +70,26 @@ private double getArmSetpointRads() { private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); + private TrapezoidProfile motionProfile; + private ArmFeedforward ff; - private ArmVisualizer measuredVisualizer; - private ArmVisualizer setpointVisualizer; + private final ArmVisualizer measuredVisualizer; + private final ArmVisualizer setpointVisualizer; + private final ArmVisualizer goalVisualizer; public Arm(ArmIO io) { this.io = io; io.setBrakeMode(true); + motionProfile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints(armVelocity.get(), armAcceleration.get())); + io.setPID(kP.get(), kI.get(), kD.get()); + ff = new ArmFeedforward(kS.get(), kG.get(), kV.get(), kA.get()); + measuredVisualizer = new ArmVisualizer("measured", Color.kBlack); setpointVisualizer = new ArmVisualizer("setpoint", Color.kGreen); + goalVisualizer = new ArmVisualizer("goal", Color.kBlue); } public void periodic() { @@ -85,19 +101,35 @@ public void periodic() { LoggedTunableNumber.ifChanged( hashCode(), () -> io.setPID(kP.get(), kI.get(), kD.get()), kP, kI, kD); LoggedTunableNumber.ifChanged( - hashCode(), () -> io.setFF(kS.get(), kV.get(), kA.get(), kG.get()), kS, kV, kA, kG); + hashCode(), + () -> ff = new ArmFeedforward(kS.get(), kV.get(), kA.get(), kG.get()), + kS, + kV, + kA, + kG); LoggedTunableNumber.ifChanged( hashCode(), - constraints -> io.setProfileConstraints(constraints[0], constraints[1]), + constraints -> + motionProfile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints(constraints[0], constraints[1])), armVelocity, armAcceleration); if (!characterizing) { - io.runSetpoint( - MathUtil.clamp( - goal.getArmSetpointRads(), - Units.degreesToRadians(armLowerLimit.get()), - Units.degreesToRadians(armUpperLimit.get()))); + // Run closed loop + var output = + motionProfile.calculate( + 0.02, + new TrapezoidProfile.State(inputs.armPositionRads, inputs.armVelocityRadsPerSec), + new TrapezoidProfile.State(goal.getRads(), 0.0)); + + io.runSetpoint(output.position, ff.calculate(output.position, output.velocity)); + + // Logs + Logger.recordOutput("Arm/SetpointAngle", output.position); + setpointVisualizer.update(Rotation2d.fromRadians(output.position)); + goalVisualizer.update(Rotation2d.fromRadians(goal.getRads())); } if (DriverStation.isDisabled()) { @@ -105,7 +137,6 @@ public void periodic() { } measuredVisualizer.update(Rotation2d.fromRadians(inputs.armPositionRads)); - setpointVisualizer.update(Rotation2d.fromRadians(inputs.armSetpointRads)); Logger.recordOutput("Arm/Goal", goal); } @@ -113,14 +144,14 @@ public void stop() { io.stop(); } - @AutoLogOutput(key = "Arm/SetpointAngle") + @AutoLogOutput(key = "Arm/GoalAngle") public Rotation2d getSetpoint() { - return Rotation2d.fromRadians(goal.getArmSetpointRads()); + return Rotation2d.fromRadians(goal.getRads()); } @AutoLogOutput(key = "Arm/AtSetpoint") public boolean atSetpoint() { - return Math.abs(inputs.armPositionRads - goal.getArmSetpointRads()) + return Math.abs(inputs.armPositionRads - goal.getRads()) <= Units.degreesToRadians(armToleranceDegreees.get()); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java index 465ff36b..46842221 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java @@ -41,7 +41,7 @@ public class ArmConstants { public static Gains gains = switch (Constants.getRobot()) { - case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01); + case SIMBOT -> new Gains(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); case DEVBOT -> new Gains(1200, 0.0, 120, 6.22, 0.0, 0.0, 8.12); case COMPBOT -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); }; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java index d32aa2db..5597b96d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java @@ -15,7 +15,6 @@ class ArmIOInputs { public double armPositionRads = 0.0; public double armEncoderPositionRads = 0.0; public double armAbsoluteEncoderPositionRads = 0.0; - public double armSetpointRads = 0.0; public double armVelocityRadsPerSec = 0.0; public double[] armAppliedVolts = new double[] {}; public double[] armCurrentAmps = new double[] {}; @@ -27,7 +26,7 @@ class ArmIOInputs { default void updateInputs(ArmIOInputs inputs) {} /** Run to setpoint angle in radians */ - default void runSetpoint(double setpointRads) {} + default void runSetpoint(double setpointRads, double feedforward) {} /** Run motors at volts */ default void runVolts(double volts) {} @@ -38,16 +37,9 @@ default void runCurrent(double amps) {} /** Set brake mode enabled */ default void setBrakeMode(boolean enabled) {} - /** Set FF values */ - default void setFF(double s, double v, double a, double g) {} - /** Set PID values */ default void setPID(double p, double i, double d) {} - /** Set MotionMagic constraints */ - default void setProfileConstraints( - double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) {} - /** Sets position of internal encoder */ default void setPosition(double positionRads) {} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java index 0437ebe0..1c4ea866 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java @@ -86,16 +86,7 @@ public ArmIOKrakenFOC() { leaderConfig.Feedback.RotorToSensorRatio = reduction; // Set up controller - controllerConfig = - new Slot0Configs() - .withKP(gains.kP()) - .withKI(gains.kI()) - .withKD(gains.kD()) - .withKS(gains.ffkS()) - .withKV(gains.ffkV()) - .withKA(gains.ffkA()) - .withKG(gains.ffkG()) - .withGravityType(GravityTypeValue.Arm_Cosine); + controllerConfig = new Slot0Configs().withKP(gains.kP()).withKI(gains.kI()).withKD(gains.kD()); leaderConfig.Slot0 = controllerConfig; // Follower configs @@ -157,7 +148,6 @@ public void updateInputs(ArmIOInputs inputs) { Units.rotationsToRadians(armEncoderPositionRotations.getValue()); inputs.armAbsoluteEncoderPositionRads = Units.rotationsToRadians(armAbsolutePositionRotations.getValue()); - inputs.armSetpointRads = setpointState.position; inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRps.getValue()); inputs.armAppliedVolts = armAppliedVoltage.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); @@ -176,16 +166,11 @@ public void updateInputs(ArmIOInputs inputs) { } @Override - public void runSetpoint(double setpointRads) { - TrapezoidProfile.State currentState = - new TrapezoidProfile.State( - Units.rotationsToRadians(armInternalPositionRotations.getValue()), - Units.rotationsToRadians(armVelocityRps.getValue())); - setpointState = - motionProfile.calculate(0.02, currentState, new TrapezoidProfile.State(setpointRads, 0.0)); - // Run control - leaderMotor.setControl(positionControl.withPosition( - Units.radiansToRotations(setpointState.position))); + public void runSetpoint(double setpointRads, double feedforward) { + leaderMotor.setControl( + positionControl + .withPosition(Units.radiansToRotations(setpointRads)) + .withFeedForward(feedforward)); } @Override @@ -212,23 +197,6 @@ public void setPID(double p, double i, double d) { leaderMotor.getConfigurator().apply(controllerConfig); } - @Override - public void setFF(double s, double v, double a, double g) { - controllerConfig.kS = s; - controllerConfig.kV = v; - controllerConfig.kA = a; - controllerConfig.kG = g; - leaderMotor.getConfigurator().apply(controllerConfig); - } - - @Override - public void setProfileConstraints( - double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) { - motionProfile = - new TrapezoidProfile( - new TrapezoidProfile.Constraints(cruiseVelocityRadsPerSec, accelerationRadsPerSec2)); - } - @Override public void setPosition(double positionRads) { leaderMotor.setPosition(Units.radiansToRotations(positionRads)); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java index 21687c7b..2bdf3da0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java @@ -10,9 +10,8 @@ import static org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants.*; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; @@ -29,7 +28,7 @@ public class ArmIOSim implements ArmIO { false, Units.degreesToRadians(0.0)); - private final ProfiledPIDController profiledController; + private final PIDController controller; private double appliedVoltage = 0.0; private double positionOffset = 0.0; @@ -37,9 +36,9 @@ public class ArmIOSim implements ArmIO { private boolean closedLoop = true; public ArmIOSim() { - profiledController = - new ProfiledPIDController(gains.kP(), gains.kI(), gains.kD(), profileConstraints, 0.02); - sim.setState(Units.degreesToRadians(45.0), 0.0); + controller = new PIDController(0.0, 0.0, 0.0); + sim.setState(0.0, 0.0); + setPosition(0.0); } @Override @@ -47,10 +46,10 @@ public void updateInputs(ArmIOInputs inputs) { if (DriverStation.isDisabled()) { controllerNeedsReset = true; } + sim.update(0.02); inputs.armPositionRads = sim.getAngleRads() + positionOffset; - inputs.armSetpointRads = profiledController.getSetpoint().position; inputs.armVelocityRadsPerSec = sim.getVelocityRadPerSec(); inputs.armAppliedVolts = new double[] {appliedVoltage}; inputs.armCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; @@ -62,18 +61,16 @@ public void updateInputs(ArmIOInputs inputs) { } @Override - public void runSetpoint(double setpointRads) { + public void runSetpoint(double setpointRads, double feedforward) { if (!closedLoop) { controllerNeedsReset = true; + closedLoop = true; } if (controllerNeedsReset) { - profiledController.reset(sim.getAngleRads(), sim.getVelocityRadPerSec()); + controller.reset(); controllerNeedsReset = false; } - // Control - runVolts( - profiledController.calculate(sim.getAngleRads() + positionOffset, setpointRads) - + profiledController.getSetpoint().velocity); + runVolts(controller.calculate(sim.getAngleRads(), setpointRads + positionOffset) + feedforward); } @Override @@ -85,14 +82,7 @@ public void runVolts(double volts) { @Override public void setPID(double p, double i, double d) { - profiledController.setPID(p, i, d); - } - - @Override - public void setProfileConstraints( - double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) { - profiledController.setConstraints( - new TrapezoidProfile.Constraints(cruiseVelocityRadsPerSec, accelerationRadsPerSec2)); + controller.setPID(p, i, d); } @Override diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java index 3b42d32b..3afef219 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java @@ -27,7 +27,7 @@ public class ArmVisualizer { public ArmVisualizer(String key, Color color) { this.key = key; - mechanism = new Mechanism2d(3.0, 3.0, new Color8Bit(Color.kAntiqueWhite)); + mechanism = new Mechanism2d(3.0, 3.0, new Color8Bit(Color.kWhite)); root = mechanism.getRoot("pivot", 1.0, 0.4); arm = new MechanismLigament2d("arm", armLength, 20.0, 6, new Color8Bit(color)); root.append(arm); @@ -41,7 +41,7 @@ public void update(Rotation2d angle) { // Log 3d poses Pose3d pivot = new Pose3d( - armOrigin.getX(), 0.0, armOrigin.getY(), new Rotation3d(0.0, -angle.getRadians(), 0.0)); + armOrigin.getX(), 0.0, armOrigin.getY(), new Rotation3d(0.0, angle.getRadians(), 0.0)); Logger.recordOutput("Arm/" + key + "3d", pivot); } }