From 6ccef6bad9dee2e989325888a69a8aa27ae93825 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Tue, 18 Feb 2025 18:47:13 -0500 Subject: [PATCH] changement limelight --- .../deploy/pathplanner/autos/BlueBas1.auto | 2 +- src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/RobotContainer.java | 38 ++++++------------- .../subsystems/CommandSwerveDrivetrain.java | 11 +++--- 4 files changed, 19 insertions(+), 36 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/BlueBas1.auto b/src/main/deploy/pathplanner/autos/BlueBas1.auto index 06657fc..6b665ea 100644 --- a/src/main/deploy/pathplanner/autos/BlueBas1.auto +++ b/src/main/deploy/pathplanner/autos/BlueBas1.auto @@ -5,7 +5,7 @@ "data": { "commands": [ { - "type": "sequential", + "type": "parallel", "data": { "commands": [ { diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 1b76a41..d25855b 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,12 +9,12 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 45.3592, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, - "maxDriveSpeed": 5.0, + "maxDriveSpeed": 5.261, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8bfde90..ba6580b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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)); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 317f62b..8c27352 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -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;