Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 08:48:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

changing for the gyro

Browse Source
This commit is contained in:
SHikhar
2026-02-25 17:50:47 -07:00
parent 4ca55e0225
commit 0173c95996
1 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/alignment/AutoAlign.java
+7 -1
View File
@@ -40,13 +40,19 @@ public class AutoAlign extends Command {
@Override
public void execute() {
Rotation2d desiredHeading;
Pose2d robotPose = vision.getPose2d();
if (robotPose == null) return;
double dx = targetpos.getX() - robotPose.getX();
double dy = targetpos.getY() - robotPose.getY();
Rotation2d desiredHeading = new Rotation2d(Math.atan2(dy, dx));
if (!isLidar){
desiredHeading = new Rotation2d(Math.atan2(dy, dx)+ Math.PI);
}else{
desiredHeading = new Rotation2d(Math.atan2(dy, dx));
}
roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees();
if (roterr > 180) roterr -= 360;