PLS5618/Reefscape-2025
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025

Browse Source
This commit is contained in:
Antoine PerreaultE
2026-01-19 18:29:04 -05:00
parent 5a8a499e05 9fa5624504
commit 3f546b61d6
5 changed files with 48 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
src/main/java/frc/robot/commands/Limelight/AprilTag3.java
View File

@@ -54,49 +54,51 @@ public class AprilTag3 extends Command {
public void execute() {
double[] BotPose = new double[6];
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
double y2 = 4;
double x2 = 0;
double angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
if(angle >180){
angle =- angle * 2;
}
if(alliance.get() == DriverStation.Alliance.Red){
BotPose = limelight3.getBotPoseRed();
x2 =11.9;
}
else{
BotPose = limelight3.getBotPoseBlue();
x2 = 4.1;
}
double botx = BotPose[0];
System.out.println(botx);
double boty = BotPose[1];
double tx = limelight3.getTx();
double tagId = limelight3.getTId();
drivetrain.setControl(drive.
withRotationalRate(tx/20).
withVelocityX((botx-5.81)*2).
withVelocityY((boty-4)*4));
// if(limelight3.getV() == true){
// if(tagId ==10){
// drivetrain.setControl(drive.
// withRotationalRate(tx/20).
// withVelocityX((botx-5.81)*2).
// withVelocityY((boty-4)*4));
// }
// else if(tagId ==7){
// drivetrain.setControl(drive.
// withRotationalRate(tx/20).
// withVelocityX(2-botx).
// withVelocityY(2-boty));
if(limelight3.getV() == true){
drivetrain.setControl(drive.
withRotationalRate(limelight3.Calcule(botx,x2,boty,y2,angle)).
withVelocityX(x.getAsDouble()).
withVelocityY(y.getAsDouble()));
// if(tagId ==10){
// drivetrain.setControl(drive.
// withRotationalRate(tx/20).
// withVelocityX((botx-5.81)*2).
// withVelocityY((boty-4)*4));
// }
// else if(tagId ==7){
// drivetrain.setControl(drive.
// withRotationalRate(tx/20).
// withVelocityX(2-botx).
// withVelocityY(2-boty));
// }
// }
// if(limelight3.getV()){
// drivetrain.setControl(drive.
// withRotationalRate(limelight3.getTx()/10).
// withVelocityX(x.getAsDouble()).
// withVelocityY(y.getAsDouble()));
// }
// else{
// drivetrain.setControl(drive.
// withRotationalRate(0).
// withVelocityX(0).
// withVelocityY(0));
// }
}
// }
}
else{
drivetrain.setControl(drive.
withRotationalRate(0).
withVelocityX(0).
withVelocityY(0));
}
}
// Called once the command ends or is interrupted.
@Override

2
src/main/java/frc/robot/subsystems/Bougie.java
View File

@@ -54,7 +54,7 @@ public class Bougie extends SubsystemBase {
if(led==15){
x=true;
System.out.println("false");
}}
}}
// candle.setLEDs(255, 0, 0,0,24,8);
// candle.setLEDs(255, 0, 0,0,40,8);
// candle.setLEDs(255, 0, 0,0,56,8);

2
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View File

@@ -48,7 +48,7 @@ Translation2d backRightLocation = new Translation2d(-0.3, -0.3);
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
private void configureAutoBuilder() {
try {
var config = RobotConfig.fromGUISettings();
RobotConfig config = RobotConfig.fromGUISettings();
AutoBuilder.configure(
() -> getState().Pose, // Supplier of current robot pose
this::resetPose, // Consumer for seeding pose against auto

11
src/main/java/frc/robot/subsystems/Limelight3.java
View File

@@ -69,6 +69,17 @@ public class Limelight3 extends SubsystemBase {
public void Forme(){
pipeline.setNumber(0);
}
public double Calcule(double x1, double x2, double y1, double y2, double angle)
{
if (x1 > 4)
{
return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) - angle)/90;
}
else
{
return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) + angle) * -1/90;
}
}
@Override
public void periodic() {
// This method will be called once per scheduler run