diff --git a/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java b/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java index e4a3748..c44fc06 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java @@ -56,7 +56,7 @@ public class BougerGaucheAuto extends Command { } else{ if(timer.get() < 0.75){ - drivetrain.setControl(drive.withVelocityY(-1.5)); + drivetrain.setControl(drive.withVelocityY(1.5)); } else{ drivetrain.setControl(drive.withVelocityY(0)); diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index 810c461..547c5f2 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -67,7 +67,7 @@ public class GrimperMur extends Command { angle = angle + 360; } if(alliance.get() == Alliance.Blue){ - y = 5; + y = 5.4; x = 1.11; angle = 0; if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ @@ -83,7 +83,7 @@ public class GrimperMur extends Command { } } else{ - x = 11.45966; + x = 15.6; y = 6.959326; angle = 180; if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){ diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index a7951e4..236f588 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -70,10 +70,10 @@ public class GrimperReservoir extends Command { pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); System.out.println(pigeonAngle); y = 5.4; - x = 15.4; + x = 15.6; angle = 180; if(pigeonAngle< 190 && pigeonAngle> 170){ - drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.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>180){