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

Merge branch 'main' of https://git.demerso.net/sdesharnais/Rebuilt-2026

Browse Source
This commit is contained in:
Antoine PerreaultE
2026-03-28 13:21:35 -04:00
parent a92a7a0ea1 ba57959f4d
commit 85da1dcad1
8 changed files with 332 additions and 304 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,7 +4,11 @@
package frc.robot.commands.ModeAuto;
import java.util.Optional;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur;
@@ -18,6 +22,7 @@ public class LancerAuto extends Command {
Limelight3G limelight3g;
double botx = 0;
double boty = 0;
Optional<Alliance> alliance = DriverStation.getAlliance();
/** Creates a new LancerAuto. */
public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) {
this.lanceur = lanceur;
@@ -38,13 +43,19 @@ public class LancerAuto extends Command {
public void execute() {
double[] BotPose = new double[6];
if(limelight3g.getV()){
BotPose = limelight3g.getBotPoseBlue();
if(alliance.get() == Alliance.Blue){
BotPose = limelight3g.getBotPoseBlue();
}
else{
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[0];
boty = BotPose[1];
}
double vitesse = 0.5;
if(limelight3g.getV()){
vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;
vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;
System.out.println(vitesse);
double output = pidController.calculate(lanceur.Vitesse(),vitesse);

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

@@ -13,7 +13,10 @@ 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.*;
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 java.util.Optional;