sdesharnais/Rebuilt-2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

limelight

Browse Source
This commit is contained in:
Olivier Dubois
2026-03-30 05:04:22 +02:00
parent 5c626f33e3
commit f7e88619e9
3 changed files with 64 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
src/main/java/frc/robot/commands/Limelighter.java
View File

@@ -50,21 +50,32 @@ public class Limelighter extends Command {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
if(!alliance.isPresent()){return;}
if(alliance.get() == Alliance.Blue){
BotPose = limelight3g.getBotPoseBlue();
}
else{
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
if(angle >180){
angle -= 360;
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle);
if(calcul < -5 && calcul > -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/Math.PI));
}
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 10;
else if(calcul > 5 && calcul < 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
}
else if(calcul >= 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/Math.PI));
}
else{
drivetrain.setControl(
drive.withRotationalRate(0));
}
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(angle);

50
src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java
View File

@@ -13,10 +13,7 @@ import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Limelight3G;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.*;
import java.util.Optional;
@@ -30,7 +27,7 @@ public class LimelighterAuto extends Command {
double boty;
double angle;
double calcul;
Optional<Alliance> alliance = DriverStation.getAlliance();
Optional<Alliance> alliance = DriverStation.getAlliance();
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -45,35 +42,44 @@ public class LimelighterAuto extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
if(!alliance.isPresent()){return;}
if(alliance.get() == Alliance.Blue){
BotPose = limelight3g.getBotPoseBlue();
}
else{
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
if(angle >180){
angle -= 360;
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle);
if(calcul < -5 && calcul > -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/Math.PI));
}
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5;
else if(calcul > 5 && calcul < 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
}
else if(calcul >= 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/Math.PI));
}
else{
drivetrain.setControl(
drive.withRotationalRate(0));
}
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println("Limelighter");
System.out.println(calcul);
if (calcul < 0.3 && calcul > -0.3) {
System.out.println(angle);
if (calcul < 0.2 && calcul > -0.2) {
drivetrain.setControl(drive.withRotationalRate(0));
}
}
@@ -92,7 +98,7 @@ public class LimelighterAuto extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return calcul < 0.3 && calcul > -0.3;
return calcul < 5 && calcul > -5;
}
}

17
src/main/java/frc/robot/subsystems/Limelight3G.java
View File

@@ -52,7 +52,22 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
}
public double Calcule(double x1, double x2, double y1, double y2, double angle)
{
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
if(x1 > x2){
if(y1 > y2){
return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle;
}
else{
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle;
}
}
else{
if(y1 > y2){
return Math.atan(90-((x2 - x1) / (y2 - y1)))* (180 / Math.PI)+270 - angle;
}
else{
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+180 - angle;
}
}
}
@Override
public void periodic() {