Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025
This commit is contained in:
Samuel
@@ -14,7 +14,6 @@ import com.pathplanner.lib.auto.AutoBuilder;
|
||||
|
||||
import edu.wpi.first.cameraserver.CameraServer;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.networktables.GenericEntry;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
@@ -23,12 +22,9 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
||||
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.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.TunerConstants.TunerConstants;
|
||||
import frc.robot.commands.RainBow;
|
||||
import frc.robot.commands.reset;
|
||||
import frc.robot.commands.Elevateur.Depart;
|
||||
import frc.robot.commands.Elevateur.ElevateurManuel;
|
||||
@@ -96,7 +92,6 @@ public class RobotContainer {
|
||||
Bougie bougie = new Bougie();
|
||||
Limelight3G limelight3g = new Limelight3G();
|
||||
Limelight3 limelight3 = new Limelight3();
|
||||
Pose2d pose = new Pose2d();
|
||||
Requin requin = new Requin();
|
||||
CorailAspir corailAspir = new CorailAspir(pince, bougie);
|
||||
public RobotContainer() {
|
||||
@@ -107,7 +102,6 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
private void configureBindings() {
|
||||
drivetrain.registerTelemetry(logger::telemeterize);
|
||||
drivetrain.setDefaultCommand(
|
||||
// Drivetrain will execute this command periodically
|
||||
drivetrain.applyRequest(() ->
|
||||
@@ -122,7 +116,7 @@ public class RobotContainer {
|
||||
manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
|
||||
manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie));
|
||||
manette1.povLeft().whileTrue(new AlgueExpire(pince, bougie));
|
||||
manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
||||
// manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain1,manette1::getLeftX,manette1::getLeftY));
|
||||
manette1.povRight().whileTrue(new CoralExpire(pince, bougie));
|
||||
manette1.leftTrigger().whileTrue(new DepartPince(pince));
|
||||
manette1.povDown().whileTrue(new Algue_inspire(pince,bougie));
|
||||
@@ -146,8 +140,8 @@ public class RobotContainer {
|
||||
manette2.x().whileTrue(new ExpireCorail(requin, bougie));
|
||||
|
||||
//limelight
|
||||
manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
||||
manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
|
||||
// manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
||||
// manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
|
||||
|
||||
//Pince manuel
|
||||
pince.setDefaultCommand(new RunCommand(()->{
|
||||
@@ -161,6 +155,7 @@ public class RobotContainer {
|
||||
|
||||
//Reset encodeur
|
||||
manette2.start().whileTrue(new reset(elevateur, pince, requin));
|
||||
drivetrain.registerTelemetry(logger::telemeterize);
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
|
||||
@@ -76,6 +76,7 @@ public class AprilTag3 extends Command {
|
||||
withRotationalRate(tx/20).
|
||||
withVelocityX(2-botx).
|
||||
withVelocityY(2-boty));
|
||||
|
||||
}
|
||||
}
|
||||
else{
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
|
||||
import java.util.function.Supplier;
|
||||
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;
|
||||
@@ -11,9 +11,9 @@ 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.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
@@ -39,46 +39,88 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
|
||||
/* Keep track if we've ever applied the operator perspective before or not */
|
||||
private boolean m_hasAppliedOperatorPerspective = false;
|
||||
private SwerveDriveOdometry odometry;
|
||||
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
|
||||
|
||||
// Position des modules par rapport au centre du robot
|
||||
Translation2d frontLeftLocation = new Translation2d(+0.3, +0.3);
|
||||
Translation2d frontRightLocation = new Translation2d(+0.3, -0.3);
|
||||
Translation2d backLeftLocation = new Translation2d(-0.3, +0.3);
|
||||
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 {
|
||||
RobotConfig config = RobotConfig.fromGUISettings();
|
||||
AutoBuilder.configure(
|
||||
|
||||
() -> getState().Pose, // Supplier of current robot pose
|
||||
this::resetOdometry, // Consumer for seeding pose against auto
|
||||
this::resetPose, // Consumer for seeding pose against auto
|
||||
() -> getState().Speeds, // Supplier of current robot speeds
|
||||
|
||||
// Consumer of ChassisSpeeds and feedforwards to drive the robot
|
||||
(speeds, feedforwards) -> setControl(
|
||||
m_pathApplyRobotSpeeds.withSpeeds(speeds)
|
||||
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons())
|
||||
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons())
|
||||
m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
|
||||
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
|
||||
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
|
||||
),
|
||||
new PPHolonomicDriveController(
|
||||
// PID constants for translation
|
||||
new PIDConstants(5, 0, 0),
|
||||
// // PID constants for rotation
|
||||
// new PIDConstants(7.9735, 0, 0.038499)
|
||||
new PIDConstants(10, 0, 0),
|
||||
// PID constants for rotation
|
||||
new PIDConstants(5,0,0)
|
||||
new PIDConstants(7, 0, 0)
|
||||
),
|
||||
config,
|
||||
// Assume the path needs to be flipped for Red vs Blue, this is normally the case
|
||||
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
|
||||
this // Subsystem for requirements
|
||||
|
||||
);
|
||||
} catch (Exception ex) {
|
||||
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
|
||||
}
|
||||
PPHolonomicDriveController.overrideRotationFeedback(()->{
|
||||
return 0;
|
||||
});
|
||||
}
|
||||
// try {
|
||||
// var config = RobotConfig.fromGUISettings();
|
||||
// AutoBuilder.configure(
|
||||
// () -> getState().Pose, // Supplier of current robot pose
|
||||
// this::resetPose, // Consumer for seeding pose against auto
|
||||
// () -> getState().Speeds, // Supplier of current robot speeds
|
||||
|
||||
// // Consumer of ChassisSpeeds and feedforwards to drive the robot
|
||||
// (speeds, feedforwards) -> setControl(
|
||||
// m_pathApplyRobotSpeeds.withSpeeds(speeds)
|
||||
// .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons())
|
||||
// .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons())
|
||||
// ),
|
||||
// new PPHolonomicDriveController(
|
||||
// // PID constants for translation
|
||||
// new PIDConstants(5, 0, 0),
|
||||
// // // PID constants for rotation
|
||||
// // new PIDConstants(7.9735, 0, 0.038499)
|
||||
// // PID constants for rotation
|
||||
// new PIDConstants(5,0,0)
|
||||
// ),
|
||||
// config,
|
||||
// // Assume the path needs to be flipped for Red vs Blue, this is normally the case
|
||||
// () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
|
||||
// this // Subsystem for requirements
|
||||
|
||||
// );
|
||||
// } catch (Exception ex) {
|
||||
// DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
|
||||
// }
|
||||
// PPHolonomicDriveController.overrideRotationFeedback(()->{
|
||||
// return 0;
|
||||
// });
|
||||
}
|
||||
// public void driveRobotRelative(ChassisSpeeds speeds) {
|
||||
|
||||
// // Convertir robot-relative ChassisSpeeds → SwerveModuleStates
|
||||
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
|
||||
|
||||
// // Limitations des roues
|
||||
// SwerveDriveKinematics.desaturateWheelSpeeds(states, TunerConstants.kSpeedAt12Volts);
|
||||
|
||||
// // Envoi aux modules
|
||||
// frontLeft.setDesiredState(states[0]);
|
||||
// frontRight.setDesiredState(states[1]);
|
||||
// backLeft.setDesiredState(states[2]);
|
||||
// backRight.setDesiredState(states[3]);
|
||||
// }
|
||||
/**
|
||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||
* <p>
|
||||
@@ -97,27 +139,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
if (Utils.isSimulation()) {
|
||||
startSimThread();
|
||||
}
|
||||
configureAutoBuilder();
|
||||
}
|
||||
// public void resetPose(Pose2d pose) {
|
||||
// // Reset gyro to match starting rotation
|
||||
// pigeon2.setYaw(pose.getRotation().getDegrees());
|
||||
|
||||
// // Reset odometry
|
||||
// m_odometry.resetPosition(
|
||||
// pose.getRotation(),
|
||||
// getState().ModulePositions,
|
||||
// pose
|
||||
// );
|
||||
//}
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
odometry = new SwerveDriveOdometry(getKinematics(), kBlueAlliancePerspectiveRotation, getState().ModulePositions);
|
||||
odometry.resetPosition(
|
||||
getPigeon2().getRotation2d(),
|
||||
getState().ModulePositions,
|
||||
pose
|
||||
);
|
||||
this.getPigeon2().setYaw(pose.getRotation().getDegrees());
|
||||
configureAutoBuilder();
|
||||
}
|
||||
/**
|
||||
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
||||
|
||||
Reference in New Issue
Block a user