From 7559b70436e937d4015d64a4948dda42d5c738dd Mon Sep 17 00:00:00 2001 From: suryatho Date: Tue, 6 Feb 2024 20:55:30 -0500 Subject: [PATCH] Devbot --- .../subsystems/superstructure/Arm/Arm.java | 41 +++------------ .../superstructure/Arm/ArmIOKrakenFOC.java | 12 +++-- .../superstructure/DevBotSuperstructure.java | 52 +++++++++++++++++++ 3 files changed, 67 insertions(+), 38 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/superstructure/DevBotSuperstructure.java diff --git a/src/main/java/frc/robot/subsystems/superstructure/Arm/Arm.java b/src/main/java/frc/robot/subsystems/superstructure/Arm/Arm.java index 6512511c..3fff4538 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Arm/Arm.java @@ -1,9 +1,7 @@ package frc.robot.subsystems.superstructure.Arm; -import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.superstructure.SuperstructureConstants.ArmConstants.*; -import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -14,7 +12,6 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.EqualsUtil; import frc.robot.util.LoggedTunableNumber; @@ -58,11 +55,8 @@ public class Arm extends SubsystemBase { private final Timer homingTimer = new Timer(); private boolean homed = false; - private double setpoint = 0.0; - private SysIdRoutine routine; - // private final MechanismLigament2d armSetpoint; public Arm(ArmIO io) { @@ -93,23 +87,6 @@ public Arm(ArmIO io) { // 10.0, // new Color8Bit(Color.kGreen)); // armRoot.append(armSetpoint); - - routine = - new SysIdRoutine( - new SysIdRoutine.Config( - Volts.of(0.05).per(Seconds.of(1.0)), - Volts.of(4.0), - Seconds.of(30.0), - state -> { - SignalLogger.writeString("SysIdState", state.toString()); - }), - new SysIdRoutine.Mechanism( - voltageMeasure -> armIO.setVoltage(voltageMeasure.in(Volts)), - sysIdRoutineLog -> { - Logger.recordOutput("Arm/SysIdVoltage", inputs.armAppliedVolts[0]); - Logger.recordOutput("Arm/SysIdCurrent", inputs.armTorqueCurrentAmps[0]); - }, - this)); } @Override @@ -129,7 +106,6 @@ public void periodic() { armVelocity, armAcceleration); - setpoint = Units.degreesToRadians(armDesiredSetpoint.get()); // Home if not already homed if (!homed && DriverStation.isEnabled()) { armIO.setVoltage(-1.0); @@ -145,7 +121,7 @@ public void periodic() { homed = true; } } else { - setSetpoint(setpoint); + armIO.setSetpoint(setpoint); } if (DriverStation.isDisabled()) { @@ -166,7 +142,6 @@ public void setVoltage(double volts) { public void setSetpoint(double setpointRads) { if (disableSupplier.getAsBoolean() || !homed) return; - armIO.setSetpoint(setpointRads); } public void setBrakeMode(boolean enabled) { @@ -182,15 +157,6 @@ public void stop() { armIO.stop(); } - // SysId command factories - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return routine.quasistatic(direction).beforeStarting(SignalLogger::start); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return routine.dynamic(direction).finallyDo(SignalLogger::stop); - } - public Command getStaticCurrent() { Timer timer = new Timer(); return run(() -> armIO.setCurrent(0.5 * timer.get())) @@ -204,4 +170,9 @@ public Command getStaticCurrent() { public boolean homed() { return homed; } + + @AutoLogOutput(key = "Arm/AtSetpoint") + public boolean atSetpoint() { + return Math.abs(inputs.armAnglePositionRads - setpoint) <= armTolerance.get(); + } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOKrakenFOC.java b/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOKrakenFOC.java index 021066cf..181562e0 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOKrakenFOC.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOKrakenFOC.java @@ -113,11 +113,17 @@ public void updateInputs(ArmIOInputs inputs) { Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue()); inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRPS.getValue()); inputs.armAppliedVolts = - new double[] {armAppliedVoltage[0].getValueAsDouble(), armAppliedVoltage[1].getValueAsDouble()}; + new double[] { + armAppliedVoltage[0].getValueAsDouble(), armAppliedVoltage[1].getValueAsDouble() + }; inputs.armCurrentAmps = - new double[] {armOutputCurrent[0].getValueAsDouble(), armOutputCurrent[1].getValueAsDouble()}; + new double[] { + armOutputCurrent[0].getValueAsDouble(), armOutputCurrent[1].getValueAsDouble() + }; inputs.armTorqueCurrentAmps = - new double[] {armTorqueCurrent[0].getValueAsDouble(), armTorqueCurrent[1].getValueAsDouble()}; + new double[] { + armTorqueCurrent[0].getValueAsDouble(), armTorqueCurrent[1].getValueAsDouble() + }; inputs.armTempCelcius = new double[] {armTempCelsius[0].getValueAsDouble(), armTempCelsius[1].getValueAsDouble()}; } diff --git a/src/main/java/frc/robot/subsystems/superstructure/DevBotSuperstructure.java b/src/main/java/frc/robot/subsystems/superstructure/DevBotSuperstructure.java new file mode 100644 index 00000000..a0292a88 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/DevBotSuperstructure.java @@ -0,0 +1,52 @@ +package frc.robot.subsystems.superstructure; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.superstructure.Arm.Arm; +import frc.robot.subsystems.superstructure.shooter.Shooter; +import lombok.Getter; +import org.littletonrobotics.junction.Logger; + +public class DevBotSuperstructure extends SubsystemBase { + public enum State { + SHOOTING, + INTAKING, + IDLE + } + + @Getter private State currentState = State.IDLE; + private final Arm arm; + private final Shooter shooter; + + public DevBotSuperstructure(Arm arm, Shooter shooter) { + this.arm = arm; + this.shooter = shooter; + } + + @Override + public void periodic() { + switch (currentState) { + case IDLE -> { + arm.setSetpoint(Units.degreesToRadians(5.0)); + shooter.stop(); + } + case INTAKING -> { + arm.setSetpoint(Units.degreesToRadians(50.0)); + if (arm.atSetpoint()) { + shooter.setIntaking(); + } + } + case SHOOTING -> { + if (arm.atSetpoint()) { + shooter.setShooting(); + } + } + } + + Logger.recordOutput("DevBotSuperstructure/currentState", currentState.toString()); + } + + public void setDesiredState(State desiredState) { + currentState = desiredState; + } +}