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
if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){
Logger.recordOutput("AutoAlignState", "Look for tag");
Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning");
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
{
Logger.recordOutput("AutoAlignState", "Center on tag");
Logger.recordOutput("AutoAlignState-Tracking", "Camera");
// [tx, ty, tz, pitch, yaw, roll]
double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
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(turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()){
Logger.recordOutput("AutoAlignState", "FIRE!");
Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
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) {
if(speed == 0)
{
launcherMotor.stopMotor();
return;
}
launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0);
// launcherMotor.set(speed/1000);
}
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)