diff --git a/lib/RoboLib b/lib/RoboLib index 0584a07..840c851 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit 0584a07e801a1823e4113c16e42b4d97f147cc51 +Subproject commit 840c851253187081a50ca9492c032cec8d376e7b diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d0cccb7..e0b4832 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -129,7 +129,7 @@ public final class Constants { 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_SET_ANGLE3 = 200; // Return to 300 once hood is fixed public static final double DEFLECTOR_SERVO_RATIO = 1.0; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d8bb69..88b10f0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -76,8 +76,8 @@ public class RobotContainer { m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); - // m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); - m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + //m_driverController.a().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); + // m_driverController:wa.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem)); m_driverController.rightBumper().onTrue(intakeSubsystem.intake()); diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index 0ceeee2..9b41e29 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -54,12 +54,12 @@ public class AutoShootCommandChassisTurret extends Command{ this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem); - cameraAutoCenterController = new PIDController(0.1, 0, 0.0); - cameraAutoCenterController.setTolerance(0.2); // Units is meters + cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0); + cameraAutoCenterController.setTolerance(0.2); // Units degrees cameraAutoCenterController.setSetpoint(0); - deadreckoningAutoCenterController = new PIDController(Math.PI, 0, 0.0); + deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0); deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI); } @@ -80,7 +80,7 @@ public class AutoShootCommandChassisTurret extends Command{ cameraAutoCenterController.calculate(-100); // Change to HUB tracking pipeline - LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_2D_PIPELINE_NUMBER); + LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER); LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters); Pose3d botPose = driveSubsystem.getBotPose(); @@ -106,7 +106,7 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning"); Pose3d botPose = driveSubsystem.getBotPose(); - driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); + //driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); } else // We have an april tag, center it to the camera frame { @@ -114,6 +114,11 @@ public class AutoShootCommandChassisTurret extends Command{ // [tx, ty, tz, pitch, yaw, roll] double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); if(targetsPose.length >= 1){ + Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]); + + // Calculate turret Angle + double targetDistance = targetsPose[2]; + driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated } } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java index 93238d5..683a056 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java @@ -115,7 +115,7 @@ public class AutoShootCommandChassisTurretPathPlanner extends Command { @Override public void end(boolean interrupted) { - pathFollowCommand.cancel(); + CommandScheduler.getInstance().cancel(pathFollowCommand); turretSubsystem.setLauncher(0); hopperSubsystem.stopKicker(); diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index 16455a0..6ce9eee 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -62,7 +62,7 @@ public class AutoTurretCommand extends Command { Pose3d botPose = driveSubsystem.getBotPose(); Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT), - new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngle().in(Units.Rotations))))); + new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative().in(Units.Rotations))))); Logger.recordOutput("AutoTurret-TurretPose", turretPose); Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); diff --git a/src/main/java/frc/robot/commands/RelayCommand.java b/src/main/java/frc/robot/commands/RelayCommand.java index 754f9d7..80f80ce 100644 --- a/src/main/java/frc/robot/commands/RelayCommand.java +++ b/src/main/java/frc/robot/commands/RelayCommand.java @@ -1,5 +1,7 @@ package frc.robot.commands; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -30,6 +32,7 @@ public class RelayCommand extends Command{ turretSubsystem.setLauncher(5500); hopperSubsystem.spindexerOn(); CommandScheduler.getInstance().schedule(waitCommand); + turretSubsystem.setTurretMotorRotation(Angle.ofBaseUnits(-6, Units.Degrees)); // Return to 0 after MIAMI VALLEY } @Override @@ -54,6 +57,7 @@ public class RelayCommand extends Command{ turretSubsystem.setLauncher(0); turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); + turretSubsystem.setTurretMotorRotation(Angle.ofBaseUnits(1, Units.Degrees)); // Return to 0 after MIAMI VALLEY } } diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 0a2425d..6f0f1d5 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -7,6 +7,7 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.hardware.TalonFX; @@ -21,6 +22,8 @@ public class HopperSubsystem extends TestableSubsystem { public HopperSubsystem() { spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID); spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)); + // spindexerMotor.getConfigurator().apply(new ClosedLoopGeneralConfigs() + // .); kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 99a289f..fe227ea 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -75,7 +75,7 @@ public class TurretSubsystem extends TestableSubsystem { .withSlot0( new Slot0Configs() .withKP(3) - .withKI(1) + .withKI(1.5) .withKD(0) .withKS(0.3) ) @@ -172,7 +172,7 @@ public class TurretSubsystem extends TestableSubsystem { turretMotor.set(speed); } - public Angle getTurretAngle() + public Angle getTurretAngleRobotRelative() { return getTurretMotorPosition().div(Constants.ShooterConstants.TURRET_GEAR_RATIO); } @@ -200,12 +200,12 @@ public class TurretSubsystem extends TestableSubsystem { turretMotor.getConfigurator().apply( new SoftwareLimitSwitchConfigs() // .withForwardSoftLimitEnable(true) - .withForwardSoftLimitThreshold(forwardLimit) + // .withForwardSoftLimitThreshold(forwardLimit) // .withReverseSoftLimitEnable(true) - .withReverseSoftLimitThreshold(reverseLimit) + // .withReverseSoftLimitThreshold(reverseLimit) ); // setTurretRobotRelative(Angle.ofRelativeUnits(0, Units.Degrees)); - setTurretMotorRotation(Angle.ofRelativeUnits(0, Units.Degrees)); + setTurretMotorRotation(Angle.ofBaseUnits(1, Units.Degrees)); // Return to 0 after MIAMI VALLEY } } @@ -214,10 +214,11 @@ public class TurretSubsystem extends TestableSubsystem { Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get()); Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); Logger.recordOutput("TurretSubsystem-TurretState", turretState); - Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngle().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretPosition", getTurretAngleRobotRelative().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw", turretMotorPosition.refresh().getValue().in(Units.Degrees)); - Logger.recordOutput("TurretSubsystem-TurretRequestedPos", turretControlMode.getPositionMeasure().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw", turretControlMode.getPositionMeasure().in(Units.Degrees)); + Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative().in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffset.in(Units.Degrees)); Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue()); @@ -245,11 +246,15 @@ public class TurretSubsystem extends TestableSubsystem { setTurretMotorRotation(pos.times(Constants.ShooterConstants.TURRET_GEAR_RATIO)); } - private void setTurretMotorRotation(Angle pos){ + public void setTurretMotorRotation(Angle pos){ turretMotor.setControl(turretControlMode.withPosition(pos.plus(turretOffset).times(-1))); turretSim.setRawRotorPosition(pos.plus(turretOffset)); } + private Angle getRequestedPosTurretRelative() { + return turretControlMode.getPositionMeasure().minus(turretOffset).div(Constants.ShooterConstants.TURRET_GEAR_RATIO); + } + @PostTest() public void TurretSubsystem_CANDevicesConnected(){ assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse();