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

Compare commits

base: sdesharnais:coaching
Branches Tags
sdesharnais:main sdesharnais:coaching
...
compare: sdesharnais:098f16665a5ad1c94200de52b79b65a979ac7a4a
Branches Tags
sdesharnais:coaching sdesharnais:main

4 Commits
coaching ... 098f16665a

Author SHA1 Message Date
odubois
098f16665a limelight pls marche 2026-03-31 05:19:16 +02:00
odubois
1240160ed7 Merge branch 'main' of https://git.demerso.net/sdesharnais/Rebuilt-2026 2026-03-31 03:31:20 +02:00
odubois
4068016d36 get alliance -> dans initialise 2026-03-31 03:31:15 +02:00
Antoine PerreaultE
6afc342006 limelight 2026-03-30 17:53:54 -04:00
9 changed files with 137 additions and 66 deletions
Expand all files Collapse all files

1
src/main/java/frc/robot/commands/Lancer.java
View File

@@ -40,6 +40,7 @@ public class Lancer extends Command {
if(limeLight3G.getV()){}
pidController = new PIDController(0.0007, 0,0, 0.001);
timer.reset();
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.

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

@@ -27,6 +27,7 @@ public class Limelighter extends Command {
double boty;
double angle;
double calcul;
double x;
Optional<Alliance> alliance = DriverStation.getAlliance();
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
@@ -42,7 +43,9 @@ public class Limelighter extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
@@ -50,38 +53,63 @@ public class Limelighter extends Command {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
// BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) {
return;
}
if (alliance.get() == Alliance.Blue) {
x = 4.6;
BotPose = limelight3g.getBotPoseBlue();
}
else {
x = 11.915394;
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
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, x, boty, 4, angle);
if(angle > 180){
angle -= 360;
}
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{
if(calcul > -5 && calcul < 5){
drivetrain.setControl(
drive.withRotationalRate(0));
}
else if(calcul > 5){
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(angle);
if (calcul < 0.2 && calcul > -0.2) {
drivetrain.setControl(drive.withRotationalRate(0));
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul < -5){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
// if(calcul < -5 && calcul > -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else if(calcul > 5 && calcul < 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul >= 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul <= -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else{
// drivetrain.setControl(
// drive.withRotationalRate(0));
// }
// drivetrain.setControl(
// drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
// System.out.println(angle);
// if (calcul < 0.2 && calcul > -0.2) {
// drivetrain.setControl(drive.withRotationalRate(0));
// }
}
else{
drivetrain.setControl(drive.withRotationalRate(0));

4
src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java
View File

@@ -47,7 +47,9 @@ public class GrimperMur extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.
@Override

4
src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
View File

@@ -47,7 +47,9 @@ public class GrimperReservoir extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.
@Override

1
src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java
View File

@@ -38,6 +38,7 @@ public class LancerAuto extends Command {
public void initialize() {
pidController = new PIDController(0.0007, 0, 0, 0.001);
timer.reset();
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.

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

@@ -27,6 +27,7 @@ public class LimelighterAuto extends Command {
double boty;
double angle;
double calcul;
double x;
Optional<Alliance> alliance = DriverStation.getAlliance();
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
@@ -42,7 +43,9 @@ public class LimelighterAuto extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
@@ -50,38 +53,63 @@ public class LimelighterAuto extends Command {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
// BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) {
return;
}
if (alliance.get() == Alliance.Blue) {
x = 4.6;
BotPose = limelight3g.getBotPoseBlue();
}
else {
x = 11.915394;
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
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, x, boty, 4, angle);
if(angle > 180){
angle -= 360;
}
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{
if(calcul > -5 && calcul < 5){
drivetrain.setControl(
drive.withRotationalRate(0));
}
else if(calcul > 5){
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(angle);
if (calcul < 0.2 && calcul > -0.2) {
drivetrain.setControl(drive.withRotationalRate(0));
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul < -5){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
// if(calcul < -5 && calcul > -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else if(calcul > 5 && calcul < 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul >= 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul <= -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else{
// drivetrain.setControl(
// drive.withRotationalRate(0));
// }
// drivetrain.setControl(
// drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
// System.out.println(angle);
// if (calcul < 0.2 && calcul > -0.2) {
// drivetrain.setControl(drive.withRotationalRate(0));
// }
}
else{
drivetrain.setControl(drive.withRotationalRate(0));
@@ -98,7 +126,6 @@ public class LimelighterAuto extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return calcul < 5 && calcul > -5;
return calcul > -5 && calcul < 5;
}
}

4
src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java
View File

@@ -37,7 +37,9 @@ public class TournerVersMur extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.
@Override

4
src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java
View File

@@ -37,7 +37,9 @@ public class TournerVersReservoir extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.
@Override

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

@@ -52,21 +52,27 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
}
public double Calcule(double x1, double x2, double y1, double y2, double angle)
{
if(x1 > x2){
// 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((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle;
// }
// else{
// return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle;
// }
// }
if(y1 > y2){
return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle;
return Math.atan((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;
}
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)-360 - angle;
}
}
@Override