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

vgft

Browse Source
This commit is contained in:
samuel desharnais
2026-03-28 20:50:33 -04:00
parent 755d79aa18
commit 673a7fcb82
7 changed files with 43 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -37,7 +37,7 @@ public class Lancer extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {
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;}
if(limeLight3G.getV()){}
pidController = new PIDController(0.0007, 0,0, 0.001);
timer.reset();
}
@@ -47,7 +47,7 @@ public class Lancer extends Command {
public void execute() {
double[] BotPose = new double[6];
if(limeLight3G.getV()){
if(!alliance.isPresent()){return;}
if(alliance.get() == Alliance.Blue){
BotPose = limeLight3G.getBotPoseBlue();
}
@@ -56,7 +56,8 @@ public class Lancer extends Command {
}
botx = BotPose[0];
boty = BotPose[1];
}
vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;
}
if(limeLight3G.getV()){
System.out.println(vitesse);

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

@@ -50,13 +50,14 @@ 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();

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

@@ -22,7 +22,8 @@ public class LancerAuto extends Command {
Limelight3G limelight3g;
double botx = 0;
double boty = 0;
Optional<Alliance> alliance = DriverStation.getAlliance();
Optional<Alliance> alliance = DriverStation.getAlliance();
/** Creates a new LancerAuto. */
public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) {
this.lanceur = lanceur;
@@ -35,41 +36,43 @@ public class LancerAuto extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {
pidController = new PIDController(0.0007, 0,0, 0.001);
pidController = new PIDController(0.0007, 0, 0, 0.001);
timer.reset();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double[] BotPose = new double[6];
if(limelight3g.getV()){
if(alliance.isPresent()){
if(alliance.get() == Alliance.Blue){
BotPose = limelight3g.getBotPoseBlue();
if (limelight3g.getV()) {
if (!alliance.isPresent()) {
return;
}
else{
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;
System.out.println(vitesse);
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;
System.out.println("lancer");
double output = pidController.calculate(lanceur.Vitesse(),vitesse);
double output = pidController.calculate(lanceur.Vitesse(), vitesse);
lanceur.Lancer(output);
if(lanceur.Vitesse() >= vitesse-800){
if (lanceur.Vitesse() >= vitesse - 800) {
timer.start();
if(timer.get() >1){
if (timer.get() > 1) {
lanceur.Demeler(1);
}
}
}
}
}
// Called once the command ends or is interrupted.
@Override
@@ -84,6 +87,6 @@ public class LancerAuto extends Command {
@Override
public boolean isFinished() {
return timer.get() > 4;
//return false;
// return false;
}
}

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

@@ -53,16 +53,15 @@ public class LimelighterAuto extends Command {
@Override
public void execute() {
double[] BotPose = new double[6];
System.out.println("a");
if (limelight3g.getV()) {
if(alliance.isPresent()){
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();
@@ -72,8 +71,9 @@ public class LimelighterAuto extends Command {
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5;
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println("Limelighter");
System.out.println(calcul);
if (calcul < 0.1 && calcul > -0.1) {
if (calcul < 0.3 && calcul > -0.3) {
drivetrain.setControl(drive.withRotationalRate(0));
}
}
@@ -92,7 +92,7 @@ public class LimelighterAuto extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return calcul < 0.1 && calcul > -0.1;
return calcul < 0.3 && calcul > -0.3;
}
}