diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a08ad072..8da54fec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -206,8 +206,8 @@ private void configureButtonBindings() { // () -> -controller.getLeftX(), // () -> -controller.getRightX())); // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); - controller.a().onTrue(arm.home()); - arm.setDefaultCommand(arm.runToSetpoint()); + + // arm.setDefaultCommand(arm.runToSetpoint()); // controller // .a() // .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), 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 29fd7a6e..0eef6248 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Arm/Arm.java @@ -2,19 +2,20 @@ import static frc.robot.subsystems.superstructure.SuperstructureConstants.ArmConstants.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.drive.DriveConstants; import frc.robot.util.EqualsUtil; import frc.robot.util.LoggedTunableNumber; import java.util.function.BooleanSupplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Arm extends SubsystemBase { @@ -50,6 +51,12 @@ public class Arm extends SubsystemBase { private final MechanismRoot2d armRoot; private final MechanismLigament2d armMeasured; + private final Timer homingTimer = new Timer(); + + private boolean homed = false; + + private double setpoint = 0.0; + // private final MechanismLigament2d armSetpoint; public Arm(ArmIO io) { @@ -59,12 +66,16 @@ public Arm(ArmIO io) { // Create a mechanism armMechanism = new Mechanism2d(2, 3, new Color8Bit(Color.kAntiqueWhite)); - armRoot = armMechanism.getRoot("Arm Joint", armOrigin2d.getX(), armOrigin2d.getY()); + armRoot = + armMechanism.getRoot( + "Arm Joint", + armOrigin2d.getX() + DriveConstants.driveConfig.trackwidthX() / 2.0, + armOrigin2d.getY()); armMeasured = new MechanismLigament2d( "Arm Measured", armLength, - inputs.armAnglePosition.getDegrees(), + Units.radiansToDegrees(inputs.armAnglePositionRads), 2.0, new Color8Bit(Color.kBlack)); armRoot.append(armMeasured); @@ -88,13 +99,32 @@ public void periodic() { LoggedTunableNumber.ifChanged( hashCode(), () -> armIO.setPID(kP.get(), kI.get(), kD.get()), kP, kI, kD); LoggedTunableNumber.ifChanged( - hashCode(), () -> armIO.setFF(kS.get(), kV.get(), kA.get(), kG.get())); + hashCode(), () -> armIO.setFF(kS.get(), kV.get(), kA.get(), kG.get()), kS, kV, kA, kG); LoggedTunableNumber.ifChanged( hashCode(), constraints -> armIO.setProfileConstraints(constraints[0], constraints[1]), armVelocity, armAcceleration); + setpoint = Units.degreesToRadians(armDesiredSetpoint.get()); + // Home if not already homed + if (!homed && DriverStation.isEnabled()) { + armIO.setVoltage(-1.0); + if (EqualsUtil.epsilonEquals( + inputs.armVelocityRadsPerSec, 0.0, Units.degreesToRadians(1.0))) { + homingTimer.start(); + } else { + homingTimer.reset(); + } + + if (homingTimer.hasElapsed(0.5)) { + armIO.setPosition(0); + homed = true; + } + } else { + setSetpoint(setpoint); + } + if (DriverStation.isDisabled()) { armIO.stop(); } @@ -102,41 +132,18 @@ public void periodic() { if (coastSupplier.getAsBoolean()) armIO.setBrakeMode(false); // Logs - armMeasured.setAngle(inputs.armAnglePosition); + armMeasured.setAngle(Units.radiansToDegrees(inputs.armAnglePositionRads)); // armSetpoint.setAngle(inputs.armReferencePosition); - Logger.recordOutput("Arm/SetpointAngle", inputs.armReferencePosition); - Logger.recordOutput("Arm/MeasuredAngle", inputs.armAnglePosition); - Logger.recordOutput("Arm/VelocityRadsPerSec", inputs.armVelocityRadsPerSec); Logger.recordOutput("Arm/Mechanism", armMechanism); } - public Command runToSetpoint() { - return run( - () -> { - if (inputs.homed) { - Rotation2d setpoint = - Rotation2d.fromDegrees( - MathUtil.clamp( - armDesiredSetpoint.get(), minAngle.getDegrees(), maxAngle.getDegrees())); - setSetpoint(setpoint); - } - }); + public void setVoltage(double volts) { + armIO.setVoltage(volts); } - public Command home() { - return run(() -> armIO.setVoltage(-1.0)) - .until( - () -> { - boolean homed = EqualsUtil.epsilonEquals(inputs.armVelocityRadsPerSec, 0.0, 0.01); - System.out.println(homed); - return homed; - }) - .andThen(() -> armIO.setHome()); - } - - public void setSetpoint(Rotation2d setpoint) { - if (disableSupplier.getAsBoolean() || !inputs.homed) return; - armIO.setSetpoint(setpoint.getRadians()); + public void setSetpoint(double setpointRads) { + if (disableSupplier.getAsBoolean() || !homed) return; + armIO.setSetpoint(setpointRads); } public void setBrakeMode(boolean enabled) { @@ -151,4 +158,9 @@ public void setOverrides(BooleanSupplier disableSupplier, BooleanSupplier coastS public void stop() { armIO.stop(); } + + @AutoLogOutput(key = "Arm/Homed") + public boolean homed() { + return homed; + } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIO.java b/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIO.java index 91d03cfd..b176206d 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIO.java @@ -1,17 +1,15 @@ package frc.robot.subsystems.superstructure.Arm; -import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface ArmIO { - @AutoLog class ArmIOInputs { - public Rotation2d armAnglePosition = new Rotation2d(); - public Rotation2d armReferencePosition = new Rotation2d(); + public boolean hasFoc = false; + public boolean hasAbsoluteSensor = false; + public double armAnglePositionRads = 0.0; + public double armTrajectorySetpointRads = 0.0; public double armVelocityRadsPerSec = 0.0; - public boolean homed = false; - public boolean atSetpoint = false; public double[] armAppliedVolts = new double[] {}; public double[] armCurrentAmps = new double[] {}; public double[] armTorqueCurrentAmps = new double[] {}; @@ -26,6 +24,9 @@ default void setSetpoint(double setpointRads) {} /** Run motors at voltage */ default void setVoltage(double volts) {} + /** Run motors at current */ + default void setCurrent(double amps) {} + /** Set brake mode enabled */ default void setBrakeMode(boolean enabled) {} @@ -39,17 +40,8 @@ default void setPID(double p, double i, double d) {} default void setProfileConstraints( double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) {} - /** - * Stops motors - * - * @params none! - */ - default void stop() {} + default void setPosition(double positionRads) {} - /** - * Set current position to home - * - * @params none! - */ - default void setHome() {} + /** Stops motors */ + default void stop() {} } 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 b5515ec6..e9daf83c 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOKrakenFOC.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOKrakenFOC.java @@ -14,15 +14,13 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; public class ArmIOKrakenFOC implements ArmIO { private final TalonFX leaderMotor; private final TalonFX followerMotor; - Double home = null; private final StatusSignal armPositionRotations; - private final StatusSignal armReferencePositionRotations; + private final StatusSignal armTrajectorySetpointPositionRotations; private final StatusSignal armVelocityRPS; private final StatusSignal[] armAppliedVoltage = new StatusSignal[2]; private final StatusSignal[] armOutputCurrent = new StatusSignal[2]; @@ -44,7 +42,7 @@ public ArmIOKrakenFOC() { leaderConfig.Voltage.PeakReverseVoltage = 12.0; leaderConfig.MotorOutput.Inverted = leaderInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - leaderConfig.Feedback.RotorToSensorRatio = 1.0 / reduction; + leaderConfig.Feedback.SensorToMechanismRatio = reduction; leaderConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; controllerConfig = @@ -63,7 +61,7 @@ public ArmIOKrakenFOC() { new MotionMagicConfigs() .withMotionMagicCruiseVelocity( Units.radiansToRotations(profileConstraints.cruiseVelocityRadPerSec())) - .withMotionMagicCruiseVelocity( + .withMotionMagicAcceleration( Units.radiansToRotations(profileConstraints.accelerationRadPerSec2())); leaderMotor.getConfigurator().apply(leaderConfig, 1); @@ -72,7 +70,7 @@ public ArmIOKrakenFOC() { followerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; // Status signals armPositionRotations = leaderMotor.getPosition(); - armReferencePositionRotations = leaderMotor.getClosedLoopReference(); + armTrajectorySetpointPositionRotations = leaderMotor.getClosedLoopReference(); armVelocityRPS = leaderMotor.getVelocity(); armAppliedVoltage[0] = leaderMotor.getMotorVoltage(); armAppliedVoltage[1] = followerMotor.getMotorVoltage(); @@ -84,9 +82,9 @@ public ArmIOKrakenFOC() { armTempCelsius[1] = followerMotor.getDeviceTemp(); BaseStatusSignal.setUpdateFrequencyForAll( - 50, + 100, armPositionRotations, - armReferencePositionRotations, + armTrajectorySetpointPositionRotations, armVelocityRPS, armAppliedVoltage[0], armAppliedVoltage[1], @@ -99,9 +97,11 @@ public ArmIOKrakenFOC() { } public void updateInputs(ArmIOInputs inputs) { + inputs.hasFoc = true; + BaseStatusSignal.refreshAll( armPositionRotations, - armReferencePositionRotations, + armTrajectorySetpointPositionRotations, armVelocityRPS, armAppliedVoltage[0], armAppliedVoltage[1], @@ -112,9 +112,9 @@ public void updateInputs(ArmIOInputs inputs) { armTempCelsius[0], armTempCelsius[1]); - inputs.armAnglePosition = Rotation2d.fromRotations(armPositionRotations.getValue() - home); - inputs.armReferencePosition = - Rotation2d.fromRotations(armReferencePositionRotations.getValue() - home); + inputs.armAnglePositionRads = Units.rotationsToRadians(armPositionRotations.getValue()); + inputs.armTrajectorySetpointRads = + Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue()); inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRPS.getValue()); inputs.armAppliedVolts = new double[] {armAppliedVoltage[0].getValue(), armAppliedVoltage[1].getValue()}; @@ -124,7 +124,6 @@ public void updateInputs(ArmIOInputs inputs) { new double[] {armTorqueCurrent[0].getValue(), armTorqueCurrent[1].getValue()}; inputs.armTempCelcius = new double[] {armTempCelsius[0].getValue(), armTempCelsius[1].getValue()}; - inputs.homed = home != null; } @Override @@ -172,8 +171,7 @@ public void setProfileConstraints( } @Override - public void setHome() { - home = armPositionRotations.getValue(); - leaderMotor.setPosition(0.0, 0.01); + public void setPosition(double positionRads) { + leaderMotor.setPosition(Units.radiansToRotations(positionRads)); } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOSim.java index c6471bd5..e6f59228 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Arm/ArmIOSim.java @@ -5,10 +5,10 @@ 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.geometry.Rotation2d; 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; public class ArmIOSim implements ArmIO { @@ -26,8 +26,10 @@ public class ArmIOSim implements ArmIO { private final ProfiledPIDController profiledController; private ArmFeedforward ff; private double appliedVoltage = 0.0; - private boolean usingVoltageControl = false; - private boolean homed = false; + private double positionOffset = 0.0; + + private boolean controllerNeedsReset = false; + private boolean closedLoop = false; public ArmIOSim() { ff = @@ -43,50 +45,56 @@ public ArmIOSim() { controllerConstants.kD(), new TrapezoidProfile.Constraints( profileConstraints.cruiseVelocityRadPerSec(), - profileConstraints.accelerationRadPerSec2())); + profileConstraints.accelerationRadPerSec2()), + 0.001); sim.setState(Units.degreesToRadians(45.0), 0.0); } @Override public void updateInputs(ArmIOInputs inputs) { - sim.update(0.02); + if (DriverStation.isDisabled()) { + controllerNeedsReset = true; + } - inputs.armAnglePosition = Rotation2d.fromRadians(sim.getAngleRads()); - inputs.armReferencePosition = Rotation2d.fromRadians(profiledController.getGoal().position); + if (controllerNeedsReset) { + profiledController.reset(sim.getAngleRads() + positionOffset, sim.getVelocityRadPerSec()); + controllerNeedsReset = false; + } + + for (int i = 0; i < 20; i++) { + // control + if (closedLoop) { + appliedVoltage = + profiledController.calculate(sim.getAngleRads() + positionOffset) + + ff.calculate( + profiledController.getSetpoint().position, + profiledController.getSetpoint().velocity); + sim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + } + sim.update(0.001); + } + + inputs.armAnglePositionRads = sim.getAngleRads() + positionOffset; + inputs.armTrajectorySetpointRads = profiledController.getSetpoint().position; inputs.armVelocityRadsPerSec = sim.getVelocityRadPerSec(); inputs.armAppliedVolts = new double[] {appliedVoltage}; inputs.armCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; inputs.armTorqueCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; inputs.armTempCelcius = new double[] {0.0}; - - inputs.homed = homed; - - // control - if (!usingVoltageControl) { - appliedVoltage = - profiledController.calculate(sim.getAngleRads()) - + ff.calculate( - profiledController.getSetpoint().position, - profiledController.getSetpoint().velocity); - System.out.println( - "Open loop voltage: " - + ff.calculate( - profiledController.getSetpoint().position, - profiledController.getSetpoint().velocity)); - appliedVoltage = MathUtil.clamp(appliedVoltage, -12.0, 12.0); - sim.setInputVoltage(appliedVoltage); - } } @Override public void setSetpoint(double setpointRads) { - usingVoltageControl = false; + if (!closedLoop) { + controllerNeedsReset = true; + } + closedLoop = true; profiledController.setGoal(setpointRads); } @Override public void setVoltage(double volts) { - usingVoltageControl = true; + closedLoop = false; appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); sim.setInputVoltage(appliedVoltage); } @@ -115,7 +123,7 @@ public void stop() { } @Override - public void setHome() { - homed = true; + public void setPosition(double position) { + positionOffset = position - sim.getAngleRads(); } } diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java index 2804fcb7..0b76c60d 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java @@ -54,7 +54,7 @@ public static class IntakeConstants { public static class ArmConstants { // reduction is 12:62 18:60 12:65 - public static double reduction = (93.2870); + 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 armOrigin2d = new Translation2d(-Units.inchesToMeters(9.37), Units.inchesToMeters(11.75));