diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index f53e67c..2778a65 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -27,6 +27,7 @@ public class Limelighter extends Command { double boty; double angle; double calcul; + double x; Optional alliance = DriverStation.getAlliance(); private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) @@ -53,26 +54,34 @@ public class Limelighter extends Command { System.out.println("e"); if (limelight3g.getV()) { BotPose = limelight3g.getBotPoseBlue(); - + if (!alliance.isPresent()) { + return; + } + if (alliance.get() == Alliance.Blue) { + x = 4.6; + } + else { + x = 11.915394; + } botx = BotPose[1]; boty = BotPose[0]; angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); - calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle); + calcul = limelight3g.Calcule(botx, x, boty, 4, angle); if(calcul < -5 && calcul > -180){ drivetrain.setControl( - drive.withRotationalRate(0.5*180/Math.PI)); + drive.withRotationalRate(0.5*(2*Math.PI))); } else if(calcul > 5 && calcul < 180){ drivetrain.setControl( - drive.withRotationalRate(-0.5*180/Math.PI)); + drive.withRotationalRate(-0.5*(2*Math.PI))); } else if(calcul >= 180){ drivetrain.setControl( - drive.withRotationalRate(-0.5*180/Math.PI)); + drive.withRotationalRate(-0.5*(2*Math.PI))); } else if(calcul <= -180){ drivetrain.setControl( - drive.withRotationalRate(0.5*180/Math.PI)); + drive.withRotationalRate(0.5*(2*Math.PI))); } else{ drivetrain.setControl( diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 71fbab5..0a60359 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -27,6 +27,7 @@ public class LimelighterAuto extends Command { double boty; double angle; double calcul; + double x; Optional alliance = DriverStation.getAlliance(); private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) @@ -52,27 +53,35 @@ public class LimelighterAuto extends Command { double[] BotPose = new double[6]; System.out.println("e"); if (limelight3g.getV()) { + if (!alliance.isPresent()) { + return; + } + if (alliance.get() == Alliance.Blue) { + x = 4.6; + } + else { + x = 11.915394; + } BotPose = limelight3g.getBotPoseBlue(); - botx = BotPose[1]; boty = BotPose[0]; angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); - calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle); + calcul = limelight3g.Calcule(botx, x, boty, 4, angle); if(calcul < -5 && calcul > -180){ drivetrain.setControl( - drive.withRotationalRate(0.5*180/Math.PI)); + drive.withRotationalRate(0.5*(2*Math.PI))); } else if(calcul > 5 && calcul < 180){ drivetrain.setControl( - drive.withRotationalRate(-0.5*180/Math.PI)); + drive.withRotationalRate(-0.5*(2*Math.PI))); } else if(calcul >= 180){ drivetrain.setControl( - drive.withRotationalRate(-0.5*180/Math.PI)); + drive.withRotationalRate(-0.5*(2*Math.PI))); } else if(calcul <= -180){ drivetrain.setControl( - drive.withRotationalRate(0.5*180/Math.PI)); + drive.withRotationalRate(0.5*(2*Math.PI))); } else{ drivetrain.setControl( diff --git a/src/main/java/frc/robot/subsystems/Limelight3G.java b/src/main/java/frc/robot/subsystems/Limelight3G.java index dbee931..da4ce72 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3G.java +++ b/src/main/java/frc/robot/subsystems/Limelight3G.java @@ -62,10 +62,10 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag") } else{ if(y1 > y2){ - return Math.atan(90-((x2 - x1) / (y2 - y1)))* (180 / Math.PI)+270 - angle; + return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle; } else{ - return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+180 - angle; + return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle; } } }