sdesharnais/Rebuilt-2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

angle

Browse Source
This commit is contained in:
Antoine PerreaultE
2026-03-25 18:09:31 -04:00
parent 22746ab8ca
commit 92b876e7b0
2 changed files with 11 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View File

@@ -7,6 +7,7 @@ import java.util.function.Supplier;
import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveRequest;
@@ -42,6 +43,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private static final double kSimLoopPeriod = 0.004; // 4 ms
private Notifier m_simNotifier = null;
private double m_lastSimTime;
Pigeon2 pigeon2;
/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
@@ -246,6 +248,13 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
@Override
public void periodic() {
if(pigeon2.getYaw().getValueAsDouble() > 360){
pigeon2.setYaw(pigeon2.getYaw().getValueAsDouble()-360);
}
else if(pigeon2.getYaw().getValueAsDouble() < 0){
pigeon2.setYaw(pigeon2.getYaw().getValueAsDouble()+360);
}
/*
* Periodically try to apply the operator perspective.
* If we haven't applied the operator perspective before, then we should apply it regardless of DS state.