diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8bcbc56..8d90da9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -96,9 +96,9 @@ public class RobotContainer { private void configureBindings() { drivetrain.setDefaultCommand( drivetrain.applyRequest(() -> - drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed, 0.05)) - .withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*Math.abs(-manette.getLeftY())*MaxSpeed, 0.05)) - .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05)) + drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*MaxSpeed, 0.05)) + .withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*MaxSpeed, 0.05)) + .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*MaxAngularRate, 0.05)) ) ); //manette 1 diff --git a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java index a617c26..a5011c0 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java @@ -43,13 +43,14 @@ public class LancerAuto extends Command { public void execute() { double[] BotPose = new double[6]; if(limelight3g.getV()){ - + if(alliance.isPresent()){ if(alliance.get() == Alliance.Blue){ BotPose = limelight3g.getBotPoseBlue(); } else{ BotPose = limelight3g.getBotPoseRed(); } + } botx = BotPose[0]; boty = BotPose[1]; } diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 516fec2..0258f9c 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -30,7 +30,7 @@ public class LimelighterAuto extends Command { double boty; double angle; double calcul; - Optional alliance = DriverStation.getAlliance(); + Optional alliance = DriverStation.getAlliance(); private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); @@ -45,7 +45,9 @@ public class LimelighterAuto extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + + } // Called every time the scheduler runs while the command is scheduled. @Override @@ -53,13 +55,14 @@ public class LimelighterAuto extends Command { double[] BotPose = new double[6]; System.out.println("e"); if (limelight3g.getV()) { - if(alliance.get() == Alliance.Blue){ + if(alliance.isPresent()){ + if(alliance.get() == Alliance.Blue){ BotPose = limelight3g.getBotPoseBlue(); } else{ BotPose = limelight3g.getBotPoseRed(); } - + } botx = BotPose[1]; boty = BotPose[0]; angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();