From 2a71b0ccb48cdb5494c0fbeeef4eb14357e72fb0 Mon Sep 17 00:00:00 2001 From: suryatho Date: Wed, 14 Feb 2024 22:53:06 -0500 Subject: [PATCH] Fix Alerts in ArmIOKrakenFOC.java Visualizer working --- Robot_MA24B/config.json | 85 +++++++++++++++++++ build.gradle | 2 +- .../littletonrobotics/frc2024/Constants.java | 2 +- .../frc2024/RobotContainer.java | 5 +- .../subsystems/superstructure/arm/Arm.java | 40 +++++---- .../superstructure/arm/ArmConstants.java | 3 +- .../superstructure/arm/ArmIOKrakenFOC.java | 22 ++--- .../superstructure/arm/ArmVisualizer.java | 2 +- 8 files changed, 122 insertions(+), 39 deletions(-) create mode 100644 Robot_MA24B/config.json diff --git a/Robot_MA24B/config.json b/Robot_MA24B/config.json new file mode 100644 index 00000000..8e4c1283 --- /dev/null +++ b/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 POV", + "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 POV", + "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/build.gradle b/build.gradle index 035e7648..1e81e373 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/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java index d19fc867..6bebedc2 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Constants.java +++ b/src/main/java/org/littletonrobotics/frc2024/Constants.java @@ -21,7 +21,7 @@ */ public final class Constants { public static final int loopPeriodMs = 20; - private static RobotType robotType = RobotType.SIMBOT; + private static RobotType robotType = RobotType.DEVBOT; public static final boolean tuningMode = true; public static RobotType getRobot() { diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 3b3172fa..b654c2c4 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -19,6 +19,7 @@ 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; @@ -179,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( 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 2f890fe5..1d4aa3c0 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 @@ -9,6 +9,7 @@ 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; @@ -32,29 +33,30 @@ 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 = + private static final LoggedTunableNumber lowerLimitDegrees = new LoggedTunableNumber("Arm/LowerLimitDegrees", minAngle.getDegrees()); - private static final LoggedTunableNumber armUpperLimit = + private static final LoggedTunableNumber upperLimitDegrees = new LoggedTunableNumber("Arm/UpperLimitDegrees", maxAngle.getDegrees()); - private static final LoggedTunableNumber armStowDegrees = + private static final LoggedTunableNumber stowDegrees = new LoggedTunableNumber("Superstructure/ArmStowDegrees", 20.0); - private static final LoggedTunableNumber armStationIntakeDegrees = + private static final LoggedTunableNumber stationIntakeDegrees = new LoggedTunableNumber("Superstructure/ArmStationIntakeDegrees", 45.0); - private static final 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; @@ -65,7 +67,6 @@ private double getRads() { @Getter @Setter private Goal goal = Goal.STOW; private boolean characterizing = false; - private boolean homed = false; private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); @@ -82,7 +83,7 @@ public Arm(ArmIO io) { motionProfile = new TrapezoidProfile( - new TrapezoidProfile.Constraints(armVelocity.get(), armAcceleration.get())); + 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()); @@ -112,8 +113,8 @@ public void periodic() { motionProfile = new TrapezoidProfile( new TrapezoidProfile.Constraints(constraints[0], constraints[1])), - armVelocity, - armAcceleration); + maxVelocity, + maxAcceleration); if (!characterizing) { // Run closed loop @@ -121,7 +122,12 @@ public void periodic() { motionProfile.calculate( 0.02, new TrapezoidProfile.State(inputs.armPositionRads, inputs.armVelocityRadsPerSec), - new TrapezoidProfile.State(goal.getRads(), 0.0)); + new TrapezoidProfile.State( + MathUtil.clamp( + goal.getRads(), + Units.degreesToRadians(lowerLimitDegrees.get()), + Units.degreesToRadians(upperLimitDegrees.get())), + 0.0)); io.runSetpoint(output.position, ff.calculate(output.position, output.velocity)); @@ -152,7 +158,7 @@ public Rotation2d getSetpoint() { @AutoLogOutput(key = "Arm/AtSetpoint") public boolean atSetpoint() { return Math.abs(inputs.armPositionRads - goal.getRads()) - <= Units.degreesToRadians(armToleranceDegreees.get()); + <= Units.degreesToRadians(toleranceDegrees.get()); } // 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 f063c837..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); 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 1c4ea866..edac6c4d 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); @@ -117,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) { @@ -134,14 +127,19 @@ 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 = @@ -157,12 +155,6 @@ 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 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 3afef219..52af0e95 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 @@ -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); } }