diff --git a/lib/RoboLib b/lib/RoboLib index 8eb0a76..aad75b9 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit 8eb0a76b2646235f173cd524fbf7462256ab1696 +Subproject commit aad75b9b559825aaf60ad5bb550bf8263d3d203d diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a22dac4..0737efd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import com.ctre.phoenix6.signals.InvertedValue; @@ -107,10 +108,10 @@ public final class Constants { public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1); public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90); public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0); - public static final Angle DEFLECTOR_STORED_ANGLE = edu.wpi.first.units.Units.Degrees.of(0); - public static final Angle DEFLECTOR_SET_ANGLE1 = edu.wpi.first.units.Units.Degrees.of(100); - public static final Angle DEFLECTOR_SET_ANGLE2 = edu.wpi.first.units.Units.Degrees.of(200); - public static final Angle DEFLECTOR_SET_ANGLE3 = edu.wpi.first.units.Units.Degrees.of(300); + public static final double DEFLECTOR_STORED_ANGLE = 0; + public static final double DEFLECTOR_SET_ANGLE1 = 100; + public static final double DEFLECTOR_SET_ANGLE2 = 200; + public static final double DEFLECTOR_SET_ANGLE3 = 300; public static final double DEFLECTOR_SERVO_RATIO = 1.0; } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 6ea77df..3b4d2d7 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -61,6 +61,7 @@ public class AutoShootCommandChassisTurret extends Command{ turretSubsystem.setLauncher(2500); hopperSubsystem.spindexerOn(); + autoCenterController.calculate(-100); // Change to HUB tracking pipeline LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER); @@ -83,7 +84,7 @@ public class AutoShootCommandChassisTurret extends Command{ Pose3d closest = botPose.nearest(tagPoses); Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closest); Transform3d delta = botPose.minus(closest); - driveSubsystem.rotateChassis(delta.getRotation().getAngle() > 0 ? chassisRotateSpeed : -chassisRotateSpeed); // May need to flip this + driveSubsystem.rotateChassis(delta.getRotation().getAngle() > 0 ? -chassisRotateSpeed : chassisRotateSpeed); // May need to flip this } else // We have an april tag, center it to the camera frame { diff --git a/src/main/java/frc/robot/commands/SetDeflectorCommand.java b/src/main/java/frc/robot/commands/SetDeflectorCommand.java index a670ed0..6a46473 100644 --- a/src/main/java/frc/robot/commands/SetDeflectorCommand.java +++ b/src/main/java/frc/robot/commands/SetDeflectorCommand.java @@ -5,15 +5,14 @@ package frc.robot.commands; import frc.robot.subsystems.TurretSubsystem; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; /** An example command that uses an example subsystem. **/ public class SetDeflectorCommand extends Command { private final TurretSubsystem m_subsystem; - private Angle deployAngle; + private double deployAngle; - public SetDeflectorCommand(TurretSubsystem subsystem, Angle deployAngle) { + public SetDeflectorCommand(TurretSubsystem subsystem, double deployAngle) { m_subsystem = subsystem; this.deployAngle = deployAngle; } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 6e93d9f..6198ada 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -81,7 +81,7 @@ public class TurretSubsystem extends TestableSubsystem { SparkMaxConfig config = new SparkMaxConfig() .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); - config.closedLoop.p(1).i(0).d(0.1).outputRange(0, 5000); + config.closedLoop.p(1).i(0).d(0.1); config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); launcherControler = launcherMotor.getClosedLoopController(); @@ -105,10 +105,11 @@ public class TurretSubsystem extends TestableSubsystem { turretMotor.setControl(new PositionVoltage(position)); } - public void setTargetElevation(Angle elev) + public void setTargetElevation(double elev) { - double angle = elev.in(Units.Degrees); - hood.setAngle(300/angle); + double pos = Math.abs((elev - 300)/300); + Logger.recordOutput("ShooterHood-Angle", pos); + hood.set(pos); } public Command revSpeedCommand() @@ -147,6 +148,7 @@ public class TurretSubsystem extends TestableSubsystem { @Override public void periodic() { + // This method will be called once per scheduler run switch (currentState) { case HOMING: