diff --git a/src/main/deploy/pathplanner/autos/BlueHaut1.auto b/src/main/deploy/pathplanner/autos/BlueHaut1.auto index 05c0205..0fb9737 100644 --- a/src/main/deploy/pathplanner/autos/BlueHaut1.auto +++ b/src/main/deploy/pathplanner/autos/BlueHaut1.auto @@ -19,34 +19,9 @@ "data": { "name": "AprilTag" } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } } ] } - }, - { - "type": "named", - "data": { - "name": "CoralExpire" - } } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d9a83c0..5f2d11d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; @@ -84,7 +85,6 @@ public class RobotContainer { public double getAngle() { return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot } - Elevateur elevateur = new Elevateur(); Pince pince = new Pince(); ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY); @@ -98,6 +98,7 @@ public class RobotContainer { autoChooser = AutoBuilder.buildAutoChooser(); CameraServer.startAutomaticCapture(); SmartDashboard.putData("Auto Chooser", autoChooser); + //NamedCommands.registerCommand("AprilTag", new AprilTag3(limelight3, drivetrain,, null)); configureBindings(); } @@ -116,7 +117,7 @@ public class RobotContainer { manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie)); manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie)); manette1.povLeft().whileTrue(new AlgueExpire(pince, bougie)); - // manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain1,manette1::getLeftX,manette1::getLeftY)); + manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); manette1.povRight().whileTrue(new CoralExpire(pince, bougie)); manette1.leftTrigger().whileTrue(new DepartPince(pince)); manette1.povDown().whileTrue(new Algue_inspire(pince,bougie)); @@ -140,8 +141,8 @@ public class RobotContainer { manette2.x().whileTrue(new ExpireCorail(requin, bougie)); //limelight - // manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); - // manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY)); + manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); + manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY)); //Pince manuel pince.setDefaultCommand(new RunCommand(()->{ diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 22d141e..47b6aa7 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -23,7 +23,7 @@ import frc.robot.subsystems.Limelight3; /* 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 AprilTag3 extends Command { private Limelight3 limelight3; - private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private CommandSwerveDrivetrain drivetrain; private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity private DoubleSupplier x; @@ -61,31 +61,42 @@ public class AprilTag3 extends Command { BotPose = limelight3.getBotPoseBlue(); } double botx = BotPose[0]; + System.out.println(botx); double boty = BotPose[1]; double tx = limelight3.getTx(); double tagId = limelight3.getTId(); - if(limelight3.getV() == true){ - if(tagId ==10){ drivetrain.setControl(drive. withRotationalRate(tx/20). withVelocityX((botx-5.81)*2). withVelocityY((boty-4)*4)); - } - else if(tagId ==7){ - drivetrain.setControl(drive. - withRotationalRate(tx/20). - withVelocityX(2-botx). - withVelocityY(2-boty)); + // if(limelight3.getV() == true){ + // if(tagId ==10){ + // drivetrain.setControl(drive. + // withRotationalRate(tx/20). + // withVelocityX((botx-5.81)*2). + // withVelocityY((boty-4)*4)); + // } + // else if(tagId ==7){ + // drivetrain.setControl(drive. + // withRotationalRate(tx/20). + // withVelocityX(2-botx). + // withVelocityY(2-boty)); - } - } - else{ - drivetrain.setControl(drive. - withRotationalRate(0). - withVelocityX(0). - withVelocityY(0)); - } - } + // } + // } + // if(limelight3.getV()){ + // drivetrain.setControl(drive. + // withRotationalRate(limelight3.getTx()/10). + // withVelocityX(x.getAsDouble()). + // withVelocityY(y.getAsDouble())); + // } + // else{ + // drivetrain.setControl(drive. + // withRotationalRate(0). + // withVelocityX(0). + // withVelocityY(0)); + // } + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java index af10b89..fffeec8 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3G.java @@ -18,7 +18,7 @@ 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 AprilTag3G extends Command { private Limelight3G limelight3g; - private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private CommandSwerveDrivetrain drivetrain; private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity private DoubleSupplier x; diff --git a/src/main/java/frc/robot/commands/Limelight/Forme3.java b/src/main/java/frc/robot/commands/Limelight/Forme3.java index 8afaec4..3535ae1 100644 --- a/src/main/java/frc/robot/commands/Limelight/Forme3.java +++ b/src/main/java/frc/robot/commands/Limelight/Forme3.java @@ -20,7 +20,7 @@ public class Forme3 extends Command { private Limelight3 limelight3; private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private CommandSwerveDrivetrain drivetrain; private DoubleSupplier x; private DoubleSupplier y; /* Setting up bindings for necessary control of the swerve drive platform */ diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java index 17517b6..96e11d1 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3.java +++ b/src/main/java/frc/robot/subsystems/Limelight3.java @@ -30,13 +30,13 @@ public class Limelight3 extends SubsystemBase { } public double[] getBotPoseBlue(){ NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue"); double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); return BotPose; } public double[] getBotPoseRed(){ NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired"); double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); return BotPose; }