FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Changed some logging and added back in chassis deadreckoning

This commit is contained in:
Tyler Wilcox 2026-03-29 21:01:34 -04:00
parent 47073cfd35
commit 8f5988ea12
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
View File

@ -102,14 +102,14 @@ public class AutoShootCommandChassisTurret extends Command{
// No april tag in camera view. Rotate towards our theoretical closest tag // No april tag in camera view. Rotate towards our theoretical closest tag
if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){ if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){
Logger.recordOutput("AutoAlignState", "Look for tag"); Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning");
Pose3d botPose = driveSubsystem.getBotPose(); Pose3d botPose = driveSubsystem.getBotPose();
//driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this
} }
else // We have an april tag, center it to the camera frame else // We have an april tag, center it to the camera frame
{ {
Logger.recordOutput("AutoAlignState", "Center on tag"); Logger.recordOutput("AutoAlignState-Tracking", "Camera");
// [tx, ty, tz, pitch, yaw, roll] // [tx, ty, tz, pitch, yaw, roll]
double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
if(targetsPose.length >= 1){ if(targetsPose.length >= 1){
@ -119,8 +119,10 @@ public class AutoShootCommandChassisTurret extends Command{
// If robot is centered on HUB and shooter is up to speed. Run the kicker // If robot is centered on HUB and shooter is up to speed. Run the kicker
if(turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()){ if(turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()){
Logger.recordOutput("AutoAlignState", "FIRE!"); Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
hopperSubsystem.feedKicker(); hopperSubsystem.feedKicker();
}else{
Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
} }
} }

10
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -99,12 +99,18 @@ public class TurretSubsystem extends TestableSubsystem {
} }
public void setLauncher(double speed) { public void setLauncher(double speed) {
if(speed == 0)
{
launcherMotor.stopMotor();
return;
}
launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0); launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0);
// launcherMotor.set(speed/1000);
} }
public boolean isLauncherUpToSpeed() { public boolean isLauncherUpToSpeed() {
return true; // Valid if velcity is within 10% of setpoint.
// May need to lower this
return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.1;
} }
public void turretControl(double position) public void turretControl(double position)