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

m

Browse Source
This commit is contained in:
samuel desharnais
2026-03-28 15:53:53 -04:00
parent fbd94fc33d
commit 799e3e34c3
4 changed files with 33 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java
View File

@@ -55,6 +55,7 @@ public class GrimperMur extends Command {
BotPose = limeLight3.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble());
if(angle < 0){
angle = angle + 360;
}
@@ -62,15 +63,15 @@ public class GrimperMur extends Command {
y = 2.961328;
x = 1.11;
angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 355 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 5){
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){
drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
}
else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(-0.5));
drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(0.5));
drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI));
}
}
}
@@ -78,15 +79,15 @@ public class GrimperMur extends Command {
x = 11.45966;
y = 6.959326;
angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 175 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 185){
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){
drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
}
else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(0.5));
drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(-0.5));
drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI));
}
}
}