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

led

Browse Source
This commit is contained in:
samuel desharnais
2026-04-01 18:20:55 -04:00
parent e5f7b05063
commit 8202b012cb
2 changed files with 36 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
View File

@@ -67,19 +67,21 @@ public class GrimperReservoir extends Command {
angle = angle + 360;
}
if(alliance.get() == Alliance.Red){
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
System.out.println(pigeonAngle);
y = 6.959326;
x = 13.57966;
angle = 180;
if(pigeonAngle< 190 && pigeonAngle> 170){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5),-2,2)));
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)));
}
else{
if(pigeonAngle>0 && pigeonAngle<180){
drivetrain.setControl(drive.withRotationalRate(1));
}
else if(pigeonAngle>180){
if(pigeonAngle>180){
drivetrain.setControl(drive.withRotationalRate(-1));
}
else{
drivetrain.setControl(drive.withRotationalRate(1));
}
}
}
else{