diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 78ad1da..df05ca0 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -55,23 +55,24 @@ public class AprilTag3 extends Command { double Botyaw = BotPose[5]; double tx = limelight3.getTx(); if(limelight3.getV() == true){ + System.out.println(Botyaw); if(Botyaw > 0 && Botyaw <=90){ drivetrain.setControl(drive. withRotationalRate(tx/20). - withVelocityX(MathUtil.clamp(x/4, -1, 1)). + withVelocityX(MathUtil.clamp(-x/4, -1, 1)). withVelocityY(MathUtil.clamp(z/4, -1, 1))); } else if(Botyaw >90 && Botyaw <=180){ drivetrain.setControl(drive. - withRotationalRate(tx/20). - withVelocityX(MathUtil.clamp(-x/4, -1, 1)). - withVelocityY(MathUtil.clamp(z/4, -1, 1))); + withRotationalRate(0). + withVelocityX(MathUtil.clamp(x/4, -1, 1)). + withVelocityY(MathUtil.clamp(-z/4, -1, 1))); } else if(Botyaw >-90 && Botyaw <=0){ drivetrain.setControl(drive. withRotationalRate(tx/20). withVelocityX(MathUtil.clamp(-x/4, -1, 1)). - withVelocityY(MathUtil.clamp(-z/4, -1, 1))); + withVelocityY(MathUtil.clamp(z/4, -1, 1))); } else if(Botyaw >-180 && Botyaw <=-90){ drivetrain.setControl(drive. diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java index 071886f..d86abc0 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3.java +++ b/src/main/java/frc/robot/subsystems/Limelight3.java @@ -21,14 +21,14 @@ public class Limelight3 extends SubsystemBase { } } public double[] getTargetPose(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace"); double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]); return targetPose; } public double[] getBotPose(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_cameraspace"); + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose"); double[] BotPose = BotPoseEntry.getDoubleArray(new double[6]); return BotPose; }