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

grimpe mode auto

Browse Source
This commit is contained in:
samuel desharnais
2026-04-02 20:34:53 -04:00
parent 6885b80d6a
commit 6c86b2ed5c
8 changed files with 40 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,6 +7,7 @@ package frc.robot.commands;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
@@ -51,7 +52,6 @@ public class Limelighter extends Command {
@Override
public void execute() {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) {
@@ -69,19 +69,21 @@ public class Limelighter extends Command {
botx = BotPose[0];
boty = BotPose[1];
angle = BotPose[5];
calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(calcul > -5 && calcul < 5){
drivetrain.setControl(
drive.withRotationalRate(0));
}
else if(calcul > 5){
drivetrain.setControl(
drive.withRotationalRate(2));
}
else if(calcul < -5){
drivetrain.setControl(drive.withRotationalRate(-2));
}
// botx = BotPose[1];
calcul = Math.toRadians(limelight3g.Calcule(boty, 4,botx, x, angle));
System.out.println(calcul);
drivetrain.setControl(drive.withRotationalRate(MathUtil.clamp(calcul, -3, 3)));
// if(calcul > -5 && calcul < 5){
// drivetrain.setControl(
// drive.withRotationalRate(0));
// }
// else if(calcul > 5){
// drivetrain.setControl(
// drive.withRotationalRate(2));
// }
// else if(calcul < -5){
// drivetrain.setControl(drive.withRotationalRate(-2));
// }
// // botx = BotPose[1];
// boty = BotPose[0];
// angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
// calcul = limelight3g.Calcule(botx, x, boty, 4, angle);

13
src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java
View File

@@ -46,23 +46,12 @@ public class BougerDroiteAuto extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(alliance.get() == Alliance.Blue){
if(timer.get() < .75){
System.out.println("8765");
if(timer.get() < 1.25){
drivetrain.setControl(drive.withVelocityY(-1.5));
}
else{
drivetrain.setControl(drive.withVelocityY(0));
}
}
else{
if(timer.get() < 0.75){
drivetrain.setControl(drive.withVelocityY(1.5));
}
else{
drivetrain.setControl(drive.withVelocityY(0));
}
}
}
// Called once the command ends or is interrupted.

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

@@ -51,7 +51,7 @@ public class GrimperMur extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {
// alliance = DriverStation.getAlliance();
alliance = DriverStation.getAlliance();
// if(drivetrain.Equipe()){
// angle+=180;
// }
@@ -70,38 +70,38 @@ public class GrimperMur extends Command {
if(alliance.get() == Alliance.Red){
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
System.out.println(pigeonAngle);
y = 2.6;
x = 15.6;
y = 3.6;
x = 14.9;
angle = 180;
if(pigeonAngle< 190 && pigeonAngle> 170){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*6.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*6.5),-2,2)));
if(pigeonAngle> 358 || pigeonAngle< 2){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*6.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*6.5),-2,2)).withRotationalRate(0));
}
else{
if(pigeonAngle>180){
drivetrain.setControl(drive.withRotationalRate(-1));
drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(1));
}
else{
drivetrain.setControl(drive.withRotationalRate(1));
drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(-1));
}
}
}
else{
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180;
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
System.out.println(pigeonAngle);
y = 5.2;
x = 1.11;
y = 4.4;
x = 1.7;
angle = 0;
if(pigeonAngle> 358 || pigeonAngle< 2){
if(pigeonAngle> 182 || pigeonAngle< 178){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0));
}
else{
if(pigeonAngle>0 && pigeonAngle<180){
drivetrain.setControl(drive.withRotationalRate(-1));
drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(-1));
System.out.println("x");
}
else if(pigeonAngle>180){
System.out.println("e");
drivetrain.setControl(drive.withRotationalRate(1));
drivetrain.setControl(drive.withVelocityY(0).withVelocityX(0).withRotationalRate(1));
}
}
}