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

changement limelight

Browse Source
This commit is contained in:
Antoine PerreaultE
2025-02-18 18:47:13 -05:00
parent 7cf7483498
commit 6ccef6bad9
4 changed files with 19 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
src/main/java/frc/robot/RobotContainer.java
View File

@@ -5,33 +5,23 @@
package frc.robot;
import static edu.wpi.first.units.Units.*;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt;
import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.util.FlippingUtil;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.robot.TunerConstants.TunerConstants;
import frc.robot.commands.AprilTag3;
import frc.robot.commands.AprilTag3G;
@@ -39,11 +29,9 @@ import frc.robot.commands.Forme3;
import frc.robot.commands.RainBow;
import frc.robot.subsystems.Bougie;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Grimpeur;
import frc.robot.subsystems.Limelight3;
import frc.robot.subsystems.Limelight3G;
public class RobotContainer {
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
@@ -88,7 +76,6 @@ public class RobotContainer {
)
);
// reset the field-centric heading on left bumper press
manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
@@ -97,19 +84,16 @@ public class RobotContainer {
drivetrain.registerTelemetry(logger::telemeterize);
}
public Command getAutonomousCommand() {
return new SequentialCommandGroup(Commands.runOnce(()->{
boolean flip = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;
if(flip){
drivetrain.resetPose(FlippingUtil.flipFieldPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose()));
}
else{
drivetrain.resetPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose());
}
}),autoChooser.getSelected(), new RainBow(bougie));
public Command getAutonomousCommand() {
return new SequentialCommandGroup(Commands.runOnce(()->{
boolean flip = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;
if(flip){
drivetrain.resetPose(FlippingUtil.flipFieldPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose()));
}
else{
drivetrain.resetPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose());
}
}),autoChooser.getSelected(), new RainBow(bougie));
}
}
}

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

@@ -3,19 +3,15 @@ package frc.robot.subsystems;
import static edu.wpi.first.units.Units.*;
import java.util.function.Supplier;
import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
@@ -140,8 +136,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
} catch (Exception ex) {
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
}
}
PPHolonomicDriveController.overrideRotationFeedback(()->{
return 0;
});
}
/* The SysId routine to test */
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;