diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4c90db2..2baa8ef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.Aspirer; import frc.robot.commands.DescendreBalyeuse; import frc.robot.commands.DescendreGrimpeur; +import frc.robot.commands.DistanceLancer; import frc.robot.commands.Lancer; import frc.robot.commands.LancerAspirer; import frc.robot.commands.LancerBaseVitesse; @@ -87,13 +88,14 @@ public class RobotContainer { NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur)); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); - + CameraServer.startAutomaticCapture(); configureBindings(); } private void configureBindings() { + led.setDefaultCommand(new DistanceLancer(limeLight3G, led)); drivetrain.setDefaultCommand( drivetrain.applyRequest(() -> drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed, 0.05)) diff --git a/src/main/java/frc/robot/commands/DitanceLancer.java b/src/main/java/frc/robot/commands/DistanceLancer.java similarity index 65% rename from src/main/java/frc/robot/commands/DitanceLancer.java rename to src/main/java/frc/robot/commands/DistanceLancer.java index 582a32f..2b7522f 100644 --- a/src/main/java/frc/robot/commands/DitanceLancer.java +++ b/src/main/java/frc/robot/commands/DistanceLancer.java @@ -9,11 +9,11 @@ import frc.robot.subsystems.Led; import frc.robot.subsystems.Limelight3G; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class DitanceLancer extends Command { +public class DistanceLancer extends Command { private Led led; private Limelight3G limelight3g; /** Creates a new DitanceLancer. */ - public DitanceLancer(Limelight3G limelight3g, Led led) { + public DistanceLancer(Limelight3G limelight3g, Led led) { this.led = led; this.limelight3g = limelight3g; addRequirements(led,limelight3g); @@ -27,12 +27,31 @@ public class DitanceLancer extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - + double botx = 0; + double boty = 0; + double[] BotPose = new double[6]; + double distance = 0; + if(limelight3g.getV()){ + BotPose = limelight3g.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + } + if(limelight3g.getV()){ + distance = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)); + } + if(distance > 1.7){ + led.Jaune2(); + } + else{ + + } } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/subsystems/Led.java b/src/main/java/frc/robot/subsystems/Led.java index 2230f29..e390952 100644 --- a/src/main/java/frc/robot/subsystems/Led.java +++ b/src/main/java/frc/robot/subsystems/Led.java @@ -68,16 +68,16 @@ public class Led extends SubsystemBase { CANDle.setLEDs(255, 255, 0,0,120,8); CANDle.setLEDs(255, 255, 0,0,136,8); } - public void Vert2(){ - CANDle.setLEDs(0, 255, 0,0,8,8); - CANDle.setLEDs(0, 255, 0,0,24,8); - CANDle.setLEDs(0, 255, 0,0,40,8); - CANDle.setLEDs(0, 255, 0,0,56,8); - CANDle.setLEDs(0, 255, 0,0,72,8); - CANDle.setLEDs(0, 255, 0,0,88,8); - CANDle.setLEDs(0, 255, 0,0,104,8); - CANDle.setLEDs(0, 255, 0,0,120,8); - CANDle.setLEDs(0, 255, 0,0,136,8); + public void Rouge2(){ + CANDle.setLEDs(255, 0, 0,0,8,8); + CANDle.setLEDs(255, 0, 0,0,24,8); + CANDle.setLEDs(255, 0, 0,0,40,8); + CANDle.setLEDs(255, 0, 0,0,56,8); + CANDle.setLEDs(255, 0, 0,0,72,8); + CANDle.setLEDs(255, 0, 0,0,88,8); + CANDle.setLEDs(255, 0, 0,0,104,8); + CANDle.setLEDs(255, 0, 0,0,120,8); + CANDle.setLEDs(255, 0, 0,0,136,8); } public void RainBow(){ CANDle.animate(rainbowAnim);