diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index 7201f79..768028c 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -64,7 +64,7 @@ public class GrimperMur extends Command { x = 1.11; angle = 0; if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ - drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); } else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ @@ -80,7 +80,7 @@ public class GrimperMur extends Command { y = 6.959326; angle = 180; if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){ - drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); } else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index ab44170..381b213 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -64,7 +64,7 @@ public class GrimperReservoir extends Command { x = 13.57966; angle = 180; if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){ - drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); } else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ @@ -80,7 +80,7 @@ public class GrimperReservoir extends Command { x = 1.11; angle = 0; if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){ - drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx)); + drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx)); } else{ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 7de1e8a..02351f6 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -69,11 +69,11 @@ public class LimelighterAuto extends Command { if(angle >180){ angle -= 360; } - calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 3; + calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5; drivetrain.setControl( drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); System.out.println(calcul); - if (calcul < 0.2 && calcul > -0.2) { + if (calcul < 0.1 && calcul > -0.1) { drivetrain.setControl(drive.withRotationalRate(0)); } } @@ -92,7 +92,7 @@ public class LimelighterAuto extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return calcul < 0.2 && calcul > -0.2; + return calcul < 0.1 && calcul > -0.1; } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java b/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java index 891e776..24249d7 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java @@ -51,10 +51,10 @@ public class TournerVersMur extends Command { angle = 180; } if(drivetrain.getPigeon2().getYaw().getValueAsDouble()>0 && drivetrain.getPigeon2().getYaw().getValueAsDouble()<180){ - drivetrain.setControl(drive.withRotationalRate(force)); + drivetrain.setControl(drive.withRotationalRate(force*180/Math.PI)); } else if(drivetrain.getPigeon2().getYaw().getValueAsDouble()>180){ - drivetrain.setControl(drive.withRotationalRate(-force)); + drivetrain.setControl(drive.withRotationalRate(-force*180/Math.PI)); } } @@ -65,6 +65,6 @@ public class TournerVersMur extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return drivetrain.getPigeon2().getYaw().getValueAsDouble()> angle && drivetrain.getPigeon2().getYaw().getValueAsDouble()< angle + 5; + return drivetrain.getPigeon2().getYaw().getValueAsDouble()> angle && drivetrain.getPigeon2().getYaw().getValueAsDouble()< angle + 10; } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java index 86932ce..0e537eb 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java @@ -42,19 +42,21 @@ public class TournerVersReservoir extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(alliance.get() == Alliance.Blue){ + if(alliance.isPresent()){ + if(alliance.get() == Alliance.Blue){ force = 0.5; angle = 180; } else{ force = -0.5; angle = 0; + } } if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ - drivetrain.setControl(drive.withRotationalRate(-force)); + drivetrain.setControl(drive.withRotationalRate(-force*180/Math.PI)); } else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ - drivetrain.setControl(drive.withRotationalRate(force)); + drivetrain.setControl(drive.withRotationalRate(force*180/Math.PI)); } } @@ -65,6 +67,6 @@ public class TournerVersReservoir extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 5; + return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10; } }