From b6e8025527306937d90a915aacb9c9aa3ae73f7e Mon Sep 17 00:00:00 2001 From: suryatho Date: Fri, 9 Feb 2024 18:28:50 -0500 Subject: [PATCH] Add absolute encoder to arm Add to DevBotSuperstructure --- .../frc2024/RobotContainer.java | 14 ++-- .../superstructure/DevBotSuperstructure.java | 54 +++++++++++----- .../superstructure/arm/ArmIOKrakenFOC.java | 10 +-- .../feeder/FeederIOKrakenFOC.java | 64 +++++++++++++++++++ .../superstructure/feeder/FeederIOSim.java | 19 ++++-- 5 files changed, 128 insertions(+), 33 deletions(-) create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOKrakenFOC.java diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 3c4828f9..6a24b45a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -200,10 +200,11 @@ private void configureButtonBindings() { .leftTrigger() .onTrue( Commands.runOnce( - () -> superstructure.setDesiredState(DevBotSuperstructure.State.PREPARE_SHOOT))) + () -> + superstructure.setGoalState(DevBotSuperstructure.SystemState.PREPARE_SHOOT))) .onFalse( Commands.runOnce( - () -> superstructure.setDesiredState(DevBotSuperstructure.State.IDLE))); + () -> superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE))); Trigger readyToShootTrigger = new Trigger(() -> drive.isAutoAimGoalCompleted() && superstructure.atShootingSetpoint()); @@ -219,20 +220,21 @@ private void configureButtonBindings() { .and(readyToShootTrigger) .onTrue( Commands.runOnce( - () -> superstructure.setDesiredState(DevBotSuperstructure.State.SHOOT)) + () -> superstructure.setGoalState(DevBotSuperstructure.SystemState.SHOOT)) .andThen(Commands.waitSeconds(0.5)) .andThen( Commands.runOnce( - () -> superstructure.setDesiredState(DevBotSuperstructure.State.IDLE)))); + () -> + superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE)))); controller .leftBumper() .onTrue( Commands.runOnce( - () -> superstructure.setDesiredState(DevBotSuperstructure.State.INTAKE))) + () -> superstructure.setGoalState(DevBotSuperstructure.SystemState.INTAKE))) .onFalse( Commands.runOnce( - () -> superstructure.setDesiredState(DevBotSuperstructure.State.IDLE))); + () -> superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE))); } controller diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java index f908f422..392a1b85 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java @@ -1,8 +1,10 @@ package org.littletonrobotics.frc2024.subsystems.superstructure; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; +import lombok.Setter; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; @@ -25,16 +27,17 @@ public class DevBotSuperstructure extends SubsystemBase { private static LoggedTunableNumber yCompensation = new LoggedTunableNumber("DevBotSuperstructure/CompensationInches", 6.0); private static LoggedTunableNumber followThroughTime = - new LoggedTunableNumber("DevBotSuperstructure/FollowthroughTimeSecs", 1.0); + new LoggedTunableNumber("DevBotSuperstructure/FollowthroughTimeSecs", 0.5); - public enum State { + public enum SystemState { PREPARE_SHOOT, SHOOT, INTAKE, IDLE } - @Getter private State currentState = State.IDLE; + @Getter private SystemState currentState = SystemState.IDLE; + @Getter @Setter private SystemState goalState = SystemState.IDLE; private final Arm arm; private final Flywheels flywheels; @@ -48,27 +51,48 @@ public DevBotSuperstructure(Arm arm, Flywheels flywheels) { @Override public void periodic() { + switch (goalState) { + case IDLE -> { + if (currentState == SystemState.SHOOT) { + if (followThroughTimer.hasElapsed(followThroughTime.get())) { + currentState = SystemState.IDLE; + followThroughTimer.stop(); + followThroughTimer.reset(); + } else { + currentState = SystemState.SHOOT; + } + } else { + currentState = SystemState.IDLE; + } + } + case INTAKE -> currentState = SystemState.INTAKE; + case PREPARE_SHOOT -> currentState = SystemState.PREPARE_SHOOT; + case SHOOT -> { + if (currentState != SystemState.PREPARE_SHOOT) { + currentState = SystemState.PREPARE_SHOOT; + } else if (atShootingSetpoint()) { + currentState = SystemState.SHOOT; + followThroughTimer.restart(); + } + } + } + switch (currentState) { - case IDLE -> {} + case IDLE -> { + arm.setSetpoint(Rotation2d.fromDegrees(armIdleSetpointDegrees.get())); + flywheels.runVolts(0.0, 0.0); + } case INTAKE -> {} case PREPARE_SHOOT -> {} case SHOOT -> {} } - Logger.recordOutput("DevBotSuperstructure/currentState", currentState.toString()); + Logger.recordOutput("DevBotSuperstructure/GoalState", goalState); + Logger.recordOutput("DevBotSuperstructure/currentState", currentState); } @AutoLogOutput(key = "DevBotSuperstructure/ReadyToShoot") public boolean atShootingSetpoint() { - return (currentState == State.PREPARE_SHOOT || currentState == State.SHOOT) - && arm.atSetpoint() - && flywheels.atSetpoint(); - } - - public void setDesiredState(State desiredState) { - if (desiredState == State.PREPARE_SHOOT && currentState == State.SHOOT) { - return; - } - currentState = desiredState; + return arm.atSetpoint() && flywheels.atSetpoint(); } } 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 7d3871ea..e09ff844 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 @@ -23,7 +23,7 @@ public class ArmIOKrakenFOC implements ArmIO { private final StatusSignal armPositionRotations; private final StatusSignal armAbsolutePositionRotations; private final StatusSignal armTrajectorySetpointPositionRotations; - private final StatusSignal armVelocityRPS; + private final StatusSignal armVelocityRps; private final List> armAppliedVoltage; private final List> armOutputCurrent; private final List> armTorqueCurrent; @@ -85,7 +85,7 @@ public ArmIOKrakenFOC() { armPositionRotations = leaderMotor.getPosition(); armAbsolutePositionRotations = armEncoder.getPosition(); armTrajectorySetpointPositionRotations = leaderMotor.getClosedLoopReference(); - armVelocityRPS = leaderMotor.getVelocity(); + armVelocityRps = leaderMotor.getVelocity(); armAppliedVoltage = List.of(leaderMotor.getMotorVoltage(), followerMotor.getMotorVoltage()); armOutputCurrent = List.of(leaderMotor.getSupplyCurrent(), followerMotor.getSupplyCurrent()); armTorqueCurrent = List.of(leaderMotor.getTorqueCurrent(), followerMotor.getTorqueCurrent()); @@ -96,7 +96,7 @@ public ArmIOKrakenFOC() { armPositionRotations, armAbsolutePositionRotations, armTrajectorySetpointPositionRotations, - armVelocityRPS, + armVelocityRps, armAppliedVoltage.get(0), armAppliedVoltage.get(1), armOutputCurrent.get(0), @@ -113,7 +113,7 @@ public void updateInputs(ArmIOInputs inputs) { BaseStatusSignal.refreshAll( armPositionRotations, armTrajectorySetpointPositionRotations, - armVelocityRPS, + armVelocityRps, armAppliedVoltage.get(0), armAppliedVoltage.get(1), armOutputCurrent.get(0), @@ -126,7 +126,7 @@ public void updateInputs(ArmIOInputs inputs) { inputs.armPositionRads = Units.rotationsToRadians(armPositionRotations.getValue()); inputs.armTrajectorySetpointRads = Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue()); - inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRPS.getValue()); + inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRps.getValue()); inputs.armAppliedVolts = armAppliedVoltage.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); inputs.armCurrentAmps = diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOKrakenFOC.java new file mode 100644 index 00000000..583251d8 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOKrakenFOC.java @@ -0,0 +1,64 @@ +package org.littletonrobotics.frc2024.subsystems.superstructure.feeder; + +import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FeederConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.util.Units; + +public class FeederIOKrakenFOC implements FeederIO { + private final TalonFX motor; + + private StatusSignal position; + private StatusSignal velocity; + private StatusSignal appliedVoltage; + private StatusSignal outputCurrent; + + public FeederIOKrakenFOC() { + motor = new TalonFX(id, "canivore"); + + TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = + inverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.CurrentLimits.SupplyCurrentLimit = 30.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + motor.getConfigurator().apply(config); + + position = motor.getPosition(); + velocity = motor.getVelocity(); + appliedVoltage = motor.getMotorVoltage(); + outputCurrent = motor.getTorqueCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, position, velocity, appliedVoltage, outputCurrent); + } + + @Override + public void updateInputs(FeederIOInputs inputs) { + BaseStatusSignal.refreshAll(position, velocity, appliedVoltage, outputCurrent); + + inputs.positionRads = Units.rotationsToRadians(position.getValueAsDouble()) / reduction; + inputs.velocityRadsPerSec = Units.rotationsToRadians(velocity.getValueAsDouble()) / reduction; + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.outputCurrent = outputCurrent.getValueAsDouble(); + } + + @Override + public void runVolts(double volts) { + motor.setControl(new VoltageOut(volts).withEnableFOC(true)); + } + + @Override + public void stop() { + motor.setControl(new NeutralOut()); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java index bfeb9993..659ade9f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSim.java @@ -2,27 +2,32 @@ import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FeederConstants.*; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.FlywheelSim; public class FeederIOSim implements FeederIO { - private final FlywheelSim sim = new FlywheelSim( - DCMotor.getKrakenX60Foc(1), - reduction, - 0.01); + private final FlywheelSim sim = new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.01); + + private double appliedVoltage = 0.0; @Override public void updateInputs(FeederIOInputs inputs) { - FeederIO.super.updateInputs(inputs); + sim.update(0.02); + inputs.positionRads += sim.getAngularVelocityRadPerSec() * 0.02; + inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + inputs.outputCurrent = sim.getCurrentDrawAmps(); } @Override public void runVolts(double volts) { - FeederIO.super.runVolts(volts); + appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + sim.setInputVoltage(appliedVoltage); } @Override public void stop() { - FeederIO.super.stop(); + runVolts(0.0); } }