PLS5618/Reefscape-2025
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

apriltag

Browse Source
This commit is contained in:
Antoine PerreaultE
2026-01-19 18:28:18 -05:00
parent 7032b95c60
commit 5a8a499e05
6 changed files with 38 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
src/main/deploy/pathplanner/autos/BlueHaut1.auto
View File

@@ -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"
}
}
]
}

9
src/main/java/frc/robot/RobotContainer.java
View File

@@ -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(()->{

47
src/main/java/frc/robot/commands/Limelight/AprilTag3.java
View File

@@ -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

2
src/main/java/frc/robot/commands/Limelight/AprilTag3G.java
View File

@@ -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;

2
src/main/java/frc/robot/commands/Limelight/Forme3.java
View File

@@ -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 */

4
src/main/java/frc/robot/subsystems/Limelight3.java
View File

@@ -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;
}