diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java index 3c237952..984facff 100644 --- a/src/main/java/frc/robot/Auto.java +++ b/src/main/java/frc/robot/Auto.java @@ -229,7 +229,7 @@ public int getTagByTeam() { public enum ArmHeight { Home(Position.Home, Position.Home), - L1(Position.L1, Position.L1Prep), + L1(Position.L1, Position.L1), L2(Position.L2, Position.L2Prep), L3(Position.L3, Position.L3Prep), L4(Position.L4, Position.L4Prep), diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0753bf4d..e84838f3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -36,7 +36,8 @@ public Robot() { Logger.addDataReceiver(new WPILOGWriter("/home/lvuser/logs")); Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables PowerDistribution distribution = - new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging + new PowerDistribution( + 33, ModuleType.kRev); // Enables power distribution logging } else { setUseTiming(true); // Run as fast as possible Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c2e25ec3..5961f739 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,8 +40,6 @@ import frc.robot.subsystems.chute.ChuteIOSim; import frc.robot.subsystems.chute.ChuteSubsystem; import frc.robot.subsystems.climber.ClimberHeadIONeo550; -import frc.robot.subsystems.climber.ClimberHeadIOSim; -import frc.robot.subsystems.climber.ClimberIOSim; import frc.robot.subsystems.climber.ClimberIOTalonFX; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.elevator.ElevatorIOSim; @@ -55,8 +53,8 @@ import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.lights.LightsSubsystem; import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; +import frc.robot.subsystems.vision.DummyPhotonCamera; import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIOLimelight; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; import frc.robot.subsystems.vision.VisionIOPhotonVisionSimML; import frc.robot.subsystems.wrist.WristIONeo550; @@ -107,23 +105,31 @@ public RobotContainer() { vision = new Vision( drivetrain::addVisionMeasurement, - new VisionIOLimelight( - VisionConstants.camera0Name, - () -> drivetrain.getRobotPose().getRotation()), - new VisionIOLimelight( - VisionConstants.camera1Name, - () -> drivetrain.getRobotPose().getRotation()), - new VisionIOLimelight( - VisionConstants.camera2Name, - () -> drivetrain.getRobotPose().getRotation())); - gripperSubsystem = new GripperSubsystem(new GripperIOFalcon()); - elevatorSubsystem = new ElevatorSubsystem(new ElevatorIOTalonFX()); + new DummyPhotonCamera(), + new DummyPhotonCamera(), + new DummyPhotonCamera()); + // new VisionIOLimelight( + // VisionConstants.camera0Name, + // () -> drivetrain.getRobotPose().getRotation()), + // new VisionIOLimelight( + // VisionConstants.camera1Name, + // () -> drivetrain.getRobotPose().getRotation()), + // new VisionIOLimelight( + // VisionConstants.camera2Name, + // () -> drivetrain.getRobotPose().getRotation())); + gripperSubsystem = + new GripperSubsystem(new GripperIOFalcon()); // new GripperIOFalcon()); + elevatorSubsystem = + new ElevatorSubsystem(new ElevatorIOTalonFX()); // new ElevatorIOTalonFX()); armSubsystem = new ArmSubsystem(new ArmPivotIOTalonFX()); - wristSubsystem = new WristSubsystem(new WristIONeo550()); + wristSubsystem = new WristSubsystem(new WristIONeo550()); // new WristIONeo550()); climberSubsystem = - new ClimberSubsystem(new ClimberIOTalonFX(), new ClimberHeadIONeo550()); + new ClimberSubsystem( + new ClimberIOTalonFX(), + new ClimberHeadIONeo550()); // new ClimberIOTalonFX(), new + // ClimberHeadIONeo550()); lights = new LightsSubsystem(); - chuteSubsystem = new ChuteSubsystem(new ChuteIONeo550()); + chuteSubsystem = new ChuteSubsystem(new ChuteIONeo550()); // new ChuteIONeo550()); intakeSubsystem = new IntakeSubsystem(new IntakeRollerIOSim(), new FlipperIOSim()); } else { @@ -148,7 +154,8 @@ public RobotContainer() { armSubsystem = new ArmSubsystem(new ArmPivotIOSim()); wristSubsystem = new WristSubsystem(new WristIOSim()); intakeSubsystem = new IntakeSubsystem(new IntakeRollerIOSim(), new FlipperIOSim()); - climberSubsystem = new ClimberSubsystem(new ClimberIOSim(), new ClimberHeadIOSim()); + climberSubsystem = + new ClimberSubsystem(new ClimberIOTalonFX(), new ClimberHeadIONeo550()); lights = new LightsSubsystem(); chuteSubsystem = new ChuteSubsystem(new ChuteIOSim()); } @@ -158,6 +165,10 @@ public RobotContainer() { elevatorSubsystem, armSubsystem, wristSubsystem, chuteSubsystem); auto = new Auto(drivetrain, this); + + wristSubsystem.elevatorHeight = () -> elevatorSubsystem.getPosition(); + wristSubsystem.armHeight = () -> armSubsystem.getPosition(); + configureBindings(); // Establish the "Trajectory Field" Field2d into the dashboard } @@ -385,12 +396,18 @@ private void configureBindings() { ARMWRIST.and(operatorController.getX()).whileTrue(wristSubsystem.turnWristLeft()); ARMWRIST.and(operatorController.getB()).whileTrue(wristSubsystem.turnWristRight()); - CORAL.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.L4)); - CORAL.and(operatorController.getX()).onTrue(stateManager.moveToPosition(Position.L3)); - CORAL.and(operatorController.getB()).onTrue(stateManager.moveToPosition(Position.L2)); + CORAL.and(operatorController.getY()) + .onTrue(stateManager.moveToPosition(Position.L4Prep)) + .onFalse(stateManager.moveToPosition(Position.L4)); + CORAL.and(operatorController.getX()) + .onTrue(stateManager.moveToPosition(Position.L3Prep)) + .onFalse(stateManager.moveToPosition(Position.L3)); + CORAL.and(operatorController.getB()) + .onTrue(stateManager.moveToPosition(Position.L2Prep)) + .onFalse(stateManager.moveToPosition(Position.L2)); CORAL.and(operatorController.getA()).onTrue(stateManager.moveToPosition(Position.L1)); - operatorController.getLeftTrigger().onTrue(stateManager.moveToPosition(Position.Climb)); + operatorController.getLeftTrigger().onTrue(stateManager.moveToPosition(Position.Start)); bindPlaceSeq(operatorController.getY(), Position.L4Prep, Position.L4, 0.1); @@ -398,7 +415,7 @@ private void configureBindings() { bindPlaceSeq(operatorController.getB(), Position.L2Prep, Position.L2, 0.1); - bindPlaceSeq(operatorController.getA(), Position.L1Prep, Position.L1, 0.1); + operatorController.getA().onTrue(stateManager.moveToPosition(Position.L1)); CORAL.and(operatorController.getStart()) .onTrue(stateManager.moveToPosition(Position.Source)); @@ -406,19 +423,20 @@ private void configureBindings() { CORAL.and(operatorController.getDPadDown()) .onTrue(stateManager.moveToPosition(Position.Home)); CORAL.and(operatorController.getDPadUp()) - .onTrue(stateManager.moveToPosition(Position.Handoff)); + .onTrue(stateManager.moveToPosition(Position.HandoffPrep)) + .onFalse(stateManager.moveToPosition(Position.Handoff)); CORAL.and(operatorController.getDPadLeft()).onTrue(chuteSubsystem.moveChuteUp()); CORAL.and(operatorController.getDPadRight()).onTrue(chuteSubsystem.moveChuteDown()); - ALGAE.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.NetAlgae)); + // ALGAE.and(operatorController.getY()).onTrue(stateManager.moveToPosition(Position.NetAlgae)); ALGAE.and(operatorController.getX()).onTrue(stateManager.moveToPosition(Position.L3Algae)); ALGAE.and(operatorController.getB()).onTrue(stateManager.moveToPosition(Position.L2Algae)); ALGAE.and(operatorController.getA()) .onTrue(stateManager.moveToPosition(Position.Processor)); ALGAE.and(operatorController.getStart()) - .onTrue(stateManager.moveToPosition(Position.Processor)); + .onTrue(stateManager.moveToPosition(Position.GroundAlgae)); ALGAE.and(operatorController.getDPadDown()) - .onTrue(stateManager.moveToPosition(Position.Home)); + .onTrue(stateManager.moveToPosition(Position.AlgaeHome)); ALGAE.and(operatorController.getDPadUp()) .onTrue(stateManager.moveToPosition(Position.Handoff)); ALGAE.and(operatorController.getDPadLeft()) @@ -426,14 +444,14 @@ private void configureBindings() { ALGAE.and(operatorController.getDPadRight()) .onTrue(stateManager.moveToPosition(Position.Quick23)); - operatorController.getBack().onTrue(wristSubsystem.flipWristPosition()); + // operatorController.getBack().onTrue(wristSubsystem.flipWristPosition()); // Driver Align Bindings, for a different/later day // CORAL.and(leftDriveController.getTrigger()).whileTrue(alignToReef(9, 0)); // Climb Bindings - leftDriveController.getLeftThumb().whileTrue(climberSubsystem.downPosition()); - leftDriveController.getRightThumb().whileTrue(climberSubsystem.upPosition()); + leftDriveController.getLeftThumb().whileTrue(climberSubsystem.moveClimberDownVoltage()); + leftDriveController.getRightThumb().whileTrue(climberSubsystem.moveClimberUpVoltage()); leftDriveController.getBottomThumb().whileTrue(climberSubsystem.intakeCage()); // leftDriveController.getBottomThumb().whileTrue(alignToPiece()); @@ -445,15 +463,24 @@ private void configureBindings() { // rightDriveController.getRightThumb().whileTrue(intakeSubsystem.openAndEject()); CORAL.and(rightDriveController.getLeftThumb()) - .whileTrue(gripperSubsystem.intakeSpinCoral()); - CORAL.and(rightDriveController.getRightThumb()) .whileTrue( gripperSubsystem - .ejectSpinCoral()); // and(normalRelease)).whileTrue(gripperSubsystem.ejectSpinCoral()); + .intakeSpinCoral() + .withDeadline( + Commands.waitSeconds(0.2) + .andThen( + Commands.waitUntil( + gripperSubsystem.HAS_PIECE)))); + CORAL.and(rightDriveController.getRightThumb()) + .whileTrue(gripperSubsystem.ejectSpinCoral()); - ALGAE.and(rightDriveController.getBottomThumb()) - .whileTrue(gripperSubsystem.intakeSpinAlgae()); - ALGAE.and(rightDriveController.getTrigger()).whileTrue(gripperSubsystem.ejectSpinAlgae()); + ALGAE.and(rightDriveController.getLeftThumb()).onTrue(gripperSubsystem.intakeSpinAlgae()); + ALGAE.and(rightDriveController.getRightThumb()) + .and(stateManager.PROCESSOR) + .whileTrue(gripperSubsystem.slowEjectSpinAlgae()); + ALGAE.and(rightDriveController.getRightThumb()) + .and(stateManager.PROCESSOR.negate()) + .whileTrue(gripperSubsystem.ejectSpinAlgae()); leftDriveController .getTrigger() @@ -481,19 +508,27 @@ private void configureBindings() { // Technical Bindings leftDriveController.getLeftBottomMiddle().onTrue(climberSubsystem.zeroClimberCommand()); + rightDriveController + .getLeftBottomMiddle() + .onTrue(stateManager.moveToPosition(Position.Start)); leftDriveController.getLeftTopMiddle().whileTrue(climberSubsystem.climberTuneable()); rightDriveController .getLeftTopLeft() .onTrue(Commands.runOnce(() -> drivetrain.seedFieldCentric())); - leftDriveController.getLeftBottomLeft().whileTrue(intakeSubsystem.rollerTuneable()); - leftDriveController.getLeftTopRight().whileTrue(intakeSubsystem.flipperTuneable()); + // leftDriveController.getLeftBottomLeft().whileTrue(wristSubsystem.tunablePose()); + // leftDriveController.getLeftTopRight().whileTrue(wristSubsystem.tuneableVoltage()); + + // leftDriveController.getLeftBottomLeft().whileTrue(intakeSubsystem.rollerTuneable()); + // leftDriveController.getLeftTopRight().whileTrue(intakeSubsystem.flipperTuneable()); + + leftDriveController.getLeftBottomLeft().whileTrue(chuteSubsystem.moveChuteUp()); + leftDriveController.getLeftTopRight().whileTrue(chuteSubsystem.moveChuteDown()); leftDriveController.getLeftBottomRight().onTrue(intakeSubsystem.zeroflipper()); leftDriveController.getLeftTopLeft().whileTrue(gripperSubsystem.gripperTuneable()); - { var tunableCommand = Commands.runOnce( @@ -522,7 +557,7 @@ private void configureBindings() { SmartDashboard.putData(stateManager); } - leftDriveController.getRightBottomLeft().onTrue(elevatorSubsystem.zeroElevatorCommand()); + // leftDriveController.getRightBottomLeft().onTrue(elevatorSubsystem.zeroElevatorCommand()); } private void bindPlaceSeq(Trigger button, Position prep, Position end, double timeout) { diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 6df7feee..8eff4978 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -4,10 +4,10 @@ public class ArmConstants { - public static final int ARM_PIVOT_MOTOR_ID = 10; // not correct motor ID - public static final String ARM_PIVOT_CANBUS = "CANivore"; + public static final int ARM_PIVOT_MOTOR_ID = 10; + public static final String ARM_PIVOT_CANBUS = "rio"; - public static final int ARM_THROUGHBORE_ENCODER_ID = 0; + public static final int ARM_THROUGHBORE_ENCODER_ID = 1; // public static final Slot0Configs slot0Configs = // new Slot0Configs() @@ -27,13 +27,12 @@ public class ArmConstants { // .withMotionMagicJerk(4); public static final CurrentLimitsConfigs currentLimitConfigs = new CurrentLimitsConfigs(); - - public static final double ARM_KP = 5; + public static final double ARM_KP = 3.5; public static final double ARM_KD = 0; - public static final double ARM_KI = 0; + public static final double ARM_KI = 0.25; public static final double ARM_TOLERANCE = 0.01; - public static final double upperLimit = 100; - public static final double lowerLimit = 0; + public static final double upperLimit = 2.15; + public static final double lowerLimit = -0.6; } diff --git a/src/main/java/frc/robot/constants/ChuteConstants.java b/src/main/java/frc/robot/constants/ChuteConstants.java index 99bd4896..5e05dd51 100644 --- a/src/main/java/frc/robot/constants/ChuteConstants.java +++ b/src/main/java/frc/robot/constants/ChuteConstants.java @@ -13,9 +13,9 @@ public class ChuteConstants { public static final double CHUTE_TOLERANCE = 0.01; - public static final double ChuteCurrent = 30; + public static final double ChuteCurrent = 40; - public static final double upperLimit = 100; + public static final double upperLimit = 10; public static final double lowerLimit = 0; public static final double ChuteCurrentTrigger = 15; diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java index 317c9699..8b5bea76 100644 --- a/src/main/java/frc/robot/constants/ClimberConstants.java +++ b/src/main/java/frc/robot/constants/ClimberConstants.java @@ -29,9 +29,11 @@ public class ClimberConstants { public static final int CLIMBER_ID = 16; - public static final int CLIMBER_HEAD_ID = 0; // or 40? the electrical manual wasn't clear + public static final int CLIMBER_HEAD_ID = 40; - public static final String CANbus = "CANivore"; + public static final String canbus = "rio"; + + public static final String WinchCanbus = "CANivore"; public static final double ClimberHeadCurrent = 30; } diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 99d112a9..f4ee38d3 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -12,16 +12,16 @@ public class ElevatorConstants { .withKD(0) .withKG(0) .withKI(0) - .withKP(0) + .withKP(2) .withKS(0) .withKV(0) .withGravityType(GravityTypeValue.Elevator_Static); ; public static final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs() - .withMotionMagicAcceleration(4) // these are guesses, come back here - .withMotionMagicCruiseVelocity(4) // also guess - .withMotionMagicJerk(4); + .withMotionMagicAcceleration(190 / .3) // these are guesses, come back here + .withMotionMagicCruiseVelocity(190) // also guess + .withMotionMagicJerk(0); public static final int elevatorLeaderId = 9; @@ -32,5 +32,5 @@ public class ElevatorConstants { public static final CurrentLimitsConfigs currentLimit = new CurrentLimitsConfigs(); public static final double lowerLimit = 0; - public static final double upperLimit = 100; + public static final double upperLimit = 297; } diff --git a/src/main/java/frc/robot/constants/GripperConstants.java b/src/main/java/frc/robot/constants/GripperConstants.java index 1fa1a119..e0cb037d 100644 --- a/src/main/java/frc/robot/constants/GripperConstants.java +++ b/src/main/java/frc/robot/constants/GripperConstants.java @@ -8,7 +8,7 @@ public class GripperConstants { public static final CurrentLimitsConfigs currentLimit = new CurrentLimitsConfigs().withStatorCurrentLimit(50); - public static final String canbus = "CANivore"; + public static final String canbus = "rio"; - public static final int gripperSensorChannel = 60; + public static final int gripperSensorChannel = 0; } diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 5d2d5172..ce0044db 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -28,8 +28,8 @@ public class VisionConstants { AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); // Camera names, must match names configured on coprocessor - public static String camera0Name = "limelight-april"; - public static String camera1Name = "limelight-april-right"; + public static String camera0Name = "limelight-left"; + public static String camera1Name = "limelight-right"; public static String camera2Name = "limelight-intake"; // public static String camera1Name = "camera_1"; diff --git a/src/main/java/frc/robot/constants/WristConstants.java b/src/main/java/frc/robot/constants/WristConstants.java index e1f369bc..e4d92523 100644 --- a/src/main/java/frc/robot/constants/WristConstants.java +++ b/src/main/java/frc/robot/constants/WristConstants.java @@ -5,9 +5,9 @@ public class WristConstants { public static final int WRIST_MOTOR_ID = 11; public static final String WRIST_CANBUS = "CANivore"; - public static final int WRIST_THROUGHBORE_ENCODER_ID = 1; + public static final int WRIST_THROUGHBORE_ENCODER_ID = 0; - public static final double WRIST_KP = 5; + public static final double WRIST_KP = 8; public static final double WRIST_KD = 0; public static final double WRIST_KI = 0; @@ -15,6 +15,6 @@ public class WristConstants { public static final double WristCurrent = 0; - public static final double upperLimit = 100; - public static final double lowerLimit = 0; + public static final double upperLimit = 1.59; + public static final double lowerLimit = -1.58; } diff --git a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java index 7bb37fbc..ac791144 100644 --- a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java +++ b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; @@ -18,6 +19,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Set; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; @@ -34,86 +36,153 @@ public class SuperstructureStateManager extends SubsystemBase { public static class SuperstructureState { private static LoggedNetworkNumber elevatorHeightLogged = - new LoggedNetworkNumber("Elevator Height", 0); + new LoggedNetworkNumber("Elevator Height", 170); private static LoggedNetworkNumber armHeightLogged = new LoggedNetworkNumber("Arm Height", 0); private static LoggedNetworkNumber wristRotationLogged = - new LoggedNetworkNumber("Wrist Rotation", 0); + new LoggedNetworkNumber("Wrist Rotation", 1.58); private static LoggedNetworkString parentLogged = - new LoggedNetworkString("Superstructure Parent", "Home"); + new LoggedNetworkString("Superstructure Parent", "CenterZoneNull"); private static LoggedNetworkString buttonTargetLogged = new LoggedNetworkString("Superstructure Button Target", "Tunable"); @FunctionalInterface public interface StateChecker { - boolean checksOut(Position position, SuperstructureStateManager stateManager); + boolean checksOut( + Position position, SuperstructureStateManager stateManager, boolean logExtra); + + default boolean checksOut(Position position, SuperstructureStateManager stateManager) { + return checksOut(position, stateManager, false); + } } - public static final StateChecker FALSE = (p, s) -> false; - public static final StateChecker TRUE = (p, s) -> true; + public static final StateChecker FALSE = (p, s, e) -> false; + public static final StateChecker TRUE = (p, s, e) -> true; public static final StateChecker DEFAULT = - (p, s) -> { + (p, s, e) -> { // return (s.internalPosition == p); double armPosition = s.armSubsystem.getPosition(); double wristPosition = s.wristSubsystem.getFlippedPosition(); double elevatorPosition = s.elevatorSubsystem.getPosition(); - if ((Math.abs(armPosition - p.armHeight()) < 0.1) - && (Math.abs(wristPosition - p.wristRotation()) < 0.1) - && (Math.abs(elevatorPosition - p.elevatorHeight()) < 0.1)) { + boolean armAtPosition = Math.abs(armPosition - p.armHeight()) < 0.1; + boolean wristAtPosition = Math.abs(wristPosition - p.wristRotation()) < 0.1; + boolean elevatorAtPosition = + Math.abs(elevatorPosition - p.elevatorHeight()) < 1.5; + if (e) { + Logger.recordOutput("/Superstructure/Arm/ArmAtPosition", armAtPosition); + Logger.recordOutput("/Superstructure/Arm/Setpoint", p.armHeight()); + Logger.recordOutput("/Superstructure/Arm/Position", armPosition); + Logger.recordOutput( + "/Superstructure/Arm/Error", armPosition - p.armHeight()); + + Logger.recordOutput( + "/Superstructure/Wrist/WristAtPosition", wristAtPosition); + Logger.recordOutput("/Superstructure/Wrist/Setpoint", p.wristRotation()); + Logger.recordOutput("/Superstructure/Wrist/Position", wristPosition); + Logger.recordOutput( + "/Superstructure/Wrist/Error", wristPosition - p.wristRotation()); + + Logger.recordOutput( + "/Superstructure/Elevator/ElevatorAtPosition", elevatorAtPosition); + Logger.recordOutput( + "/Superstructure/Elevator/Setpoint", p.elevatorHeight()); + Logger.recordOutput("/Superstructure/Elevator/Position", elevatorPosition); + Logger.recordOutput( + "/Superstructure/Elevator/Error", + elevatorPosition - p.elevatorHeight()); + + Logger.recordOutput("/Superstructure/Checker", "DEFAULT"); + + Logger.recordOutput("Superstructure/Heartbeat", Timer.getTimestamp()); + } + if (armAtPosition && wristAtPosition && elevatorAtPosition) { return true; } else return false; }; public static final StateChecker AUTO = DEFAULT; + // Any positions going lower than elevator 160 need to have Chute Up as a parent eventually. + // (The handoff is the exception). + // No arm positions should be negative unless they are for the handoff. + // Wrist positions should be 1.58 unless the arm angle is positive. Then they can be + // whatever. public static enum Position { Sussy(1, 1, 1, null), - None(1, 1, 1, null, TRUE, FALSE), - Home(1, 0, 1, None), - ChuteDown( - 0, + CenterZoneNull(1, 1, 1, null, TRUE, FALSE), + Home(170, 0, 1.58, CenterZoneNull), + ChuteDownPre( + 170, 0, + 1.58, + CenterZoneNull, + (a, s, e) -> s.chuteSubsystem.DOWN.getAsBoolean() || DEFAULT.checksOut(a, s, e), + (a, s, e) -> !s.chuteSubsystem.DOWN.getAsBoolean()), + ChuteDown( + 170, 0, - None, - (a, s) -> s.chuteSubsystem.DOWN.getAsBoolean(), - (a, s) -> s.chuteSubsystem.DOWN.getAsBoolean()), + 1.58, + ChuteDownPre, + (a, s, e) -> s.chuteSubsystem.DOWN.getAsBoolean(), + (a, s, e) -> !s.chuteSubsystem.DOWN.getAsBoolean()), + ChuteDownNull(0, 0, 0, ChuteDown, TRUE, FALSE), + HandoffPrep(170, -0.5, 1.58, ChuteDownNull), + Handoff(132, -0.5, 1.58, HandoffPrep), + PointUp(170, 2.15, 1.58, CenterZoneNull), + // (a, s, e) -> { + // boolean armChecksOut = s.armSubsystem.getPosition() > 1.0; + // boolean wristAtPosition = Math.abs(s.wristSubsystem.getFlippedPosition() - + // a.wristRotation()) < 0.1; + // return armChecksOut && wristAtPosition; + // }, TRUE), + UpZoneNull(0, 0, 0, PointUp, TRUE, FALSE), + ChuteUpPre( + 170, + 2.15, + -1.58, + UpZoneNull, + (a, s, e) -> s.chuteSubsystem.UP.getAsBoolean() || DEFAULT.checksOut(a, s, e), + (a, s, e) -> !s.chuteSubsystem.UP.getAsBoolean()), ChuteUp( - 0, - 0, - 0, - None, - (a, s) -> s.chuteSubsystem.UP.getAsBoolean(), - (a, s) -> s.chuteSubsystem.UP.getAsBoolean()), - ChuteDownNull(0, 0, 0, ChuteDown), - ChuteUpNull(0, 0, 0, ChuteUp), - HandoffPrep(1, 0, 1, ChuteDownNull), - Handoff(1, 0, 1, HandoffPrep), - Quick34(4, 2, 1, None), - Quick23(3, 3, 1, None), - PointUp(1, 2, 1, None), - UpZoneNull(1, 3, 1, PointUp, TRUE, FALSE), - L1Prep(2, 2, 1, ChuteUpNull), - L1(2, 1, 4, L1Prep), - L2Prep(3, 2, 1, UpZoneNull), - L2(3, 1, 1, L2Prep), - L3Prep(4, 2, 1, UpZoneNull), - L3(4, 1, 1, L3Prep), - L4Prep(5, 2, 1, UpZoneNull), - L4(5, 1, 1, L4Prep), - L2AlgaePrep(3, 2, 1, UpZoneNull), - L2Algae(3, 1, 1, L2AlgaePrep), - L3AlgaePrep(4, 2, 1, UpZoneNull), - L3Algae(4, 1, 1, L3AlgaePrep), - NetAlgaePrep(5, 2, 1, UpZoneNull), - NetAlgae(5, 1, 1, NetAlgaePrep), - Source(1, -2, 1, None), - AlgaeHome(1, 1, 1, None), - Climb(0, 0, 1.58, ChuteUpNull), - Processor(1, 1, 1, ChuteUpNull), - IcecreamCoral(1, 1, 1, ChuteUpNull), - IcecreamAlgae(1, 1, 1, ChuteUpNull), - Tunable(0, 0, 0, None, FALSE); + 170, + 2.15, + -1.58, + ChuteUpPre, + (a, s, e) -> s.chuteSubsystem.UP.getAsBoolean(), + (a, s, e) -> !s.chuteSubsystem.UP.getAsBoolean()), + ChuteUpNull(0, 0, 0, ChuteUp, TRUE, FALSE), + // L1Prep(297, 2.15, -1.58, ChuteUpNull), + L1(130, 0.9, 0, UpZoneNull), + L2Prep(120, 2.15, -1.58, UpZoneNull), + L2(90, 2.15, -1.58, L2Prep), + L3Prep(190, 2.15, -1.58, UpZoneNull), + L3(170, 2.15, -1.58, L3Prep), + L4Prep(297, 2.15, -1.58, UpZoneNull), + L4(297, 1.7, -1.58, L4Prep), + Quick34(250, 0.7, 0, UpZoneNull), + Quick23(150, 0.7, 0, UpZoneNull), + // L2AlgaePrep(297, 2.15, -1.58, UpZoneNull), + L2Algae(170, 1.2, -1.58, UpZoneNull), + // L3AlgaePrep(297, 2.15, -1.58, UpZoneNull), + L3Algae(250, 1.2, -1.58, UpZoneNull), + // NetAlgaePrep(297, 2.15, -1.58, UpZoneNull), + // NetAlgae(297, 2.15, -1.58, NetAlgaePrep), + Source(130, 2.15, 0, PointUp), + AlgaeHome(100, 2.15, -1.58, ChuteUpNull), + AlgaeHomeNull(0.0, 0.0, 0.0, AlgaeHome, TRUE, FALSE), + // Climb(170, 0, 1.58, ChuteUpNull), + Processor(65, 1.58, -1.58, AlgaeHomeNull), + + // IcecreamCoral(170, 0, 1.58, AlgaeHome), + // IcecreamAlgae(170, 0, 1.58, AlgaeHome), + GroundAlgaePrep(65, 1.58, 1.58, AlgaeHomeNull), + GroundAlgae(20, 1.58, 1.58, GroundAlgaePrep), + StartPrepPrepPrep(170, 2.15, 1.58, ChuteUpNull), + StartPrepPrep(140, 0, 1.58, StartPrepPrepPrep), + StartPrep(140, 0, -1.58, StartPrepPrep), + Start(0, 0, -1.58, StartPrep), + Tunable(170, 0, 1.58, CenterZoneNull, FALSE); private double elevatorHeight; private double armHeight; @@ -167,25 +236,23 @@ public boolean isRealPosition(SuperstructureStateManager stateManager) { // #region Pointer Methods public double elevatorHeight() { if (this == Position.Tunable) { - elevatorHeight = - SuperstructureState.elevatorHeightLogged - .get(); // ref: tunable variable armHeight + return SuperstructureState.elevatorHeightLogged + .get(); // ref: tunable variable armHeight } return elevatorHeight; } public double armHeight() { if (this == Position.Tunable) { - armHeight = - SuperstructureState.armHeightLogged - .get(); // ref: tunable variable armHeight + return SuperstructureState.armHeightLogged + .get(); // ref: tunable variable armHeight } return armHeight; } public double wristRotation() { if (this == Position.Tunable) { - wristRotation = SuperstructureState.wristRotationLogged.get(); + return SuperstructureState.wristRotationLogged.get(); // ref: tunable variable wristRotation } return wristRotation; @@ -193,7 +260,7 @@ public double wristRotation() { public Position parent() { if (this == Position.Tunable) { - parent = Position.valueOf(parentLogged.get()); // ref: tunable variable parent + return Position.valueOf(parentLogged.get()); // ref: tunable variable parent } return parent; } @@ -202,8 +269,15 @@ public Position parent() { } } - private SuperstructureState.Position targetPostition = SuperstructureState.Position.Home; - private SuperstructureState.Position lastPosition = SuperstructureState.Position.Home; + @AutoLogOutput + public boolean isAtTargetAllegedly() { + return SuperstructureState.DEFAULT.checksOut(targetPostition, this, true); + } + + private SuperstructureState.Position targetPostition = SuperstructureState.Position.Start; + private SuperstructureState.Position lastPosition = SuperstructureState.Position.Start; + + private SuperstructureState.Position trueFinalTarget = SuperstructureState.Position.Start; private List outList = new ArrayList<>(); @@ -220,7 +294,7 @@ private enum CoralAlgaeMode { private Pose2d lastScoringPose = Pose2d.kZero; - private boolean chuteCanMove = false; + @AutoLogOutput private boolean chuteCanMove = false; public void setLastScoringPose(Pose2d pose) { lastScoringPose = pose; @@ -236,6 +310,8 @@ public Pose2d getLastScoringPose() { public final Trigger ALGAE = new Trigger(() -> coralAlgaeMode == CoralAlgaeMode.Algae); public final Trigger ARMWRIST = new Trigger(() -> coralAlgaeMode == CoralAlgaeMode.ArmWrist); + public final Trigger PROCESSOR = new Trigger(() -> trueFinalTarget == Position.Processor); + public Command setLeftCoralMode() { return Commands.runOnce(() -> coralAlgaeMode = CoralAlgaeMode.LeftCoral); } @@ -279,6 +355,8 @@ public void periodic() { } private void setFinalTarget(SuperstructureState.Position myPosition) { + trueFinalTarget = myPosition; + outList.clear(); // Set the [ (A') => (C') ] target of the system (initialize pathing command) boolean found = false; @@ -288,7 +366,7 @@ private void setFinalTarget(SuperstructureState.Position myPosition) { for (int i = 0; i < 20; i++) { // SuperstructureState head = SuperstructureState.posDictionary.get(myPosition); outList.add(0, myPosition); - if (myPosition == SuperstructureState.Position.None) { + if (myPosition == SuperstructureState.Position.CenterZoneNull) { found = true; break; } @@ -340,7 +418,9 @@ private Command internalGoToPosition(SuperstructureState.Position myPosition) { if (myPosition == Position.ChuteUpNull) { chuteCanMove = false; } - if (myPosition == Position.None) { + if ((lastPosition.parent() == Position.CenterZoneNull && lastPosition.isRealPosition(this)) + || (lastPosition.parent() == Position.UpZoneNull + && lastPosition.isRealPosition(this))) { chuteCanMove = true; } @@ -448,7 +528,7 @@ private Command followInPath() { SuperstructureState.Position nextPose = getChilderNodeInBranch(lastPosition, targetPostition).parent(); if (nextPose == null) { - return internalGoToPosition(Position.None); + return internalGoToPosition(Position.CenterZoneNull); } return internalGoToPosition(nextPose) .beforeStarting(() -> updateTarget(nextPose)) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java index dc835bef..61387f99 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java @@ -5,7 +5,7 @@ public class ArmPivotIOSim implements ArmPivotIO { private double voltage = 0; public void updateInputs(ArmPivotIOInputs inputs) { - position += 0.02 * voltage; + position += 0.02 * voltage * 0.5; inputs.position = position; inputs.throughboreEncoderPosition = position; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmPivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/arm/ArmPivotIOTalonFX.java index 7515301f..58fd7c7c 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmPivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmPivotIOTalonFX.java @@ -1,8 +1,10 @@ package frc.robot.subsystems.arm; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.DutyCycleEncoder; import frc.robot.constants.ArmConstants; @@ -30,7 +32,11 @@ public ArmPivotIOTalonFX() { // SoftwareLimitSwitchConfigs softwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs(); talonConfig.apply( - new TalonFXConfiguration().withCurrentLimits(ArmConstants.currentLimitConfigs)); + new TalonFXConfiguration() + .withCurrentLimits(ArmConstants.currentLimitConfigs) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive))); armPivotMotor.setNeutralMode(NeutralModeValue.Brake); } @@ -42,7 +48,7 @@ public void updateInputs(ArmPivotIOInputs inputs) { inputs.velocity = armPivotMotor.getVelocity().getValueAsDouble(); inputs.temperature = armPivotMotor.getDeviceTemp().getValueAsDouble(); inputs.current = armPivotMotor.getStatorCurrent().getValueAsDouble(); - inputs.throughboreEncoderPosition = throughboreEncoder.get(); + inputs.throughboreEncoderPosition = (throughboreEncoder.get() * -1 + 2 * Math.PI) - 3.065; inputs.throughboreConnected = throughboreEncoder.isConnected(); if (inputs.throughboreEncoderPosition >= ArmConstants.upperLimit && lastVoltage > 0) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index b69b8a9a..cbf86655 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -2,7 +2,8 @@ import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -13,10 +14,14 @@ public class ArmSubsystem extends SubsystemBase { private ArmPivotIO armPivotIO; public ArmPivotIOInputsAutoLogged armPivotInputs = new ArmPivotIOInputsAutoLogged(); - public LoggedNetworkNumber armTuneables = new LoggedNetworkNumber("arm tuneable", 9); + public LoggedNetworkNumber armTuneables = new LoggedNetworkNumber("arm tuneable", 0); - private PIDController controller = - new PIDController(ArmConstants.ARM_KP, ArmConstants.ARM_KI, ArmConstants.ARM_KD); + private ProfiledPIDController controller = + new ProfiledPIDController( + ArmConstants.ARM_KP, + ArmConstants.ARM_KI, + ArmConstants.ARM_KD, + new TrapezoidProfile.Constraints(3, 4.5)); // try 6 and 6 private double reference = 0; @@ -64,7 +69,7 @@ public Command armpivotDown() { } public Command tuneableVoltage() { - return run(() -> setVoltage(armTuneables.get())); + return run(() -> armPivotIO.setVoltage(armTuneables.get())); } public Command tunablePose() { @@ -97,12 +102,23 @@ private Command followReference() { double voltage = controller.calculate( armPivotInputs.throughboreEncoderPosition, reference); - if (controller.atSetpoint()) { + TrapezoidProfile.State state = controller.getSetpoint(); + voltage += state.velocity * 1.0 / 0.5; + voltage += Math.sin(state.position) * 0.2; + if (voltage > 0) { + voltage += 0.2; + } else { + voltage -= 0.2; + } + if (controller.atGoal()) { voltage = 0; } else { voltage = Math.min(12.0, Math.max(-12.0, voltage)); // Clamp voltage } armPivotIO.setVoltage(voltage); + Logger.recordOutput("Arm/setpoint", state.position); + Logger.recordOutput("Arm/setpoint velocity", state.velocity); + Logger.recordOutput("Arm/reference", reference); }); } diff --git a/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java b/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java index 9ce9cb64..3096a951 100644 --- a/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java +++ b/src/main/java/frc/robot/subsystems/chute/ChuteIOSim.java @@ -7,7 +7,7 @@ public class ChuteIOSim implements ChuteIO { private double voltage = 0; public void updateInputs(ChuteIOInputs inputs) { - position += 0.02 * voltage * 20; + position += 0.02 * (voltage + 0.5) * 20; inputs.voltage = voltage; diff --git a/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java b/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java index 191f7267..5e5145e7 100644 --- a/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java +++ b/src/main/java/frc/robot/subsystems/chute/ChuteSubsystem.java @@ -2,9 +2,11 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.ChuteConstants; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; @@ -13,10 +15,10 @@ public class ChuteSubsystem extends SubsystemBase { private ChuteIOInputsAutoLogged chuteInputs = new ChuteIOInputsAutoLogged(); private LoggedNetworkNumber chuteTuneable = new LoggedNetworkNumber("chute tuneable", 0); - private boolean isUp = false; + @AutoLogOutput private boolean isUp = false; public final Trigger UP = new Trigger(() -> isUp); - private boolean isDown = false; + @AutoLogOutput private boolean isDown = false; public final Trigger DOWN = new Trigger(() -> isDown); private PIDController controller = @@ -31,7 +33,7 @@ public class ChuteSubsystem extends SubsystemBase { public ChuteSubsystem(ChuteIO chuteIO) { controller.setTolerance(ChuteConstants.CHUTE_TOLERANCE); this.chuteIO = chuteIO; - setDefaultCommand(setVoltage(0)); + setDefaultCommand(Commands.either(moveChuteDown(), moveChuteUp(), () -> lastSetDown)); } public void periodic() { @@ -68,21 +70,34 @@ public Command setVoltage(double voltage) { public final Trigger STALLING = new Trigger(() -> chuteInputs.current >= ChuteConstants.ChuteCurrentTrigger); + private boolean lastSetDown = false; + public Command moveChuteUp() { - setNull(); - return setVoltage(12) - .withTimeout(0.2) - .andThen(setVoltage(12).until(STALLING)) + return setVoltage(-4) + .withTimeout(0.05) + .andThen(setVoltage(-4).until(STALLING)) .andThen(setUp()) - .andThen(setVoltage(1)); + .andThen(setVoltage(-1)) + .beforeStarting( + () -> { + isDown = false; + lastSetDown = false; + }); } public Command moveChuteDown() { - setNull(); - return setVoltage(-12) - .withTimeout(0.2) - .andThen( - setVoltage(-12).until(STALLING).andThen(setDown()).andThen(setVoltage(-1))); + return (setVoltage(1.5) + .withTimeout(0.05) + .andThen( + setVoltage(1.5) + .until(STALLING) + .andThen(setDown()) + .andThen(setVoltage(0.0)))) + .beforeStarting( + () -> { + isUp = false; + lastSetDown = true; + }); } // public void setUp() { @@ -111,12 +126,6 @@ public Command setDown() { }); } - public void setNull() { - - isUp = false; - isDown = false; - } - // private Command followReferenceThrubore() { // return run( // () -> { diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java index 9eb8a3f8..85ed2962 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java @@ -11,7 +11,7 @@ public class ClimberIOTalonFX implements ClimberIO { // TBD: Hardcode IDs or add support to make changeable in method private final TalonFX climbermotor = - new TalonFX(ClimberConstants.CLIMBER_ID, ClimberConstants.CANbus); + new TalonFX(ClimberConstants.CLIMBER_ID, ClimberConstants.WinchCanbus); private final MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0); public ClimberIOTalonFX() { @@ -23,10 +23,10 @@ public ClimberIOTalonFX() { SoftwareLimitSwitchConfigs softwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) + .withForwardSoftLimitEnable(false) .withForwardSoftLimitThreshold(ClimberConstants.upperLimit) .withReverseSoftLimitThreshold(ClimberConstants.lowerLimit) - .withReverseSoftLimitEnable(true); + .withReverseSoftLimitEnable(false); talonConfig.apply( new TalonFXConfiguration() diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index bb01e161..95f0c3a8 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -106,6 +106,7 @@ public Command stopClimber() { return run( () -> { climberIO.setVoltage(0); + climberHeadIO.setVoltage(0); }); } @@ -120,14 +121,14 @@ public Command setHeadVoltage(double voltage) { public Command intakeCage() { return run( () -> { - climberHeadIO.setVoltage(12); + climberHeadIO.setVoltage(-12); }); } public Command ejectCage() { return run( () -> { - climberHeadIO.setVoltage(-12); // +- is a guess + climberHeadIO.setVoltage(12); // +- is a guess }); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java index 23e18e15..4b87790d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -1,9 +1,10 @@ package frc.robot.subsystems.elevator; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -// import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; +// import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import frc.robot.constants.ElevatorConstants; @@ -28,14 +29,17 @@ public ElevatorIOTalonFX() { .withForwardSoftLimitEnable(true) .withForwardSoftLimitThreshold(ElevatorConstants.upperLimit) .withReverseSoftLimitEnable(true) - .withForwardSoftLimitThreshold(ElevatorConstants.lowerLimit); + .withReverseSoftLimitThreshold(ElevatorConstants.lowerLimit); + + FeedbackConfigs feedbackConfigs = new FeedbackConfigs().withSensorToMechanismRatio(0.56250); TalonFXConfiguration config = new TalonFXConfiguration() .withSoftwareLimitSwitch(softwareLimitSwitchConfigs) .withSlot0(ElevatorConstants.slot0Configs) .withMotionMagic(ElevatorConstants.motionMagicConfigs) - .withCurrentLimits(ElevatorConstants.currentLimit); + .withCurrentLimits(ElevatorConstants.currentLimit) + .withFeedback(feedbackConfigs); elevatorLeader.getConfigurator().apply(config); // elevatorFollower.getConfigurator().apply(config); diff --git a/src/main/java/frc/robot/subsystems/gripper/GripperIOFalcon.java b/src/main/java/frc/robot/subsystems/gripper/GripperIOFalcon.java index 557eb432..5e2f3c1d 100644 --- a/src/main/java/frc/robot/subsystems/gripper/GripperIOFalcon.java +++ b/src/main/java/frc/robot/subsystems/gripper/GripperIOFalcon.java @@ -1,23 +1,29 @@ package frc.robot.subsystems.gripper; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.AnalogInput; import frc.robot.constants.GripperConstants; public class GripperIOFalcon implements GripperIO { private final TalonFX armRoller = new TalonFX(GripperConstants.id, GripperConstants.canbus); - private DigitalInput gripperGPSensor = new DigitalInput(GripperConstants.gripperSensorChannel); + private AnalogInput gripperGPSensor = new AnalogInput(GripperConstants.gripperSensorChannel); public GripperIOFalcon() { armRoller.setPosition(0); armRoller .getConfigurator() - .apply(new TalonFXConfiguration().withCurrentLimits(GripperConstants.currentLimit)); - + .apply( + new TalonFXConfiguration() + .withCurrentLimits(GripperConstants.currentLimit) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive))); armRoller.setNeutralMode(NeutralModeValue.Brake); } @@ -26,7 +32,7 @@ public void updateInputs(GripperIOInputs inputs) { inputs.voltage = armRoller.getMotorVoltage().getValueAsDouble(); inputs.current = armRoller.getStatorCurrent().getValueAsDouble(); inputs.temperature = armRoller.getDeviceTemp().getValueAsDouble(); - inputs.sensor = gripperGPSensor.get(); + inputs.sensor = gripperGPSensor.getValue() < 50; } public void setVoltage(double voltage) { diff --git a/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java b/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java index 8647ab05..b3c2208f 100644 --- a/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/gripper/GripperSubsystem.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; @@ -11,6 +12,8 @@ public class GripperSubsystem extends SubsystemBase { private GripperIOInputsAutoLogged armrollerInputs = new GripperIOInputsAutoLogged(); + public final Trigger HAS_PIECE = new Trigger(this::hasPiece); + // NetworkTableInstance nInstance = NetworkTableInstance.getDefault(); // NetworkTable table = nInstance.getTable("SmartDashboard"); // NetworkTableValue grippervoltage = table.getValue("grippervoltage"); @@ -40,7 +43,7 @@ public Command intakeSpinCoral() { } public Command ejectSpinCoral() { - return setVoltage(-12); + return setVoltage(-1); } public Command intakeSpinAlgae() { @@ -51,6 +54,10 @@ public Command ejectSpinAlgae() { return setVoltage(-12); } + public Command slowEjectSpinAlgae() { + return setVoltage(-3); + } + public Command setVoltage(double voltage) { return run( () -> { @@ -61,4 +68,10 @@ public Command setVoltage(double voltage) { public boolean hasPiece() { return armrollerInputs.sensor; } + + private boolean hasAlgae = false; + + public void setHasAlgae(boolean hasAlgae) { + hasAlgae = true; + } } diff --git a/src/main/java/frc/robot/subsystems/lights/LightsSubsystem.java b/src/main/java/frc/robot/subsystems/lights/LightsSubsystem.java index b500484d..ee56f4d1 100644 --- a/src/main/java/frc/robot/subsystems/lights/LightsSubsystem.java +++ b/src/main/java/frc/robot/subsystems/lights/LightsSubsystem.java @@ -19,7 +19,7 @@ public class LightsSubsystem extends SubsystemBase { public static final class LightsConstants { - public static final int CANDLE_PORT = 12; + public static final int CANDLE_PORT = 13; public static final int SENSOR_PORT = 0; } @@ -28,8 +28,10 @@ public static final class LightsConstants { private static final CANdle candle; + private static final boolean isReal = false; + static { - if (RobotBase.isReal()) { + if (RobotBase.isReal() && false) { candle = new CANdle(LightsConstants.CANDLE_PORT); } else { candle = null; diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIO.java b/src/main/java/frc/robot/subsystems/wrist/WristIO.java index 6ade56d8..0e0507cd 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIO.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIO.java @@ -17,4 +17,6 @@ public class WristIOInputs { } public void setVoltage(double voltage); + + public void setPositionControl(double reference); } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java b/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java index aade5804..7a6b740a 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIONeo550.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.wrist; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel; @@ -18,27 +19,44 @@ public class WristIONeo550 implements WristIO { new DutyCycleEncoder(WristConstants.WRIST_THROUGHBORE_ENCODER_ID, 2 * Math.PI, 0); public WristIONeo550() { - wristMotor.getEncoder().setPosition(0); - SparkBaseConfig config = new SparkMaxConfig() .smartCurrentLimit((int) WristConstants.WristCurrent) .secondaryCurrentLimit(WristConstants.WristCurrent) .idleMode(IdleMode.kBrake); + + config.encoder.positionConversionFactor(Math.PI / 76.1); + + config.closedLoop + .p(WristConstants.WRIST_KP / 12) + .i(WristConstants.WRIST_KI / 12) + .d(WristConstants.WRIST_KD / 12); + + config.softLimit + .forwardSoftLimit(WristConstants.upperLimit) + .forwardSoftLimitEnabled(true) + .reverseSoftLimit(WristConstants.lowerLimit) + .reverseSoftLimitEnabled(true); + wristMotor.configure( config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + + wristMotor.getEncoder().setPosition(-Math.PI / 2); } private boolean shutdown = false; private double lastVoltage = 0; + private boolean usingVoltage = true; + public void updateInputs(WristIOInputs inputs) { - inputs.position = wristMotor.getEncoder().getPosition(); inputs.voltage = wristMotor.getBusVoltage() * wristMotor.getAppliedOutput(); inputs.current = wristMotor.getOutputCurrent(); inputs.temperature = wristMotor.getMotorTemperature(); - inputs.throughboreEncoderPosition = throughboreEncoder.get(); + inputs.throughboreEncoderPosition = throughboreEncoder.get() - 4.22; + wristMotor.getEncoder().setPosition(inputs.throughboreEncoderPosition); + inputs.position = wristMotor.getEncoder().getPosition(); inputs.throughboreConnected = throughboreEncoder.isConnected(); if (inputs.temperature > 60) { @@ -49,23 +67,37 @@ public void updateInputs(WristIOInputs inputs) { inputs.shutdown = shutdown; - if (inputs.throughboreEncoderPosition >= WristConstants.upperLimit && lastVoltage > 0) { + if (inputs.throughboreEncoderPosition >= WristConstants.upperLimit + && (inputs.voltage > 0 || lastVoltage > 0)) { lastVoltage = 0; + usingVoltage = true; } - if (inputs.throughboreEncoderPosition <= WristConstants.lowerLimit && lastVoltage < 0) { + if (inputs.throughboreEncoderPosition <= WristConstants.lowerLimit + && (inputs.voltage < 0 || lastVoltage < 0)) { lastVoltage = 0; + usingVoltage = true; } if (!inputs.throughboreConnected) { lastVoltage = 0; + usingVoltage = true; } if (shutdown) { lastVoltage = 0; + usingVoltage = true; } - wristMotor.setVoltage(lastVoltage); + if (usingVoltage) { + wristMotor.setVoltage(lastVoltage); + } } public void setVoltage(double voltage) { lastVoltage = voltage; + usingVoltage = true; + } + + public void setPositionControl(double reference) { + wristMotor.getClosedLoopController().setReference(reference, ControlType.kPosition); + usingVoltage = false; } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java index d9df3989..ced810d7 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java @@ -1,10 +1,24 @@ package frc.robot.subsystems.wrist; +import edu.wpi.first.math.controller.PIDController; +import frc.robot.constants.WristConstants; + public class WristIOSim implements WristIO { private double position = 0; private double voltage = 0; + private double reference = 0; + private boolean usingPosition = false; + + private PIDController controller = + new PIDController( + WristConstants.WRIST_KP, WristConstants.WRIST_KI, WristConstants.WRIST_KD); + public void updateInputs(WristIOInputs inputs) { + if (usingPosition) { + voltage = controller.calculate(position, reference); + } + position += 0.02 * voltage; inputs.position = position; @@ -14,5 +28,11 @@ public void updateInputs(WristIOInputs inputs) { public void setVoltage(double voltage) { this.voltage = voltage; + usingPosition = false; + } + + public void setPositionControl(double reference) { + this.reference = reference; + usingPosition = true; } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 020c91ac..b1856a66 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -2,9 +2,10 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.WristConstants; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; @@ -30,18 +31,22 @@ public WristSubsystem(WristIO wristIO) { public void periodic() { wristIO.updateInputs(wristInputs); Logger.processInputs("RealOutputs/Wrist", wristInputs); + + if (!isSafeToMove()) { + wristIO.setVoltage(0); + } } public Command turnWristRight() { - return setVoltage(12); + return setVoltage(1); } public Command turnWristLeft() { - return setVoltage(-12); + return setVoltage(-1); } public Command tuneableVoltage() { - return run(() -> setVoltage(wristTuneable.get())); + return run(() -> setVoltageSave(wristTuneable.get())); } public Command tunablePose() { @@ -49,7 +54,7 @@ public Command tunablePose() { } public Command setVoltage(double voltage) { - return run(() -> wristIO.setVoltage(voltage)); + return run(() -> setVoltageSave(voltage)); } public Command setPosition(double position) { @@ -67,12 +72,12 @@ public Command setPosition(double position) { .andThen(followReferenceThrubore()); } - public Command flipWristPosition() { - return Commands.runOnce( - () -> { - isWristFlipped = !isWristFlipped; - }); - } + // public Command flipWristPosition() { + // return Commands.runOnce( + // () -> { + // isWristFlipped = !isWristFlipped; + // }); + // } private Command followReferenceThrubore() { return run( @@ -83,10 +88,11 @@ private Command followReferenceThrubore() { isWristFlipped ? -reference : reference); if (controller.atSetpoint()) { voltage = 0; + setVoltageSave(voltage); } else { - voltage = Math.min(12.0, Math.max(-12.0, voltage)); // Clamp voltage + voltage = Math.min(12.0, Math.max(-12.0, voltage)); + setPositionSave(isWristFlipped ? -reference : reference); // Clamp voltage } - wristIO.setVoltage(voltage); }); } @@ -107,4 +113,34 @@ public double getInternalEncoderPosition() { public boolean isEncoderConnected() { return wristInputs.throughboreConnected; } + + public DoubleSupplier elevatorHeight = () -> 0; + public DoubleSupplier armHeight = () -> -1; + + @AutoLogOutput + private boolean isSafeToMove() { + return elevatorHeight.getAsDouble() > 139 || armHeight.getAsDouble() > 0.5; + } + + private void setVoltageSave(double voltage) { + if (isSafeToMove()) { + wristIO.setVoltage(voltage); + } else { + wristIO.setVoltage(0); + } + // if ( + // wristIO.setVoltage(voltage); + // ) + } + + private void setPositionSave(double referenceInternal) { + if (isSafeToMove()) { + wristIO.setPositionControl(referenceInternal); + } else { + wristIO.setVoltage(0); + } + // if ( + // wristIO.setVoltage(voltage); + // ) + } } diff --git a/tuner-project.json b/tuner-project.json index 2e0a26a7..340b114a 100644 --- a/tuner-project.json +++ b/tuner-project.json @@ -1 +1 @@ -{"Version":"1.0.0.0","LastState":11,"Modules":[{"SteerMotor":null,"DriveMotor":null,"SelectedEncoderType":null,"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"AvailEncoders":[],"IsEncoderTypeCAN":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"AvailEncoderTypes":["CANcoder"],"ModuleName":"Front Left","ModuleId":0,"Encoder":null,"IsEncoderInverted":false,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"SteerMotor":null,"DriveMotor":null,"SelectedEncoderType":null,"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"AvailEncoders":[],"IsEncoderTypeCAN":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"AvailEncoderTypes":["CANcoder"],"ModuleName":"Front Right","ModuleId":1,"Encoder":null,"IsEncoderInverted":false,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"SteerMotor":null,"DriveMotor":null,"SelectedEncoderType":null,"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"AvailEncoders":[],"IsEncoderTypeCAN":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"AvailEncoderTypes":["CANcoder"],"ModuleName":"Back Left","ModuleId":2,"Encoder":null,"IsEncoderInverted":false,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"SteerMotor":null,"DriveMotor":null,"SelectedEncoderType":null,"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"AvailEncoders":[],"IsEncoderTypeCAN":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"AvailEncoderTypes":["CANcoder"],"ModuleName":"Back Right","ModuleId":3,"Encoder":null,"IsEncoderInverted":false,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false}],"SwerveOptions":{"kSpeedAt12Volts":4.731460295787658,"IsValidConfiguration":true,"Gyro":null,"IsValidGyroCANbus":false,"VerticalTrackSizeInches":23.5,"HorizontalTrackSizeInches":23.5,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":3,"SwerveModuleConfiguration":{"_CouplingRatio":3.5714285714285716,"_CustomName":"L2","SteerRatio":12.8,"ModuleBrand":3,"DriveRatio":6.746031746031747,"CouplingRatio":3.5714285714285716,"CustomName":"L2"},"HasVerifiedSteer":false,"HasVerifiedDrive":false}} \ No newline at end of file +{"Version":"1.0.0.0","LastState":11,"Modules":[{"SteerMotor":null,"DriveMotor":null,"SelectedEncoderType":null,"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"AvailEncoders":[],"IsEncoderTypeCAN":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"AvailEncoderTypes":["CANcoder"],"ModuleName":"Front Left","ModuleId":0,"Encoder":null,"IsEncoderInverted":false,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"SteerMotor":null,"DriveMotor":null,"SelectedEncoderType":null,"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"AvailEncoders":[],"IsEncoderTypeCAN":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"AvailEncoderTypes":["CANcoder"],"ModuleName":"Front Right","ModuleId":1,"Encoder":null,"IsEncoderInverted":false,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"SteerMotor":null,"DriveMotor":null,"SelectedEncoderType":null,"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"AvailEncoders":[],"IsEncoderTypeCAN":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"AvailEncoderTypes":["CANcoder"],"ModuleName":"Back Left","ModuleId":2,"Encoder":null,"IsEncoderInverted":false,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"SteerMotor":null,"DriveMotor":null,"SelectedEncoderType":null,"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"AvailEncoders":[],"IsEncoderTypeCAN":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"AvailEncoderTypes":["CANcoder"],"ModuleName":"Back Right","ModuleId":3,"Encoder":null,"IsEncoderInverted":false,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false}],"SwerveOptions":{"kSpeedAt12Volts":4.731460295787658,"IsValidConfiguration":true,"Gyro":null,"IsValidGyroCANbus":false,"VerticalTrackSizeInches":23.5,"HorizontalTrackSizeInches":23.5,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":3,"SwerveModuleConfiguration":{"_CouplingRatio":3.5714285714285716,"_CustomName":"L2","SteerRatio":12.8,"ModuleBrand":3,"DriveRatio":6.746031746031747,"CouplingRatio":3.5714285714285716,"CustomName":"L2"},"HasVerifiedSteer":false,"HasVerifiedDrive":false}} \ No newline at end of file diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 03df051a..c587313a 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "4.1.0", + "version": "4.1.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2025", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "4.1.0" + "version": "4.1.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "4.1.0", + "version": "4.1.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [