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

Fixed turret auto aim

This commit is contained in:
Team578 Driver Station 2026-04-13 20:46:10 -04:00
parent 2a09405bba
commit 2ada8c9408
2 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/java/frc/robot/commands/AutoTurretCommand.java
View File

@ -62,7 +62,7 @@ public class AutoTurretCommand extends Command {
Pose3d botPose = driveSubsystem.getBotPose(); Pose3d botPose = driveSubsystem.getBotPose();
Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT), Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative())))); new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5))));
Logger.recordOutput("AutoTurret-TurretPose", turretPose); Logger.recordOutput("AutoTurret-TurretPose", turretPose);
Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());

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

@ -7,6 +7,7 @@ package frc.robot.subsystems;
import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.TestableSubsystem;
import org.fairportrobotics.frc.posty.test.PostTest; import org.fairportrobotics.frc.posty.test.PostTest;
import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.ConsoleSource.RoboRIO;
import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
@ -247,9 +248,11 @@ public class TurretSubsystem extends TestableSubsystem {
double robotRotations = robotPose.getRotation().toRotation2d().getRotations(); double robotRotations = robotPose.getRotation().toRotation2d().getRotations();
Angle turretAngle = Angle.ofRelativeUnits(robotRotations, Units.Rotations).plus(Angle.ofRelativeUnits(correctedPos, Units.Rotations)); Rotation2d turretAngle = Rotation2d.fromRotations(correctedPos).plus(Rotation2d.fromRotations(robotRotations)).minus(Rotation2d.fromRotations(0.5));
setTurretMotorRotation((turretAngle.in(Units.Rotation) * Constants.ShooterConstants.TURRET_GEAR_RATIO)); Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotations());
setTurretRobotRelative(((turretAngle.getRotations())));
} }
public void setTurretRobotRelative(double pos) public void setTurretRobotRelative(double pos)