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

Compare commits

base: PLS5618:Limelight
Branches Tags
PLS5618:main PLS5618:grimpe PLS5618:requin PLS5618:pince PLS5618:elevateur PLS5618:Limelight PLS5618:Swerve PLS5618:Led
..
compare: PLS5618:8bc8f0390f722f16a19c4cfecd45a7a33ffee8dd
Branches Tags
PLS5618:main PLS5618:grimpe PLS5618:requin PLS5618:pince PLS5618:elevateur PLS5618:Limelight PLS5618:Swerve PLS5618:Led

1 Commits
Limelight ... 8bc8f0390f

Author SHA1 Message Date
samuel desharnais
8bc8f0390f mode auto leds mieux 2025-02-11 19:57:16 -05:00
21 changed files with 102 additions and 2278 deletions
Expand all files Collapse all files

1
.SysId/sysid.json
View File

@@ -1 +0,0 @@
{}

5
simgui-ds.json
View File

@@ -1,9 +1,4 @@
{ {
"System Joysticks": {
"window": {
"enabled": false
}
},
"keyboardJoysticks": [ "keyboardJoysticks": [
{ {
"axisConfig": [ "axisConfig": [

126
src/main/deploy/pathplanner/autos/BlueBas3 sans composantes.auto
View File

@@ -1,126 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "BlueBasStart"
}
},
{
"type": "named",
"data": {
"name": "AprilTag"
}
}
]
}
},
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "BlueBasChercher"
}
},
{
"type": "named",
"data": {
"name": "AprilTag"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "AprilTag"
}
},
{
"type": "path",
"data": {
"pathName": "BlueBasPorter"
}
}
]
}
},
{
"type": "wait",
"data": {
"waitTime": 1.0
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "BlueBasChercher2"
}
},
{
"type": "sequential",
"data": {
"commands": []
}
},
{
"type": "named",
"data": {
"name": "AprilTag"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "BlueBasPorter2"
}
},
{
"type": "named",
"data": {
"name": "AprilTag"
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

19
src/main/deploy/pathplanner/autos/New Auto.auto
View File

@@ -1,19 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "New Path"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

6
src/main/deploy/pathplanner/autos/Sortir.auto
View File

@@ -4,12 +4,6 @@
"type": "sequential", "type": "sequential",
"data": { "data": {
"commands": [ "commands": [
{
"type": "wait",
"data": {
"waitTime": 3.0
}
},
{ {
"type": "path", "type": "path",
"data": { "data": {

2
src/main/deploy/pathplanner/navgrid.json
View File

File diff suppressed because one or more lines are too long

10
src/main/deploy/pathplanner/paths/BlueBasStart.path
View File

@@ -8,8 +8,8 @@
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 6.40062663880838, "x": 6.400623816881145,
"y": 1.8077964923366423 "y": 1.8077934662846444
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
@@ -33,8 +33,8 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 1.0, "maxVelocity": 3.0,
"maxAcceleration": 1.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": 119.99999999999999 "rotation": 119.99999999999999
}, },
"useDefaultConstraints": false "useDefaultConstraints": true
} }

54
src/main/deploy/pathplanner/paths/New Path.path
View File

@@ -1,54 +0,0 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.8820696721311476,
"y": 6.0
},
"prevControl": null,
"nextControl": {
"x": 1.8706512438743716,
"y": 5.690872003047288
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.8820696721311476,
"y": 3.1279999999999997
},
"prevControl": {
"x": 1.8863317961339294,
"y": 3.3779636659576444
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0.0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": false
}

20
src/main/deploy/pathplanner/paths/Sortir.path
View File

@@ -8,20 +8,20 @@
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 7.119564732833961, "x": 7.084733606557376,
"y": 6.478684258786044 "y": 6.488473360655737
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 5.815652951699465, "x": 5.849999999999999,
"y": 6.464497950819672 "y": 6.464497950819672
}, },
"prevControl": { "prevControl": {
"x": 6.633711863494241, "x": 6.32950819672131,
"y": 6.455611583097 "y": 6.464497950819672
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@@ -33,10 +33,10 @@
"pointTowardsZones": [], "pointTowardsZones": [],
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 1.0, "maxVelocity": 3.0,
"maxAcceleration": 1.0, "maxAcceleration": 3.0,
"maxAngularVelocity": 50.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 100.0, "maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0, "nominalVoltage": 12.0,
"unlimited": false "unlimited": false
}, },
@@ -50,5 +50,5 @@
"velocity": 0, "velocity": 0,
"rotation": 180.0 "rotation": 180.0
}, },
"useDefaultConstraints": false "useDefaultConstraints": true
} }

4
src/main/deploy/pathplanner/settings.json
View File

@@ -9,12 +9,12 @@
"defaultMaxAngVel": 540.0, "defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0, "defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0, "defaultNominalVoltage": 12.0,
"robotMass": 45.3592, "robotMass": 74.088,
"robotMOI": 6.883, "robotMOI": 6.883,
"robotTrackwidth": 0.546, "robotTrackwidth": 0.546,
"driveWheelRadius": 0.048, "driveWheelRadius": 0.048,
"driveGearing": 5.143, "driveGearing": 5.143,
"maxDriveSpeed": 5.261, "maxDriveSpeed": 5.45,
"driveMotorType": "krakenX60", "driveMotorType": "krakenX60",
"driveCurrentLimit": 60.0, "driveCurrentLimit": 60.0,
"wheelCOF": 1.2, "wheelCOF": 1.2,

1645
src/main/java/frc/robot/LimelightHelpers.java
View File

File diff suppressed because it is too large Load Diff

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

@@ -5,33 +5,32 @@
package frc.robot; package frc.robot;
import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.*;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; 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.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands; 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.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.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.robot.TunerConstants.TunerConstants; import frc.robot.TunerConstants.TunerConstants;
import frc.robot.commands.AprilTag3;
import frc.robot.commands.AprilTag3G;
import frc.robot.commands.Forme3;
import frc.robot.commands.RainBow; import frc.robot.commands.RainBow;
import frc.robot.subsystems.Bougie; import frc.robot.subsystems.Bougie;
import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Limelight3; import frc.robot.subsystems.Grimpeur;
import frc.robot.subsystems.Limelight3G;
public class RobotContainer { public class RobotContainer {
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
@@ -45,24 +44,18 @@ public class RobotContainer {
private final Telemetry logger = new Telemetry(MaxSpeed); private final Telemetry logger = new Telemetry(MaxSpeed);
private final CommandXboxController manette1 = new CommandXboxController(0); private final CommandXboxController manette1 = new CommandXboxController(0);
private final CommandXboxController manette2 = new CommandXboxController(1); private final CommandXboxController manette2 = new CommandXboxController(0);
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
private final SendableChooser<Command> autoChooser; private final SendableChooser<Command> autoChooser;
Bougie bougie = new Bougie(); Bougie bougie = new Bougie();
Limelight3G limelight3g = new Limelight3G();
Limelight3 limelight3 = new Limelight3();
Pose2d pose = new Pose2d();
private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2
public double getAngle() {
return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot
}
public RobotContainer() { public RobotContainer() {
autoChooser = AutoBuilder.buildAutoChooser("New Auto"); autoChooser = AutoBuilder.buildAutoChooser("New Auto");
SmartDashboard.putData("Auto Mode", autoChooser); SmartDashboard.putData("Auto Mode", autoChooser);
configureBindings(); configureBindings();
NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null));
} }
private void configureBindings() { private void configureBindings() {
@@ -71,34 +64,31 @@ public class RobotContainer {
drivetrain.setDefaultCommand( drivetrain.setDefaultCommand(
// Drivetrain will execute this command periodically // Drivetrain will execute this command periodically
drivetrain.applyRequest(() -> drivetrain.applyRequest(() ->
drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.2)) // Drive forward with negative Y (forward) drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY(), 0.5)) // Drive forward with negative Y (forward)
.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.2)) // Drive left with negative X (left) .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX(), 0.5)) // Drive left with negative X (left)
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.15)) // Drive counterclockwise with negative X (left) .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.5)) // Drive counterclockwise with negative X (left)
) )
); );
// reset the field-centric heading on left bumper press // reset the field-centric heading on left bumper press
manette1.y().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
manette1.a().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
manette1.b().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
manette1.x().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
drivetrain.registerTelemetry(logger::telemeterize); 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(
autoChooser
.getSelected().andThen(
new
RainBow
(
bougie
)
));
}
} }

12
src/main/java/frc/robot/TunerConstants/TunerConstants.java
View File

@@ -23,19 +23,15 @@ public class TunerConstants {
// The steer motor uses any SwerveModule.SteerRequestType control request with the // The steer motor uses any SwerveModule.SteerRequestType control request with the
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
//private static final Slot0Configs steerGains = new Slot0Configs()
// .withKP(68.294).withKI(0).withKD(4.7806)
// .withKS(0.20754).withKV(2.4832).withKA(0.099824)
// .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
private static final Slot0Configs steerGains = new Slot0Configs() private static final Slot0Configs steerGains = new Slot0Configs()
.withKP(43.502).withKI(0).withKD(2.7353) .withKP(100).withKI(0).withKD(0.5)
.withKS(0.027275).withKV(2.5818).withKA(0.1055) .withKS(0.1).withKV(2.66).withKA(0)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
// When using closed-loop control, the drive motor uses the control // When using closed-loop control, the drive motor uses the control
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
private static final Slot0Configs driveGains = new Slot0Configs() private static final Slot0Configs driveGains = new Slot0Configs()
.withKP(63.167).withKI(0).withKD(0.54521) .withKP(0.1).withKI(0).withKD(0)
.withKS(0.18227).withKV(0.12483); .withKS(0).withKV(0.124);
// The closed-loop output type to use for the steer motors; // The closed-loop output type to use for the steer motors;
// This affects the PID/FF gains for the steer motors // This affects the PID/FF gains for the steer motors

81
src/main/java/frc/robot/commands/AprilTag3.java
View File

@@ -1,81 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.commands;
import static edu.wpi.first.units.Units.*;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.TunerConstants.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Limelight3;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class AprilTag3 extends Command {
private Limelight3 limelight3;
private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
private DoubleSupplier x;
private DoubleSupplier y;
/* Setting up bindings for necessary control of the swerve drive platform */
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
/** Creates a new AprilTag3G. */
public AprilTag3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
this.limelight3 = limelight3;
this.drivetrain = drivetrain;
this.x = x;
this.y = y;
addRequirements(limelight3,drivetrain);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
limelight3.Apriltag();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double a = limelight3.getX();
if(limelight3.getV() == true){
drivetrain.setControl(drive.
withRotationalRate(a/10).
withVelocityX(x.getAsDouble()).
withVelocityY(y.getAsDouble()));
System.out.println(a/10);
}
else{
drivetrain.setControl(drive.
withRotationalRate(0).
withVelocityX(0).
withVelocityY(0));
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drivetrain.setControl(drive.
withRotationalRate(0)
.withVelocityX(0)
.withVelocityY(0));
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

78
src/main/java/frc/robot/commands/AprilTag3G.java
View File

@@ -1,78 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.commands;
import static edu.wpi.first.units.Units.*;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.TunerConstants.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Limelight3G;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class AprilTag3G extends Command {
private Limelight3G limelight3g;
private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
private DoubleSupplier x;
private DoubleSupplier y;
/* Setting up bindings for necessary control of the swerve drive platform */
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
/** Creates a new AprilTag3G. */
/** Creates a new AprilTag3G. */
public AprilTag3G(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
this.limelight3g = limelight3g;
this.drivetrain = drivetrain;
this.x = x;
this.y = y;
addRequirements(limelight3g,drivetrain);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double a = limelight3g.getX();
if(limelight3g.getV() == true){
drivetrain.setControl(drive.
withRotationalRate(-a/5).
withVelocityX(x.getAsDouble()).
withVelocityY(y.getAsDouble()));
System.out.println(a/5);
}
else{
drivetrain.setControl(drive.
withRotationalRate(0).
withVelocityX(0).
withVelocityY(0));
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drivetrain.setControl(drive.
withRotationalRate(0).
withVelocityX(0).
withVelocityY(0));
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return limelight3g.getX()<1 && limelight3g.getX()>-1;
}
}

79
src/main/java/frc/robot/commands/Forme3.java
View File

@@ -1,79 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.commands;
import static edu.wpi.first.units.Units.*;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.TunerConstants.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Limelight3;
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
public class Forme3 extends Command {
private Limelight3 limelight3;
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
private DoubleSupplier x;
private DoubleSupplier y;
/* Setting up bindings for necessary control of the swerve drive platform */
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
/** Creates a new Forme3. */
public Forme3(Limelight3 limelight3,CommandSwerveDrivetrain drivetrain, DoubleSupplier x, DoubleSupplier y) {
this.limelight3 = limelight3;
this.drivetrain = drivetrain;
this.x = x;
this.y = y;
addRequirements(limelight3,drivetrain);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
limelight3.Forme();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double a = limelight3.getX();
if(limelight3.getV() == true){
drivetrain.setControl(drive.
withRotationalRate(a/10).
withVelocityX(x.getAsDouble()).
withVelocityY(y.getAsDouble()));
System.out.println(a/10);
}
else{
drivetrain.setControl(drive.
withRotationalRate(0).
withVelocityX(0).
withVelocityY(0));
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drivetrain.setControl(drive.
withRotationalRate(0).
withVelocityX(0).
withVelocityY(0));
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

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

@@ -3,15 +3,18 @@ package frc.robot.subsystems;
import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.*;
import java.util.function.Supplier; import java.util.function.Supplier;
import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N1;
@@ -27,7 +30,7 @@ import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain;
/** /**
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements * Class that extends the Phoenix 6 SwerveDrivetrain class and implements
* Subsystem so it can easily be used in command-based projeScts. * Subsystem so it can easily be used in command-based projects.
*/ */
public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
@@ -50,7 +53,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
new SysIdRoutine.Config( new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s) null, // Use default ramp rate (1 V/s)
@@ -124,9 +126,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
), ),
new PPHolonomicDriveController( new PPHolonomicDriveController(
// PID constants for translation // PID constants for translation
new PIDConstants(63.167, 0, 0.54521), new PIDConstants(10, 0, 0),
// PID constants for rotation // PID constants for rotation
new PIDConstants(7.9735, 0, 0.038499) new PIDConstants(7, 0, 0)
), ),
config, config,
// Assume the path needs to be flipped for Red vs Blue, this is normally the case // Assume the path needs to be flipped for Red vs Blue, this is normally the case
@@ -136,13 +138,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
} catch (Exception ex) { } catch (Exception ex) {
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
} }
PPHolonomicDriveController.overrideRotationFeedback(()->{
return 0;
});
} }
/* The SysId routine to test */ /* The SysId routine to test */
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineSteer; private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
/** /**
* Constructs a CTRE SwerveDrivetrain using the specified constants. * Constructs a CTRE SwerveDrivetrain using the specified constants.

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

@@ -1,39 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.LimelightHelpers;
public class Limelight3 extends SubsystemBase {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry pipeline = table.getEntry("pipeline");
/** Creates a new Limelight3. */
public Limelight3() {
for(int port = 5800; port <=5807; port++){
PortForwarder.add(port, "limelight.local", port);
}
}
public double getX(){
return LimelightHelpers.getTX("limelight-balon");
}
public boolean getV(){
return LimelightHelpers.getTV("limelight-balon");
}
public void Apriltag(){
pipeline.setNumber(1);
}
public void Forme(){
pipeline.setNumber(0);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}

28
src/main/java/frc/robot/subsystems/Limelight3G.java
View File

@@ -1,28 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot.subsystems;
import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.LimelightHelpers;
public class Limelight3G extends SubsystemBase {
/** Creates a new Limelight3. */
public Limelight3G() {
for(int port = 5800; port <=5807; port++){
PortForwarder.add(port, "limelight.local", port);
}
}
public double getX(){
return LimelightHelpers.getTX("limelight-tag");
}
public boolean getV(){
return LimelightHelpers.getTV("limelight-tag");
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}

8
vendordeps/PathplannerLib-2025.2.3.json → vendordeps/PathplannerLib-2025.2.2.json
View File

@@ -1,7 +1,7 @@
{ {
"fileName": "PathplannerLib-2025.2.3.json", "fileName": "PathplannerLib-2025.2.2.json",
"name": "PathplannerLib", "name": "PathplannerLib",
"version": "2025.2.3", "version": "2025.2.2",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2025", "frcYear": "2025",
"mavenUrls": [ "mavenUrls": [
@@ -12,7 +12,7 @@
{ {
"groupId": "com.pathplanner.lib", "groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java", "artifactId": "PathplannerLib-java",
"version": "2025.2.3" "version": "2025.2.2"
} }
], ],
"jniDependencies": [], "jniDependencies": [],
@@ -20,7 +20,7 @@
{ {
"groupId": "com.pathplanner.lib", "groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp", "artifactId": "PathplannerLib-cpp",
"version": "2025.2.3", "version": "2025.2.2",
"libName": "PathplannerLib", "libName": "PathplannerLib",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": false, "sharedLibrary": false,

58
vendordeps/Phoenix6-25.2.2.json → vendordeps/Phoenix6-25.2.1.json
View File

@@ -1,7 +1,7 @@
{ {
"fileName": "Phoenix6-25.2.2.json", "fileName": "Phoenix6-25.2.1.json",
"name": "CTRE-Phoenix (v6)", "name": "CTRE-Phoenix (v6)",
"version": "25.2.2", "version": "25.2.1",
"frcYear": "2025", "frcYear": "2025",
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
"mavenUrls": [ "mavenUrls": [
@@ -19,14 +19,14 @@
{ {
"groupId": "com.ctre.phoenix6", "groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-java", "artifactId": "wpiapi-java",
"version": "25.2.2" "version": "25.2.1"
} }
], ],
"jniDependencies": [ "jniDependencies": [
{ {
"groupId": "com.ctre.phoenix6", "groupId": "com.ctre.phoenix6",
"artifactId": "api-cpp", "artifactId": "api-cpp",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -40,7 +40,7 @@
{ {
"groupId": "com.ctre.phoenix6", "groupId": "com.ctre.phoenix6",
"artifactId": "tools", "artifactId": "tools",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -54,7 +54,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "api-cpp-sim", "artifactId": "api-cpp-sim",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -68,7 +68,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim", "artifactId": "tools-sim",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -82,7 +82,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX", "artifactId": "simTalonSRX",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -96,7 +96,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX", "artifactId": "simVictorSPX",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -110,7 +110,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU", "artifactId": "simPigeonIMU",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -124,7 +124,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder", "artifactId": "simCANCoder",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -138,7 +138,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX", "artifactId": "simProTalonFX",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -152,7 +152,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS", "artifactId": "simProTalonFXS",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -166,7 +166,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder", "artifactId": "simProCANcoder",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -180,7 +180,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2", "artifactId": "simProPigeon2",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -194,7 +194,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange", "artifactId": "simProCANrange",
"version": "25.2.2", "version": "25.2.1",
"isJar": false, "isJar": false,
"skipInvalidPlatforms": true, "skipInvalidPlatforms": true,
"validPlatforms": [ "validPlatforms": [
@@ -210,7 +210,7 @@
{ {
"groupId": "com.ctre.phoenix6", "groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-cpp", "artifactId": "wpiapi-cpp",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_Phoenix6_WPI", "libName": "CTRE_Phoenix6_WPI",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -226,7 +226,7 @@
{ {
"groupId": "com.ctre.phoenix6", "groupId": "com.ctre.phoenix6",
"artifactId": "tools", "artifactId": "tools",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_PhoenixTools", "libName": "CTRE_PhoenixTools",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -242,7 +242,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "wpiapi-cpp-sim", "artifactId": "wpiapi-cpp-sim",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_Phoenix6_WPISim", "libName": "CTRE_Phoenix6_WPISim",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -258,7 +258,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim", "artifactId": "tools-sim",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_PhoenixTools_Sim", "libName": "CTRE_PhoenixTools_Sim",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -274,7 +274,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX", "artifactId": "simTalonSRX",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_SimTalonSRX", "libName": "CTRE_SimTalonSRX",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -290,7 +290,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX", "artifactId": "simVictorSPX",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_SimVictorSPX", "libName": "CTRE_SimVictorSPX",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -306,7 +306,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU", "artifactId": "simPigeonIMU",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_SimPigeonIMU", "libName": "CTRE_SimPigeonIMU",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -322,7 +322,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder", "artifactId": "simCANCoder",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_SimCANCoder", "libName": "CTRE_SimCANCoder",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -338,7 +338,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX", "artifactId": "simProTalonFX",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_SimProTalonFX", "libName": "CTRE_SimProTalonFX",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -354,7 +354,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS", "artifactId": "simProTalonFXS",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_SimProTalonFXS", "libName": "CTRE_SimProTalonFXS",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -370,7 +370,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder", "artifactId": "simProCANcoder",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_SimProCANcoder", "libName": "CTRE_SimProCANcoder",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -386,7 +386,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2", "artifactId": "simProPigeon2",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_SimProPigeon2", "libName": "CTRE_SimProPigeon2",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,
@@ -402,7 +402,7 @@
{ {
"groupId": "com.ctre.phoenix6.sim", "groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange", "artifactId": "simProCANrange",
"version": "25.2.2", "version": "25.2.1",
"libName": "CTRE_SimProCANrange", "libName": "CTRE_SimProCANrange",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": true, "sharedLibrary": true,