diff --git a/ascope_assets/Robot_MA24B/config.json b/ascope_assets/Robot_MA24B/config.json new file mode 100644 index 00000000..a8b0c6c1 --- /dev/null +++ b/ascope_assets/Robot_MA24B/config.json @@ -0,0 +1,85 @@ +{ + "name": "MA24B", + "rotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "position": [ + 0, + 0, + 0 + ], + "cameras": [ + { + "name": "Northstar 0", + "rotations": [ + { + "axis": "y", + "degrees": -28.125 + }, + { + "axis": "z", + "degrees": 30.0 + } + ], + "position": [ + 0.247269, + 0.24729186, + 0.2244598 + ], + "resolution": [ + 1600, + 1200 + ], + "fov": 75 + }, + { + "name": "Northstar 1", + "rotations": [ + { + "axis": "y", + "degrees": -28.125 + }, + { + "axis": "z", + "degrees": -30.0 + } + ], + "position": [ + 0.247269, + -0.24729186, + 0.2244598 + ], + "resolution": [ + 1600, + 1200 + ], + "fov": 75 + } + ], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90 + }, + { + "axis": "z", + "degrees": 90 + } + ], + "zeroedPosition": [ + 0.238, + 0.0, + -0.298 + ] + } + ] +} diff --git a/ascope_assets/Robot_MA24B/model.glb b/ascope_assets/Robot_MA24B/model.glb new file mode 100644 index 00000000..b5e527c7 Binary files /dev/null and b/ascope_assets/Robot_MA24B/model.glb differ diff --git a/ascope_assets/Robot_MA24B/model_0.glb b/ascope_assets/Robot_MA24B/model_0.glb new file mode 100644 index 00000000..e6f9e898 Binary files /dev/null and b/ascope_assets/Robot_MA24B/model_0.glb differ diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 0cbb9363..e3445a14 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -35,12 +35,15 @@ import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder; import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIO; import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOKrakenFOC; +import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOSim; import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer; import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIO; +import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSim; import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSparkFlex; import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake; import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIO; import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOKrakenFOC; +import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOSim; import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO; @@ -121,11 +124,18 @@ 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()); } } } + // No-op implementation for replay if (drive == null) { drive = new Drive( @@ -200,20 +210,20 @@ private void configureButtonBindings() { () -> drive.setTeleopDriveGoal( -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX())) - .withName("DriveTeleop")); + .withName("Drive Teleop Input")); + + // Aim and Rev Flywheels controller .a() .whileTrue( Commands.startEnd(drive::setAutoAimGoal, drive::clearAutoAimGoal) - .alongWith(superstructure.aimCommand(), flywheels.shootCommand())); - - Trigger readyToShootTrigger = + .alongWith(superstructure.aim(), flywheels.shoot()) + .withName("Aim")); + // Shoot + Trigger readyToShoot = new Trigger( - () -> - drive.isAutoAimGoalCompleted() - && flywheels.atSetpoint() - && superstructure.atArmSetpoint()); - readyToShootTrigger + () -> drive.isAutoAimGoalCompleted() && superstructure.atGoal() && flywheels.atGoal()); + readyToShoot .whileTrue( Commands.run( () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0))) @@ -222,26 +232,29 @@ private void configureButtonBindings() { () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0))); controller .rightTrigger() - .and(readyToShootTrigger) + .and(readyToShoot) .onTrue( - rollers - .feedShooterCommand() - .withTimeout(1.0) - // Take over superstructure and flywheels, cancelling the main aiming command - .deadlineWith(superstructure.aimCommand(), flywheels.shootCommand())); - + Commands.parallel( + Commands.waitSeconds(0.5), + Commands.waitUntil(controller.rightTrigger().negate())) + .deadlineWith(rollers.feedShooter(), superstructure.aim(), flywheels.shoot())); + // Intake Floor controller .leftTrigger() .whileTrue( superstructure - .floorIntakeCommand() - .alongWith(Commands.waitSeconds(0.25).andThen(rollers.floorIntakeCommand()))); + .intake() + .alongWith( + Commands.waitUntil(superstructure::atGoal).andThen(rollers.floorIntake())) + .withName("Floor Intake")); + // Eject Floor controller .leftBumper() .whileTrue( superstructure - .floorIntakeCommand() - .alongWith(Commands.waitSeconds(0.25).andThen(rollers.ejectFloorCommand()))); + .intake() + .alongWith(Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor())) + .withName("Eject To Floor")); controller .y() diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java index a21d893d..9ed51192 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj.simulation.DCMotorSim; public class ModuleIOSim implements ModuleIO { - private final DCMotorSim driveSim = new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.driveReduction(), 0.025); private final DCMotorSim turnSim = diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java index 913c3ed3..815cf54d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java @@ -24,7 +24,7 @@ public class FlywheelConstants { case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0); }; - public record Config(int leftID, int rightID, double reduction, double toleranceRPM) {} + public record Config(int leftID, int rightID, double reduction, double toleranceRpm) {} public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {} } 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 622cca30..666ab786 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -26,31 +26,30 @@ public class Flywheels extends SubsystemBase { private static final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheels/kS", gains.kS()); private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV()); private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA()); - private static LoggedTunableNumber shootingLeftRPM = - new LoggedTunableNumber("Superstructure/ShootingLeftRPM", 6000.0); - private static LoggedTunableNumber shootingRightRPM = - new LoggedTunableNumber("Superstructure/ShootingRightRPM", 4000.0); - private static LoggedTunableNumber idleLeftRPM = - new LoggedTunableNumber("Superstructure/IdleLeftRPM", 200.0); - private static LoggedTunableNumber idleRightRPM = - new LoggedTunableNumber("Superstructure/IdleRightRPM", 200.0); - - private static LoggedTunableNumber intakingLeftRPM = - new LoggedTunableNumber("Superstructure/IntakingLeftRPM", -2000.0); - private static LoggedTunableNumber intakingRightRPM = - new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0); + private static final LoggedTunableNumber shootingLeftRpm = + new LoggedTunableNumber("Superstructure/ShootingLeftRpm", 6000.0); + private static final LoggedTunableNumber shootingRightRpm = + new LoggedTunableNumber("Superstructure/ShootingRightRpm", 4000.0); + private static final LoggedTunableNumber idleLeftRpm = + new LoggedTunableNumber("Superstructure/IdleLeftRpm", 1500.0); + private static final LoggedTunableNumber idleRightRpm = + new LoggedTunableNumber("Superstructure/IdleRightRpm", 1000.0); + private static final LoggedTunableNumber intakingRpm = + new LoggedTunableNumber("Superstructure/IntakingRpm", -2000.0); + private static final LoggedTunableNumber ejectingRpm = + new LoggedTunableNumber("Superstructure/EjectingRpm", 2000.0); private static final LoggedTunableNumber shooterTolerance = - new LoggedTunableNumber("Flywheels/ToleranceRPM", config.toleranceRPM()); + new LoggedTunableNumber("Flywheels/ToleranceRpm", config.toleranceRpm()); private final FlywheelsIO io; private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged(); @RequiredArgsConstructor public enum Goal { - STOP(() -> 0.0, () -> 0.0), - IDLE(idleLeftRPM, idleRightRPM), - SHOOTING(shootingLeftRPM, shootingRightRPM), - INTAKING(intakingLeftRPM, intakingRightRPM), + IDLE(idleLeftRpm, idleRightRpm), + SHOOT(shootingLeftRpm, shootingRightRpm), + INTAKE(intakingRpm, intakingRpm), + EJECT(ejectingRpm, ejectingRpm), CHARACTERIZING(() -> 0.0, () -> 0.0); private final DoubleSupplier leftSetpoint; @@ -69,7 +68,8 @@ private double getRightSetpoint() { public Flywheels(FlywheelsIO io) { this.io = io; - setDefaultCommand(runOnce(() -> goal = Goal.IDLE).withName("FlywheelsIdle")); + + setDefaultCommand(runOnce(this::goIdle).withName("Flywheels Idle")); } @Override @@ -83,20 +83,18 @@ public void periodic() { hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA); if (DriverStation.isDisabled()) { - goal = Goal.STOP; + io.stop(); } - switch (goal) { - case STOP -> io.stop(); - case CHARACTERIZING -> {} // Handled by runCharacterizationVolts - default -> io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint()); + if (goal != Goal.CHARACTERIZING) { + io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint()); } Logger.recordOutput("Flywheels/Goal", goal); - Logger.recordOutput("Flywheels/LeftSetpointRPM", goal.getLeftSetpoint()); - Logger.recordOutput("Flywheels/RightSetpointRPM", goal.getRightSetpoint()); - Logger.recordOutput("Flywheels/LeftRPM", inputs.leftVelocityRpm); - Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm); + Logger.recordOutput("Flywheels/LeftSetpointRpm", goal.getLeftSetpoint()); + Logger.recordOutput("Flywheels/RightSetpointRpm", goal.getRightSetpoint()); + Logger.recordOutput("Flywheels/LeftRpm", inputs.leftVelocityRpm); + Logger.recordOutput("Flywheels/RightRpm", inputs.rightVelocityRpm); } public void runCharacterizationVolts(double volts) { @@ -109,19 +107,25 @@ public double getCharacterizationVelocity() { return (inputs.leftVelocityRpm + inputs.rightVelocityRpm) / 2.0; } - @AutoLogOutput(key = "Shooter/AtSetpoint") - public boolean atSetpoint() { - return Math.abs(inputs.leftVelocityRpm - goal.leftSetpoint.getAsDouble()) - <= shooterTolerance.get() - && Math.abs(inputs.rightVelocityRpm - goal.rightSetpoint.getAsDouble()) - <= shooterTolerance.get(); + @AutoLogOutput(key = "Flywheels/AtGoal") + public boolean atGoal() { + return Math.abs(inputs.leftVelocityRpm - goal.getLeftSetpoint()) <= shooterTolerance.get() + && Math.abs(inputs.rightVelocityRpm - goal.getRightSetpoint()) <= shooterTolerance.get(); + } + + private void goIdle() { + goal = Goal.IDLE; + } + + public Command shoot() { + return startEnd(() -> goal = Goal.SHOOT, this::goIdle).withName("Flywheels Shooting"); } - public Command shootCommand() { - return startEnd(() -> goal = Goal.SHOOTING, () -> goal = Goal.IDLE).withName("FlywheelsShoot"); + public Command intake() { + return startEnd(() -> goal = Goal.INTAKE, this::goIdle).withName("Flywheels Intaking"); } - public Command intakeCommand() { - return startEnd(() -> goal = Goal.INTAKING, () -> goal = Goal.IDLE).withName("FlywheelsIntake"); + public Command eject() { + return startEnd(() -> goal = Goal.EJECT, this::goIdle).withName("Flywheels Ejecting"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystem.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystem.java index 37d3ba91..9beb2bb5 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystem.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystem.java @@ -29,6 +29,6 @@ public void periodic() { Logger.processInputs(name, inputs); io.runVolts(getGoal().getVoltageSupplier().getAsDouble()); - Logger.recordOutput(name + "/Goal", getGoal().toString()); + Logger.recordOutput("Rollers/" + name + "Goal", getGoal().toString()); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIO.java index f282275e..d1d18b0b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIO.java @@ -16,6 +16,7 @@ abstract class GenericRollerSystemIOInputs { public double velocityRadsPerSec = 0.0; public double appliedVoltage = 0.0; public double outputCurrent = 0.0; + public double tempCelsius = 0.0; } default void updateInputs(GenericRollerSystemIOInputs inputs) {} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOKrakenFOC.java index 24925b11..74519e93 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOKrakenFOC.java @@ -25,6 +25,7 @@ public abstract class GenericRollerSystemIOKrakenFOC implements GenericRollerSys private final StatusSignal velocity; private final StatusSignal appliedVoltage; private final StatusSignal outputCurrent; + private final StatusSignal tempCelsius; // Single shot for voltage mode, robot loop will call continuously private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); @@ -49,9 +50,10 @@ public GenericRollerSystemIOKrakenFOC( velocity = motor.getVelocity(); appliedVoltage = motor.getMotorVoltage(); outputCurrent = motor.getTorqueCurrent(); + tempCelsius = motor.getDeviceTemp(); BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, position, velocity, appliedVoltage, outputCurrent); + 50.0, position, velocity, appliedVoltage, outputCurrent, tempCelsius); } @Override @@ -62,6 +64,7 @@ public void updateInputs(GenericRollerSystemIOInputs inputs) { inputs.velocityRadsPerSec = Units.rotationsToRadians(velocity.getValueAsDouble()) / reduction; inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.outputCurrent = outputCurrent.getValueAsDouble(); + inputs.tempCelsius = tempCelsius.getValueAsDouble(); } @Override diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java new file mode 100644 index 00000000..0709b1c6 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java @@ -0,0 +1,46 @@ +// 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; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class GenericRollerSystemIOSim implements GenericRollerSystemIO { + private final DCMotorSim sim; + private double appliedVoltage = 0.0; + + public GenericRollerSystemIOSim(DCMotor motorModel, double reduction, double moi) { + sim = new DCMotorSim(motorModel, reduction, moi); + } + + @Override + public void updateInputs(GenericRollerSystemIOInputs inputs) { + if (DriverStation.isDisabled()) { + runVolts(0.0); + } + + sim.update(0.02); + inputs.positionRads = sim.getAngularPositionRad(); + inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + inputs.outputCurrent = sim.getCurrentDrawAmps(); + } + + @Override + public void runVolts(double volts) { + appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + sim.setInputVoltage(appliedVoltage); + } + + @Override + public void stop() { + runVolts(0.0); + } +} 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 da0037ed..d226f82d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import lombok.Getter; import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder; import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer; import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake; @@ -29,16 +30,18 @@ public enum Goal { FLOOR_INTAKE, STATION_INTAKE, EJECT_TO_FLOOR, - FEED_SHOOTER + FEED_TO_SHOOTER } + @Getter private Goal goal = Goal.IDLE; + public Rollers(Feeder feeder, Indexer indexer, Intake intake, RollersSensorsIO sensorsIO) { this.feeder = feeder; this.indexer = indexer; this.intake = intake; this.sensorsIO = sensorsIO; - setDefaultCommand(runOnce(this::goIdle).withName("RollersIdle")); + setDefaultCommand(runOnce(this::goIdle).withName("Rollers Idling")); } @Override @@ -50,64 +53,64 @@ public void periodic() { goIdle(); } + switch (goal) { + case IDLE -> { + feeder.setGoal(Feeder.Goal.IDLING); + indexer.setGoal(Indexer.Goal.IDLING); + intake.setGoal(Intake.Goal.IDLING); + } + case FLOOR_INTAKE -> { + feeder.setGoal(Feeder.Goal.FLOOR_INTAKING); + indexer.setGoal(Indexer.Goal.FLOOR_INTAKING); + intake.setGoal(Intake.Goal.FLOOR_INTAKING); + if (sensorInputs.shooterStaged) { + indexer.setGoal(Indexer.Goal.IDLING); + } + } + case STATION_INTAKE -> { + feeder.setGoal(Feeder.Goal.IDLING); + indexer.setGoal(Indexer.Goal.STATION_INTAKING); + intake.setGoal(Intake.Goal.IDLING); + if (sensorInputs.shooterStaged) { // TODO: add this banner sensor + indexer.setGoal(Indexer.Goal.IDLING); + } + } + case EJECT_TO_FLOOR -> { + feeder.setGoal(Feeder.Goal.EJECTING); + indexer.setGoal(Indexer.Goal.EJECTING); + intake.setGoal(Intake.Goal.EJECTING); + } + case FEED_TO_SHOOTER -> { + feeder.setGoal(Feeder.Goal.SHOOTING); + indexer.setGoal(Indexer.Goal.SHOOTING); + intake.setGoal(Intake.Goal.IDLING); + } + } + feeder.periodic(); indexer.periodic(); intake.periodic(); } private void goIdle() { - feeder.setGoal(Feeder.Goal.IDLE); - indexer.setGoal(Indexer.Goal.IDLE); - intake.setGoal(Intake.Goal.IDLE); + goal = Goal.IDLE; } - public Command floorIntakeCommand() { - return startEnd( - () -> { - feeder.setGoal(Feeder.Goal.FLOOR_INTAKING); - indexer.setGoal(Indexer.Goal.FLOOR_INTAKING); - intake.setGoal(Intake.Goal.FLOOR_INTAKING); - if (sensorInputs.shooterStaged) { - indexer.setGoal(Indexer.Goal.IDLE); - } - }, - this::goIdle) - .withName("RollersFloorIntake"); + public Command floorIntake() { + return startEnd(() -> goal = Goal.FLOOR_INTAKE, this::goIdle).withName("Rollers Floor Intake"); } - public Command stationIntakeCommand() { - return startEnd( - () -> { - feeder.setGoal(Feeder.Goal.IDLE); - indexer.setGoal(Indexer.Goal.STATION_INTAKING); - intake.setGoal(Intake.Goal.IDLE); - if (sensorInputs.shooterStaged) { // TODO: ADD THIS BANNER - indexer.setGoal(Indexer.Goal.IDLE); - } - }, - this::goIdle) - .withName("RollersStationIntake"); + public Command stationIntake() { + return startEnd(() -> goal = Goal.STATION_INTAKE, this::goIdle) + .withName("Rollers Station Intake"); } - public Command ejectFloorCommand() { - return startEnd( - () -> { - feeder.setGoal(Feeder.Goal.EJECTING); - indexer.setGoal(Indexer.Goal.EJECTING); - intake.setGoal(Intake.Goal.EJECTING); - }, - this::goIdle) - .withName("RollersEjectFloor"); + public Command ejectFloor() { + return startEnd(() -> goal = Goal.EJECT_TO_FLOOR, this::goIdle).withName("Rollers Eject Floor"); } - public Command feedShooterCommand() { - return startEnd( - () -> { - feeder.setGoal(Feeder.Goal.SHOOTING); - indexer.setGoal(Indexer.Goal.SHOOTING); - intake.setGoal(Intake.Goal.IDLE); - }, - this::goIdle) - .withName("RollersFeedShooter"); + public Command feedShooter() { + return startEnd(() -> goal = Goal.FEED_TO_SHOOTER, this::goIdle) + .withName("Rollers Feed Shooter"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java index 81860ad7..22337e24 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java @@ -12,6 +12,7 @@ import lombok.RequiredArgsConstructor; import lombok.Setter; import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystem; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystem.VoltageGoal; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; @Setter @@ -19,17 +20,16 @@ public class Feeder extends GenericRollerSystem { @RequiredArgsConstructor @Getter - public enum Goal implements GenericRollerSystem.VoltageGoal { - IDLE(() -> 0.0), + public enum Goal implements VoltageGoal { + IDLING(() -> 0.0), FLOOR_INTAKING(new LoggedTunableNumber("Feeder/FloorIntakingVoltage", 8.0)), - BACKSTOPPING(new LoggedTunableNumber("Feeder/BackstoppingVoltage", -4.0)), SHOOTING(new LoggedTunableNumber("Feeder/Shooting", 8.0)), EJECTING(new LoggedTunableNumber("Feeder/EjectingVoltage", -6.0)); private final DoubleSupplier voltageSupplier; } - private Feeder.Goal goal = Feeder.Goal.IDLE; + private Feeder.Goal goal = Feeder.Goal.IDLING; public Feeder(FeederIO io) { super("Feeder", io); 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 new file mode 100644 index 00000000..26e4e82e --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/FeederIOSim.java @@ -0,0 +1,21 @@ +// 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; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystemIOSim; + +public class FeederIOSim extends GenericRollerSystemIOSim implements FeederIO { + private static final DCMotor motorModel = DCMotor.getKrakenX60Foc(1); + private static final double reduction = 18.0 / 12.0; + private static final double moi = 0.001; + + public FeederIOSim() { + super(motorModel, reduction, moi); + } +} 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 c5255613..a005759c 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 @@ -12,6 +12,7 @@ import lombok.RequiredArgsConstructor; import lombok.Setter; import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystem; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystem.VoltageGoal; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; @Setter @@ -19,8 +20,8 @@ public class Indexer extends GenericRollerSystem { @RequiredArgsConstructor @Getter - public enum Goal implements GenericRollerSystem.VoltageGoal { - IDLE(() -> 0.0), + public enum Goal implements VoltageGoal { + IDLING(() -> 0.0), FLOOR_INTAKING(new LoggedTunableNumber("Indexer/FloorIntakingVoltage", 2.0)), STATION_INTAKING(new LoggedTunableNumber("Indexer/StationIntakingVoltage", -2.0)), SHOOTING(new LoggedTunableNumber("Indexer/ShootingVoltage", 12.0)), @@ -29,7 +30,7 @@ public enum Goal implements GenericRollerSystem.VoltageGoal { private final DoubleSupplier voltageSupplier; } - private Indexer.Goal goal = Indexer.Goal.IDLE; + private Indexer.Goal goal = Indexer.Goal.IDLING; public Indexer(IndexerIO io) { super("Indexer", io); 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 new file mode 100644 index 00000000..441f1dbe --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/IndexerIOSim.java @@ -0,0 +1,21 @@ +// 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; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystemIOSim; + +public class IndexerIOSim extends GenericRollerSystemIOSim implements IndexerIO { + private static final DCMotor motorModel = DCMotor.getKrakenX60Foc(1); + private static final double reduction = (1.0 / 1.0); + private static final double moi = 0.001; + + public IndexerIOSim() { + super(motorModel, reduction, moi); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java index f414ea0e..6d96e83a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java @@ -12,6 +12,7 @@ import lombok.RequiredArgsConstructor; import lombok.Setter; import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystem; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystem.VoltageGoal; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; @Setter @@ -19,16 +20,15 @@ public class Intake extends GenericRollerSystem { @RequiredArgsConstructor @Getter - public enum Goal implements GenericRollerSystem.VoltageGoal { - IDLE(() -> 0.0), + public enum Goal implements VoltageGoal { + IDLING(() -> 0.0), FLOOR_INTAKING(new LoggedTunableNumber("Intake/FloorIntakingVoltage", 8.0)), - SHOOTING(new LoggedTunableNumber("Intake/Shooting", 6.0)), EJECTING(new LoggedTunableNumber("Intake/EjectingVoltage", -8.0)); private final DoubleSupplier voltageSupplier; } - private Goal goal = Goal.IDLE; + private Goal goal = Goal.IDLING; public Intake(IntakeIO io) { super("Intake", io); 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 new file mode 100644 index 00000000..ade16761 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/IntakeIOSim.java @@ -0,0 +1,21 @@ +// 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; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystemIOSim; + +public class IntakeIOSim extends GenericRollerSystemIOSim implements IntakeIO { + private static final DCMotor motorModel = DCMotor.getKrakenX60Foc(1); + private static final double reduction = (18.0 / 12.0); + private static final double moi = 0.001; + + public IntakeIOSim() { + super(motorModel, reduction, moi); + } +} 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 8a0e2ee2..53a51715 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -11,87 +11,77 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; -import lombok.Setter; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class Superstructure extends SubsystemBase { - public enum SystemState { - // PREPARE_SHOOT, + public enum Goal { + STOW, AIM, - // PREPARE_INTAKE, - FLOOR_INTAKE, + INTAKE, STATION_INTAKE, - // REVERSE_INTAKE, - IDLE + AMP, + PREPARE_CLIMB, + CLIMB, + TRAP } - public enum GamepieceState { - NO_GAMEPIECE, - HOLDING_SHOOTER, - HOLDING_BACKPACK - } - - @AutoLogOutput @Getter private SystemState currentState = SystemState.IDLE; - @AutoLogOutput @Getter private SystemState goalState = SystemState.IDLE; - @Getter @Setter private GamepieceState gamepieceState = GamepieceState.NO_GAMEPIECE; + @Getter private Goal currentGoal = Goal.STOW; + @Getter private Goal desiredGoal = Goal.STOW; private final Arm arm; public Superstructure(Arm arm) { this.arm = arm; - setDefaultCommand(runOnce(() -> goalState = SystemState.IDLE).withName("SuperstructureIdle")); + + setDefaultCommand(runOnce(this::stow).withName("Superstructure Stowing")); } @Override public void periodic() { if (DriverStation.isDisabled()) { - goalState = SystemState.IDLE; + desiredGoal = Goal.STOW; + arm.stop(); } - switch (goalState) { - case AIM -> currentState = SystemState.AIM; - case FLOOR_INTAKE -> currentState = SystemState.FLOOR_INTAKE; - case STATION_INTAKE -> currentState = SystemState.STATION_INTAKE; - case IDLE -> currentState = SystemState.IDLE; - } + currentGoal = desiredGoal; // Will change soon - switch (currentState) { - case AIM -> { - arm.setGoal(Arm.Goal.AIM); - } - case FLOOR_INTAKE -> { - arm.setGoal(Arm.Goal.FLOOR_INTAKE); - } - case STATION_INTAKE -> { - arm.setGoal(Arm.Goal.STATION_INTAKE); - } - case IDLE -> { - arm.setGoal(Arm.Goal.STOW); - } + switch (currentGoal) { + case STOW -> arm.setGoal(Arm.Goal.STOW); + case AIM -> arm.setGoal(Arm.Goal.AIM); + case INTAKE -> arm.setGoal(Arm.Goal.FLOOR_INTAKE); + case STATION_INTAKE -> arm.setGoal(Arm.Goal.STATION_INTAKE); + default -> {} // DO NOTHING ELSE } arm.periodic(); + + Logger.recordOutput("Superstructure/GoalState", desiredGoal); + Logger.recordOutput("Superstructure/CurrentState", currentGoal); + } + + @AutoLogOutput(key = "Superstructure/CompletedGoal") + public boolean atGoal() { + return currentGoal == desiredGoal && arm.atGoal(); } - public boolean atArmSetpoint() { - return arm.atSetpoint(); + public void stow() { + desiredGoal = Goal.STOW; } - public Command aimCommand() { - return startEnd(() -> goalState = SystemState.AIM, () -> goalState = SystemState.IDLE) - .withName("SuperstructureAim"); + public Command aim() { + return startEnd(() -> desiredGoal = Goal.AIM, this::stow).withName("Superstructure Aiming"); } - public Command floorIntakeCommand() { - return startEnd(() -> goalState = SystemState.FLOOR_INTAKE, () -> goalState = SystemState.IDLE) - .withName("SuperstructureFloorIntake"); + public Command intake() { + return startEnd(() -> desiredGoal = Goal.INTAKE, this::stow) + .withName("Superstructure Intaking"); } - public Command stationIntakeCommand() { - return startEnd( - () -> goalState = SystemState.STATION_INTAKE, () -> goalState = SystemState.IDLE) - .withName("SuperstructureStationIntake"); + public Command stationIntake() { + return startEnd(() -> desiredGoal = Goal.STATION_INTAKE, this::stow) + .withName("Superstructure Station Intaking"); } } 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 0629d53f..cbcc1a7e 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 @@ -10,14 +10,18 @@ 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.util.Color; import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; import org.littletonrobotics.frc2024.RobotState; +import org.littletonrobotics.frc2024.util.EqualsUtil; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -30,45 +34,64 @@ public class Arm { private static final LoggedTunableNumber kV = new LoggedTunableNumber("Arm/kV", gains.ffkV()); private static final LoggedTunableNumber kA = new LoggedTunableNumber("Arm/kA", gains.ffkA()); private static final LoggedTunableNumber kG = new LoggedTunableNumber("Arm/kG", gains.ffkG()); - private static final LoggedTunableNumber armVelocity = + private static final LoggedTunableNumber maxVelocity = new LoggedTunableNumber("Arm/Velocity", profileConstraints.maxVelocity); - private static final LoggedTunableNumber armAcceleration = + private static final LoggedTunableNumber maxAcceleration = new LoggedTunableNumber("Arm/Acceleration", profileConstraints.maxAcceleration); - private static final LoggedTunableNumber armToleranceDegreees = + private static final LoggedTunableNumber toleranceDegrees = new LoggedTunableNumber("Arm/ToleranceDegrees", positionTolerance.getDegrees()); - private static final LoggedTunableNumber armLowerLimit = - new LoggedTunableNumber("Arm/LowerLimitDegrees", 15.0); - private static final LoggedTunableNumber armUpperLimit = - new LoggedTunableNumber("Arm/UpperLimitDegrees", 90.0); - private static LoggedTunableNumber armStowDegrees = + private static final LoggedTunableNumber lowerLimitDegrees = + new LoggedTunableNumber("Arm/LowerLimitDegrees", minAngle.getDegrees()); + private static final LoggedTunableNumber upperLimitDegrees = + new LoggedTunableNumber("Arm/UpperLimitDegrees", maxAngle.getDegrees()); + private static final LoggedTunableNumber stowDegrees = new LoggedTunableNumber("Superstructure/ArmStowDegrees", 20.0); - private static LoggedTunableNumber armStationIntakeDegrees = + private static final LoggedTunableNumber stationIntakeDegrees = new LoggedTunableNumber("Superstructure/ArmStationIntakeDegrees", 45.0); - private static LoggedTunableNumber armIntakeDegrees = + private static final LoggedTunableNumber intakeDegrees = new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0); @RequiredArgsConstructor public enum Goal { - FLOOR_INTAKE(() -> Units.degreesToRadians(armIntakeDegrees.get())), - STATION_INTAKE(() -> Units.degreesToRadians(armStationIntakeDegrees.get())), + FLOOR_INTAKE(() -> Units.degreesToRadians(intakeDegrees.get())), + STATION_INTAKE(() -> Units.degreesToRadians(stationIntakeDegrees.get())), AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getRadians()), - STOW(() -> Units.degreesToRadians(armStowDegrees.get())); + STOW(() -> Units.degreesToRadians(stowDegrees.get())), + CUSTOM(new LoggedTunableNumber("Arm/CustomSetpoint", 20.0)); private final DoubleSupplier armSetpointSupplier; - private double getArmSetpointRads() { + private double getRads() { return armSetpointSupplier.getAsDouble(); } } - @Getter @Setter Goal goal = Goal.STOW; + @Getter @Setter private Goal goal = Goal.STOW; + private boolean characterizing = false; private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); + private TrapezoidProfile motionProfile; + private TrapezoidProfile.State setpointState = new TrapezoidProfile.State(); + private ArmFeedforward ff; + + 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(maxVelocity.get(), maxAcceleration.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() { @@ -80,23 +103,48 @@ 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]), - armVelocity, - armAcceleration); - - io.runSetpoint( - MathUtil.clamp( - goal.getArmSetpointRads(), - Units.degreesToRadians(armLowerLimit.get()), - Units.degreesToRadians(armUpperLimit.get()))); + constraints -> + motionProfile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints(constraints[0], constraints[1])), + maxVelocity, + maxAcceleration); + + if (!characterizing) { + // Run closed loop + setpointState = + motionProfile.calculate( + 0.02, + new TrapezoidProfile.State(inputs.armPositionRads, inputs.armVelocityRadsPerSec), + new TrapezoidProfile.State( + MathUtil.clamp( + goal.getRads(), + Units.degreesToRadians(lowerLimitDegrees.get()), + Units.degreesToRadians(upperLimitDegrees.get())), + 0.0)); + + io.runSetpoint( + setpointState.position, ff.calculate(setpointState.position, setpointState.velocity)); + } if (DriverStation.isDisabled()) { io.stop(); } + // Logs + measuredVisualizer.update(inputs.armPositionRads); + setpointVisualizer.update(setpointState.position); + goalVisualizer.update(goal.getRads()); + Logger.recordOutput("Arm/SetpointAngle", setpointState.position); + Logger.recordOutput("Arm/SetpointVelocity", setpointState.velocity); Logger.recordOutput("Arm/Goal", goal); } @@ -104,19 +152,14 @@ public void stop() { io.stop(); } - public Rotation2d getAngle() { - return Rotation2d.fromRadians(inputs.armPositionRads); - } - - @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()) - <= Units.degreesToRadians(armToleranceDegreees.get()); + @AutoLogOutput(key = "Arm/AtGoal") + public boolean atGoal() { + return EqualsUtil.epsilonEquals(setpointState.position, goal.getRads(), 1e-3); } // public Command getStaticCurrent() { 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..bacb7720 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 @@ -17,8 +17,7 @@ public class ArmConstants { // reduction is 12:62 18:60 12:65 public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0); public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0); - public static Translation2d armOrigin = - new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75)); + public static Translation2d armOrigin = new Translation2d(-0.238, 0.298); public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0); public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0); @@ -41,7 +40,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(90.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 a398fed4..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 armTrajectorySetpointRads = 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 22ce3f4f..4806c874 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 @@ -10,7 +10,6 @@ import static org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants.*; import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Slot0Configs; @@ -19,7 +18,6 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.*; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import java.util.List; import org.littletonrobotics.frc2024.util.Alert; @@ -42,8 +40,6 @@ public class ArmIOKrakenFOC implements ArmIO { // Control private final Slot0Configs controllerConfig; - private TrapezoidProfile motionProfile; - private TrapezoidProfile.State setpointState = new TrapezoidProfile.State(); private final VoltageOut voltageControl = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0.0); @@ -86,16 +82,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 @@ -126,9 +113,6 @@ public ArmIOKrakenFOC() { armTorqueCurrent.get(1), armTempCelsius.get(0), armTempCelsius.get(1)); - - // Init profile - motionProfile = new TrapezoidProfile(profileConstraints); } public void updateInputs(ArmIOInputs inputs) { @@ -143,21 +127,25 @@ public void updateInputs(ArmIOInputs inputs) { armOutputCurrent.get(0), armTorqueCurrent.get(0), armTempCelsius.get(0)) - == StatusCode.OK); + .isOK()); followerMotorDisconnected.set( BaseStatusSignal.refreshAll( armAppliedVoltage.get(1), armOutputCurrent.get(1), armTorqueCurrent.get(1), armTempCelsius.get(1)) - == StatusCode.OK); + .isOK()); + + inputs.absoluteEncoderConnected = + BaseStatusSignal.refreshAll(armEncoderPositionRotations, armAbsolutePositionRotations) + .isOK(); + absoluteEncoderDisconnected.set(!inputs.absoluteEncoderConnected); inputs.armPositionRads = Units.rotationsToRadians(armInternalPositionRotations.getValue()); inputs.armEncoderPositionRads = Units.rotationsToRadians(armEncoderPositionRotations.getValue()); inputs.armAbsoluteEncoderPositionRads = Units.rotationsToRadians(armAbsolutePositionRotations.getValue()); - inputs.armTrajectorySetpointRads = setpointState.position; inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRps.getValue()); inputs.armAppliedVolts = armAppliedVoltage.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); @@ -167,24 +155,14 @@ public void updateInputs(ArmIOInputs inputs) { armTorqueCurrent.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); inputs.armTempCelcius = armTempCelsius.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); - - // Check encoder connected - inputs.absoluteEncoderConnected = - BaseStatusSignal.refreshAll(armEncoderPositionRotations, armAbsolutePositionRotations) - == StatusCode.OK; - absoluteEncoderDisconnected.set(inputs.absoluteEncoderConnected); } @Override - public void runSetpoint(double setpointRads) { - TrapezoidProfile.State currentState = - new TrapezoidProfile.State( - Units.rotationsToRadians(armInternalPositionRotations.getValue()), - Units.rotationsToRadians(armVelocityRps.getValue())); - setpointState = - motionProfile.calculate(0.0, currentState, new TrapezoidProfile.State(setpointRads, 0.0)); - // Run control - leaderMotor.setControl(positionControl.withPosition(setpointState.position)); + public void runSetpoint(double setpointRads, double feedforward) { + leaderMotor.setControl( + positionControl + .withPosition(Units.radiansToRotations(setpointRads)) + .withFeedForward(feedforward)); } @Override @@ -211,23 +189,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 9cde8c3d..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,10 +10,8 @@ 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.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; @@ -27,33 +25,31 @@ public class ArmIOSim implements ArmIO { armLength, minAngle.getRadians(), maxAngle.getRadians(), - true, + false, Units.degreesToRadians(0.0)); - private final ProfiledPIDController profiledController; - private ArmFeedforward ff; + private final PIDController controller; private double appliedVoltage = 0.0; private double positionOffset = 0.0; private boolean controllerNeedsReset = false; - private boolean closedLoop = false; + private boolean closedLoop = true; public ArmIOSim() { - ff = new ArmFeedforward(gains.ffkS(), gains.ffkG(), gains.ffkV(), gains.ffkA()); - profiledController = - new ProfiledPIDController(gains.kP(), gains.kI(), gains.kD(), profileConstraints, 0.001); - 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 public void updateInputs(ArmIOInputs inputs) { - sim.update(0.02); if (DriverStation.isDisabled()) { controllerNeedsReset = true; } + sim.update(0.02); + inputs.armPositionRads = sim.getAngleRads() + positionOffset; - inputs.armTrajectorySetpointRads = profiledController.getSetpoint().position; inputs.armVelocityRadsPerSec = sim.getVelocityRadPerSec(); inputs.armAppliedVolts = new double[] {appliedVoltage}; inputs.armCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; @@ -65,17 +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 - double feedback = profiledController.calculate(sim.getAngleRads(), setpointRads); - sim.setInputVoltage( - feedback + ff.calculate(sim.getAngleRads(), profiledController.getSetpoint().velocity)); + runVolts(controller.calculate(sim.getAngleRads(), setpointRads + positionOffset) + feedforward); } @Override @@ -87,19 +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 setFF(double s, double v, double a, double g) { - ff = new ArmFeedforward(s, g, v, a); - } - - @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..3c91508c 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 @@ -21,27 +21,25 @@ public class ArmVisualizer { private final Mechanism2d mechanism; - private final MechanismRoot2d root; private final MechanismLigament2d arm; private final String key; public ArmVisualizer(String key, Color color) { this.key = key; - mechanism = new Mechanism2d(3.0, 3.0, new Color8Bit(Color.kAntiqueWhite)); - root = mechanism.getRoot("pivot", 1.0, 0.4); + mechanism = new Mechanism2d(3.0, 3.0, new Color8Bit(Color.kWhite)); + MechanismRoot2d root = mechanism.getRoot("pivot", 1.0, 0.4); arm = new MechanismLigament2d("arm", armLength, 20.0, 6, new Color8Bit(color)); root.append(arm); } /** Update arm visualizer with current arm angle */ - public void update(Rotation2d angle) { - arm.setAngle(angle); + public void update(double angleRads) { + arm.setAngle(Rotation2d.fromRadians(angleRads)); Logger.recordOutput("Arm/" + key + "Mechanism2d", mechanism); // Log 3d poses Pose3d pivot = - new Pose3d( - armOrigin.getX(), 0.0, armOrigin.getY(), new Rotation3d(0.0, -angle.getRadians(), 0.0)); + new Pose3d(armOrigin.getX(), 0.0, armOrigin.getY(), new Rotation3d(0.0, -angleRads, 0.0)); Logger.recordOutput("Arm/" + key + "3d", pivot); } }