sdesharnais/Rebuilt-2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: sdesharnais:bdffb48ece610aa1d80056b147841c80fae0d49c
Branches Tags
sdesharnais:main sdesharnais:coaching
..
compare: sdesharnais:coaching
Branches Tags
sdesharnais:coaching sdesharnais:main

1 Commits
bdffb48ece ... coaching

Author SHA1 Message Date
Olivier Demers
4ecaef3d1d big stuff 2026-04-22 10:21:28 -04:00
45 changed files with 972 additions and 2146 deletions
Expand all files Collapse all files

2
build.gradle
View File

@@ -33,7 +33,7 @@ deploy {
frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
files = project.fileTree('src/main/deploy') files = project.fileTree('src/main/deploy')
directory = '/home/lvuser/deploy' directory = '/home/lvuser/deploy'
deleteOldFiles = false // Change to true to delete files on roboRIO that no deleteOldFiles = true // Change to true to delete files on roboRIO that no
// longer exist in deploy directory of this project // longer exist in deploy directory of this project
} }
} }

99
src/main/deploy/pathplanner/autos/Copy of DepotTirerReservoir.auto
View File

@@ -1,99 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 3.0
}
},
{
"type": "path",
"data": {
"pathName": "Depot"
}
},
{
"type": "named",
"data": {
"name": "Aspirer"
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "Tir"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Lancer"
}
},
{
"type": "named",
"data": {
"name": "Aspirer"
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "GrimperReservoir"
}
},
{
"type": "named",
"data": {
"name": "GrimperReservoir"
}
},
{
"type": "named",
"data": {
"name": "MonterGrimpeur"
}
},
{
"type": "named",
"data": {
"name": "gauche"
}
},
{
"type": "named",
"data": {
"name": "DescendreGrimpeur"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

33
src/main/deploy/pathplanner/autos/DepotTirerMur.auto
View File

@@ -14,12 +14,6 @@
"type": "deadline", "type": "deadline",
"data": { "data": {
"commands": [ "commands": [
{
"type": "wait",
"data": {
"waitTime": 3.0
}
},
{ {
"type": "path", "type": "path",
"data": { "data": {
@@ -48,22 +42,15 @@
} }
}, },
{ {
"type": "deadline", "type": "named",
"data": { "data": {
"commands": [ "name": "Lancer"
{ }
"type": "named", },
"data": { {
"name": "Lancer" "type": "named",
} "data": {
}, "name": "TournerA180"
{
"type": "named",
"data": {
"name": "Aspirer"
}
}
]
} }
}, },
{ {
@@ -85,9 +72,9 @@
} }
}, },
{ {
"type": "named", "type": "path",
"data": { "data": {
"name": "droite" "pathName": "MonterMur"
} }
}, },
{ {

33
src/main/deploy/pathplanner/autos/DepotTirerReservoir.auto
View File

@@ -14,12 +14,6 @@
"type": "deadline", "type": "deadline",
"data": { "data": {
"commands": [ "commands": [
{
"type": "wait",
"data": {
"waitTime": 3.0
}
},
{ {
"type": "path", "type": "path",
"data": { "data": {
@@ -48,22 +42,15 @@
} }
}, },
{ {
"type": "deadline", "type": "named",
"data": { "data": {
"commands": [ "name": "Lancer"
{ }
"type": "named", },
"data": { {
"name": "Lancer" "type": "named",
} "data": {
}, "name": "TournerAZero"
{
"type": "named",
"data": {
"name": "Aspirer"
}
}
]
} }
}, },
{ {
@@ -85,9 +72,9 @@
} }
}, },
{ {
"type": "named", "type": "path",
"data": { "data": {
"name": "gauche" "pathName": "MonterReservoir"
} }
}, },
{ {

16
src/main/deploy/pathplanner/paths/MonterReservoir.path
View File

@@ -3,25 +3,25 @@
"waypoints": [ "waypoints": [
{ {
"anchor": { "anchor": {
"x": 1.0, "x": 1.0502282453637657,
"y": 2.6 "y": 2.4758915834522113
}, },
"prevControl": null, "prevControl": null,
"nextControl": { "nextControl": {
"x": 0.9906601936299568, "x": 1.0408884389937225,
"y": 2.866268181798411 "y": 2.742159765250622
}, },
"isLocked": false, "isLocked": false,
"linkedName": null "linkedName": null
}, },
{ {
"anchor": { "anchor": {
"x": 1.0, "x": 1.0502282453637657,
"y": 2.9 "y": 2.7734807417974334
}, },
"prevControl": { "prevControl": {
"x": 1.00452638848785, "x": 1.0547546338516156,
"y": 2.6500409797441646 "y": 2.523521721541598
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,

67
src/main/java/frc/robot/Commandes.java Normal file
View File

@@ -0,0 +1,67 @@
package frc.robot;
import java.util.function.Supplier;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.subsystems.Balayeuse;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Demeleur;
import frc.robot.subsystems.Grimpeur;
import frc.robot.subsystems.Lanceur;
import frc.robot.subsystems.PivotBalayeuse;
public class Commandes {
private final CommandSwerveDrivetrain drivetrain;
private final Lanceur lanceur;
private final Demeleur demeleur;
private final Balayeuse balayeuse;
private final Grimpeur grimpeur;
private final PivotBalayeuse pivotBalayeuse;
public Commandes(CommandSwerveDrivetrain drivetrain,
Lanceur lanceur,
Demeleur demeleur,
Balayeuse balayeuse,
Grimpeur grimpeur,
PivotBalayeuse pivotBalayeuse) {
this.drivetrain = drivetrain;
this.lanceur = lanceur;
this.demeleur = demeleur;
this.balayeuse = balayeuse;
this.grimpeur = grimpeur;
this.pivotBalayeuse = pivotBalayeuse;
}
public double vitesseLanceur() {
return Coordinates.diffFromHub(drivetrain.getState().Pose.getTranslation()).getNorm() * 410.57 + 2250;
}
public Command viserLancer() {
return viserLancer(() -> 0.0, () -> 0.0);
}
public Command viserLancer(Supplier<Double> vitesseX, Supplier<Double> vitesseY) {
return Commands.parallel(
drivetrain.viserReservoir(vitesseX, vitesseY),
this.lancer());
}
public Command lancer() {
return lancer(this::vitesseLanceur);
}
public Command lancer(Supplier<Double> vitesse) {
return Commands.parallel(
lanceur.lancer(vitesse),
Commands.waitUntil(lanceur::setPointAtteint).andThen(demeleur.run()));
}
public Command lancerAspirer() {
return Commands.parallel(
this.lancer(),
balayeuse.aspirer());
}
}

41
src/main/java/frc/robot/Coordinates.java Normal file
View File

@@ -0,0 +1,41 @@
package frc.robot;
import static edu.wpi.first.units.Units.Meters;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
public final class Coordinates {
public static final Translation2d kRedHub = new Translation2d(Meters.of(16.5 - 4.6), Meters.of(4.03));
public static final Translation2d kBlueHub = new Translation2d(Meters.of(4.6), Meters.of(4.03));
public static final Translation2d kGrimpeDepotRouge = new Translation2d(Meters.of(16.5 - 1.1), Meters.of(4.9));
public static final Translation2d kGrimpeMurRouge = new Translation2d(Meters.of(16.5 - 1.1), Meters.of(3.7));
public static final Translation2d kGrimpeDepotBleu = new Translation2d(Meters.of(1.1), Meters.of(3.1));
public static final Translation2d kGrimpeMurBleu = new Translation2d(Meters.of(1.1), Meters.of(4.4));
public static Translation2d diffFromHub(Translation2d robotTranslation) {
if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue) {
return kBlueHub.minus(robotTranslation);
} else {
return kRedHub.minus(robotTranslation);
}
}
public static Translation2d diffFromGrimpeMur(Translation2d robotTranslation) {
if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue) {
return kGrimpeMurBleu.minus(robotTranslation);
} else {
return kGrimpeMurRouge.minus(robotTranslation);
}
}
public static Translation2d diffFromGrimpeDepot(Translation2d robotTranslation) {
if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue) {
return kGrimpeDepotBleu.minus(robotTranslation);
} else {
return kGrimpeDepotRouge.minus(robotTranslation);
}
}
}

75
src/main/java/frc/robot/LimeLight3.java Normal file
View File

@@ -0,0 +1,75 @@
// 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;
import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class LimeLight3 {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
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[] getBotPoseBlue() {
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
public double[] getBotPoseRed() {
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
public double getTx() {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry tx = table.getEntry("tx");
return tx.getDouble(0.0);
}
public double getTId() {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry tid = table.getEntry("tid");
return tid.getDouble(0.0);
}
public double getTA() {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry ta = table.getEntry("ta");
return ta.getDouble(0.0);
}
public boolean getV() {
return LimelightHelpers.getTV("limelight-balaie");
}
public void AprilTag() {
pipeline.setNumber(0);
}
public void Forme() {
pipeline.setNumber(1);
}
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;
}
}
}

74
src/main/java/frc/robot/Limelight3G.java Normal file
View File

@@ -0,0 +1,74 @@
// 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;
import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class Limelight3G {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry pipeline = table.getEntry("pipeline");
/** Creates a new LimeLight3. */
public Limelight3G() {
for (int port = 5800; port <= 5807; port++) {
PortForwarder.add(port, "limelight.local", port);
}
}
public double[] getBotPoseBlue() {
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
public double[] getBotPoseRed() {
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
public double getTx() {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry tx = table.getEntry("tx");
return tx.getDouble(0.0);
}
public double getTId() {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry tid = table.getEntry("tid");
return tid.getDouble(0.0);
}
public double getTA() {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry ta = table.getEntry("ta");
return ta.getDouble(0.0);
}
public boolean getV() {
return LimelightHelpers.getTV("limelight-tag");
}
public double Calcule(double x1, double x2, double y1, double y2, double angle) {
if (x1 > x2) {
if (y1 > y2) {
return Math.atan(90 - ((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle;
} else {
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) + 90 - angle;
}
} else {
if (y1 > y2) {
return Math.atan(90 - ((x2 - x1) / (y2 - y1))) * (180 / Math.PI) + 270 - angle;
} else {
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) + 180 - angle;
}
}
}
}

36
src/main/java/frc/robot/Robot.java
View File

@@ -4,6 +4,7 @@
package frc.robot; package frc.robot;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.util.Units; import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
@@ -27,19 +28,28 @@ public class Robot extends TimedRobot {
LimelightHelpers.SetRobotOrientation("limelight_tag", headingDeg, 0, 0, 0, 0, 0); LimelightHelpers.SetRobotOrientation("limelight_tag", headingDeg, 0, 0, 0, 0, 0);
var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight_tag"); var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight_tag");
var stdDevs = LimelightHelpers.getLimelightDoubleArrayEntry("limelight_tag", "stddevs");
var limelightStdDevs = VecBuilder.fill(.5, .5, 9999999);
if (stdDevs.exists()) {
limelightStdDevs = VecBuilder.fill(stdDevs.get()[6], stdDevs.get()[7], stdDevs.get()[12]);
}
if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) { if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds); m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds,
limelightStdDevs);
} }
} }
@Override @Override
public void disabledInit() {} public void disabledInit() {
}
@Override @Override
public void disabledPeriodic() {} public void disabledPeriodic() {
}
@Override @Override
public void disabledExit() {} public void disabledExit() {
}
@Override @Override
public void autonomousInit() { public void autonomousInit() {
@@ -51,10 +61,12 @@ public class Robot extends TimedRobot {
} }
@Override @Override
public void autonomousPeriodic() {} public void autonomousPeriodic() {
}
@Override @Override
public void autonomousExit() {} public void autonomousExit() {
}
@Override @Override
public void teleopInit() { public void teleopInit() {
@@ -64,10 +76,12 @@ public class Robot extends TimedRobot {
} }
@Override @Override
public void teleopPeriodic() {} public void teleopPeriodic() {
}
@Override @Override
public void teleopExit() {} public void teleopExit() {
}
@Override @Override
public void testInit() { public void testInit() {
@@ -75,8 +89,10 @@ public class Robot extends TimedRobot {
} }
@Override @Override
public void testPeriodic() {} public void testPeriodic() {
}
@Override @Override
public void testExit() {} public void testExit() {
}
} }

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

@@ -4,7 +4,9 @@
package frc.robot; package frc.robot;
import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveRequest;
@@ -12,31 +14,11 @@ import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.MathUtil;
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.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.commands.Aspirer;
import frc.robot.commands.DescendreBalyeuse;
import frc.robot.commands.DescendreGrimpeur;
import frc.robot.commands.Inverser;
import frc.robot.commands.Lancer;
import frc.robot.commands.LancerAspirer;
import frc.robot.commands.LancerBaseVitesse;
import frc.robot.commands.Limelighter;
import frc.robot.commands.ModeOposer;
import frc.robot.commands.ModeOposerBalayeuse;
import frc.robot.commands.ModeOposerDemeleur;
import frc.robot.commands.MonterBalyeuse;
import frc.robot.commands.MonterGrimpeur;
import frc.robot.commands.ModeAuto.AspirerAuto;
import frc.robot.commands.ModeAuto.BougerDroiteAuto;
import frc.robot.commands.ModeAuto.BougerGaucheAuto;
import frc.robot.commands.ModeAuto.GrimperMur;
import frc.robot.commands.ModeAuto.GrimperReservoir;
import frc.robot.commands.ModeAuto.LancerAuto;
import frc.robot.commands.ModeAuto.LimelighterAuto;
import frc.robot.commands.ModeAuto.RetourMilieuDroite; import frc.robot.commands.ModeAuto.RetourMilieuDroite;
import frc.robot.commands.ModeAuto.RetourMilieuGauche; import frc.robot.commands.ModeAuto.RetourMilieuGauche;
import frc.robot.commands.ModeAuto.TournerVersMur; import frc.robot.commands.ModeAuto.TournerVersMur;
@@ -44,93 +26,89 @@ import frc.robot.commands.ModeAuto.TournerVersReservoir;
import frc.robot.generated.TunerConstants; import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.Balayeuse; import frc.robot.subsystems.Balayeuse;
import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Demeleur;
import frc.robot.subsystems.Grimpeur; import frc.robot.subsystems.Grimpeur;
import frc.robot.subsystems.LEDSubsystem;
import frc.robot.subsystems.Lanceur; import frc.robot.subsystems.Lanceur;
import frc.robot.subsystems.LimeLight3; import frc.robot.subsystems.Led;
import frc.robot.subsystems.Limelight3G; import frc.robot.subsystems.PivotBalayeuse;
public class RobotContainer { public class RobotContainer {
private final SendableChooser<Command> autoChooser; private final SendableChooser<Command> autoChooser;
Balayeuse balayeuse = new Balayeuse(); public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
Grimpeur grimpeur = new Grimpeur(); private final Balayeuse balayeuse = new Balayeuse();
Lanceur lanceur = new Lanceur(); private final PivotBalayeuse pivotBalayeuse = new PivotBalayeuse();
LimeLight3 limeLight3 = new LimeLight3(); private final Demeleur demeleur = new Demeleur();
Limelight3G limeLight3G = new Limelight3G(); private final Grimpeur grimpeur = new Grimpeur();
LEDSubsystem ledSubsystem = new LEDSubsystem(); private final Lanceur lanceur = new Lanceur();
CommandXboxController manette = new CommandXboxController(0); private final Commandes commandes = new Commandes(drivetrain, lanceur, demeleur, balayeuse, grimpeur, pivotBalayeuse);
CommandXboxController manette1 = new CommandXboxController(1); private final LimeLight3 limeLight3 = new LimeLight3();
private final Limelight3G limeLight3G = new Limelight3G();
private final Led led = new Led();
private final CommandXboxController manette = new CommandXboxController(0);
private final CommandXboxController manette1 = new CommandXboxController(1);
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed 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 private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max
// angular velocity
/* Setting up bindings for necessary control of the swerve drive platform */ /* Setting up bindings for necessary control of the swerve drive platform */
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
private final Telemetry logger = new Telemetry(MaxSpeed);
public RobotContainer() { public RobotContainer() {
NamedCommands.registerCommand("droite", new BougerDroiteAuto(drivetrain)); NamedCommands.registerCommand("GrimperMur", drivetrain.allerGrimpeMur());
NamedCommands.registerCommand("gauche", new BougerGaucheAuto(drivetrain)); NamedCommands.registerCommand("GrimperReservoir", drivetrain.allerGrimpeDepot());
NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain)); NamedCommands.registerCommand("Lancer", commandes.lancer());
NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain)); NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain, limeLight3G));
NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain,limeLight3G));
NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche(drivetrain, limeLight3G)); NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche(drivetrain, limeLight3G));
NamedCommands.registerCommand("Limelighter", new LimelighterAuto(limeLight3G,drivetrain)); NamedCommands.registerCommand("Limelighter", commandes.viserLancer());
NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse)); NamedCommands.registerCommand("DescendreBalayeuse", pivotBalayeuse.descendre());
NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse)); NamedCommands.registerCommand("Aspirer", balayeuse.aspirer());
NamedCommands.registerCommand("TournerA180", new TournerVersReservoir(drivetrain)); NamedCommands.registerCommand("TournerA180", new TournerVersReservoir(drivetrain));
NamedCommands.registerCommand("TournerAZero", new TournerVersMur(drivetrain)); NamedCommands.registerCommand("TournerAZero", new TournerVersMur(drivetrain));
NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur)); NamedCommands.registerCommand("MonterGrimpeur", grimpeur.sortir());
NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur)); NamedCommands.registerCommand("DescendreGrimpeur", grimpeur.rentrer());
autoChooser = AutoBuilder.buildAutoChooser(); autoChooser = AutoBuilder.buildAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser); SmartDashboard.putData("Auto Chooser", autoChooser);
CameraServer.startAutomaticCapture(); CameraServer.startAutomaticCapture();
configureBindings(); configureBindings();
} }
private void configureBindings() { private void configureBindings() {
ledSubsystem.setDefaultCommand(ledSubsystem.updateLEDs());
// Manette 0 (pilote)
drivetrain.setDefaultCommand( drivetrain.setDefaultCommand(
drivetrain.applyRequest(() -> drivetrain.applyRequest(() -> drive
drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed*0.7, 0.05)) .withVelocityY(-manette.getLeftX() * MaxSpeed * 0.7)
.withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*Math.abs(-manette.getLeftY())*MaxSpeed*0.7, 0.05)) .withVelocityX(-manette.getLeftY() * MaxSpeed * 0.7)
.withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05)) .withRotationalRate(-manette.getRightX() * MaxAngularRate)));
) manette.rightBumper().whileTrue(balayeuse.aspirer());
); manette.povUp().whileTrue(grimpeur.sortir());
//manette 1 manette.povDown().whileTrue(grimpeur.rentrer());
// manette1.povUp().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
// manette1.povDown().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
// manette1.povRight().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
// manette1.povLeft().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
manette.povUp().whileTrue(new MonterGrimpeur(grimpeur));
manette.povDown().whileTrue(new DescendreGrimpeur(grimpeur));
manette.rightTrigger().whileTrue(new Lancer(lanceur, limeLight3G));
manette.leftTrigger().whileTrue(new Limelighter(limeLight3G, drivetrain));
manette.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
manette.rightBumper().whileTrue(new Aspirer(balayeuse));
manette.b().whileTrue(new GrimperReservoir(limeLight3G, drivetrain));
manette.x().whileTrue(new GrimperMur(limeLight3, drivetrain));
manette.rightTrigger().whileTrue(commandes.lancer());
manette.leftTrigger().whileTrue(
commandes.viserLancer(() -> -manette.getLeftY() * MaxSpeed * 0.7, () -> -manette.getLeftX() * MaxSpeed * 0.7));
manette.leftBumper().whileTrue(pivotBalayeuse.descendre());
manette.b().whileTrue(drivetrain.allerGrimpeDepot());
manette.x().whileTrue(drivetrain.allerGrimpeMur());
//manette 2 // Manette 1 (co-pilote)
manette1.rightTrigger().whileTrue(new Aspirer(balayeuse)); manette1.rightTrigger().whileTrue(balayeuse.aspirer());
manette1.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));
manette1.leftTrigger().whileTrue(new LancerBaseVitesse(lanceur)); manette1.rightBumper().whileTrue(pivotBalayeuse.monter());
manette1.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse)); manette1.leftTrigger().whileTrue(commandes.lancer(() -> lanceur.ntBasseVitesse()));
manette1.x().whileTrue(new LancerAspirer(lanceur, balayeuse, limeLight3G)); manette1.leftBumper().whileTrue(pivotBalayeuse.descendre());
manette1.b().whileTrue(new ModeOposer(lanceur)); manette1.x().whileTrue(commandes.lancerAspirer());
manette1.a().whileTrue(new ModeOposerDemeleur(lanceur)); manette1.b().whileTrue(Commands.parallel(lanceur.inverse(), demeleur.inverse()));
manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse)); manette1.a().whileTrue(demeleur.inverse());
manette1.povRight().whileTrue(new BougerDroiteAuto(drivetrain)); manette1.y().whileTrue(balayeuse.ejecter());
manette1.povLeft().whileTrue(new BougerGaucheAuto(drivetrain));
manette1.start().whileTrue(new Inverser(drivetrain)); drivetrain.registerTelemetry(logger::telemeterize);
} }
public Command getAutonomousCommand() { public Command getAutonomousCommand() {

44
src/main/java/frc/robot/commands/Aspirer.java
View File

@@ -1,44 +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 edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Balayeuse;
/* 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 Aspirer extends Command {
private Balayeuse balayeuse;
/** Creates a new Aspirer. */
public Aspirer(Balayeuse balayeuse) {
this.balayeuse = balayeuse;
addRequirements(balayeuse);
// 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() {
balayeuse.BalayerEnbas(-0.5);
balayeuse.BalayerPadle(-0.2);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.BalayerEnbas(0);
balayeuse.BalayerPadle(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

46
src/main/java/frc/robot/commands/DescendreBalyeuse.java
View File

@@ -1,46 +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 edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Balayeuse;
/* 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 DescendreBalyeuse extends Command {
private Balayeuse balayeuse;
/** Creates a new Descendre. */
public DescendreBalyeuse(Balayeuse balayeuse) {
this.balayeuse = balayeuse;
addRequirements(balayeuse);
// 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() {
if(Math.abs(balayeuse.Distance()) < balayeuse.EncodeurBalayeuse()){
balayeuse.Pivoter(-0.5);
}
else{
balayeuse.Pivoter(0);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.Pivoter(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return Math.abs(balayeuse.Distance()) > balayeuse.EncodeurBalayeuse();
}
}

46
src/main/java/frc/robot/commands/DescendreGrimpeur.java
View File

@@ -1,46 +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 edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Grimpeur;
/* 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 DescendreGrimpeur extends Command {
private Grimpeur grimpeur;
/** Creates a new DescendreGrimpeur. */
public DescendreGrimpeur(Grimpeur grimpeur) {
this.grimpeur = grimpeur;
// 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() {
if(!grimpeur.Limit()){
grimpeur.Grimper(-0.4);
}
else{
grimpeur.Reset();
grimpeur.Grimper(0);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
grimpeur.Grimper(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

41
src/main/java/frc/robot/commands/Inverser.java
View File

@@ -1,41 +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 edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.CommandSwerveDrivetrain;
/* 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 Inverser extends Command {
CommandSwerveDrivetrain drivetrain;
boolean inverse = false;
/** Creates a new Inverser. */
public Inverser(CommandSwerveDrivetrain drivetrain) {
this.drivetrain = drivetrain;
addRequirements(drivetrain);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

94
src/main/java/frc/robot/commands/Lancer.java
View File

@@ -1,94 +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 java.util.Optional;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur;
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 Lancer extends Command {
private Lanceur lanceur;
private PIDController pidController;
private Limelight3G limeLight3G;
private Timer timer;
double vitesse = 0.5;
double botx = 0;
double boty = 0;
Optional<Alliance> alliance = DriverStation.getAlliance();
/** Creates a new Lancer. */
public Lancer(Lanceur lanceur, Limelight3G limeLight3G) {
this.lanceur = lanceur;
this.timer = new Timer();
this.limeLight3G = limeLight3G;
addRequirements(lanceur, limeLight3G);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
if(limeLight3G.getV()){}
pidController = new PIDController(0.0007, 0,0, 0.001);
timer.reset();
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double[] BotPose = new double[6];
if(limeLight3G.getV()){
if(!alliance.isPresent()){
return;
}
if(alliance.get() == Alliance.Blue){
BotPose = limeLight3G.getBotPoseBlue();
}
else{
BotPose = limeLight3G.getBotPoseRed();
}
botx = BotPose[0];
boty = BotPose[1];
vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)))) + 2250;
}
if(limeLight3G.getV()){
double output = pidController.calculate(lanceur.Vitesse(),vitesse);
lanceur.Lancer(output);
System.out.println(output);
if(lanceur.Vitesse() >= vitesse-800){
timer.start();
if(timer.get() >1){
lanceur.Demeler(1);
}
}
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
lanceur.Demeler(0);
lanceur.Lancer(0);
timer.reset();
timer.stop();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

23
src/main/java/frc/robot/commands/LancerAspirer.java
View File

@@ -1,23 +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 edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import frc.robot.subsystems.Balayeuse;
import frc.robot.subsystems.Lanceur;
import frc.robot.subsystems.Limelight3G;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class LancerAspirer extends ParallelCommandGroup {
/** Creates a new LacerAspirer. */
public LancerAspirer(Lanceur lanceur, Balayeuse balayeuse, Limelight3G limeLight3G) {
addCommands(new Lancer(lanceur, limeLight3G), new Aspirer(balayeuse));
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
}
}

63
src/main/java/frc/robot/commands/LancerBaseVitesse.java
View File

@@ -1,63 +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 edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur;
/* 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 LancerBaseVitesse extends Command {
private Lanceur lanceur;
private PIDController pidController;
private Timer timer;
double tempsDebut = 0;
/** Creates a new Lancer. */
public LancerBaseVitesse(Lanceur lanceur) {
this.lanceur = lanceur;
this.timer = new Timer();
addRequirements(lanceur);
//this.temp = 0;
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
pidController = new PIDController(0.0007, 0,0, 0.001);
timer.reset();
timer.start();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
System.out.println(DriverStation.getMatchTime());
double vitesse = lanceur.vitesseDemander();
double output = pidController.calculate(lanceur.Vitesse(),vitesse);
lanceur.Lancer(output);
if(timer.get() > 1){
lanceur.Demeler(1);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
lanceur.Demeler(0);
lanceur.Lancer(0);
timer.reset();
timer.stop();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

131
src/main/java/frc/robot/commands/Limelighter.java
View File

@@ -1,131 +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 com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Limelight3G;
import static edu.wpi.first.units.Units.*;
import java.util.Optional;
/* 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 Limelighter extends Command {
Limelight3G limelight3g;
CommandSwerveDrivetrain drivetrain;
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
double botx;
double boty;
double angle;
double calcul;
double x;
Optional<Alliance> alliance = DriverStation.getAlliance();
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
/** Creates a new Limelighter. */
public Limelighter(Limelight3G limelight3g, CommandSwerveDrivetrain drivetrain) {
this.limelight3g = limelight3g;
this.drivetrain = drivetrain;
addRequirements(drivetrain, limelight3g);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) {
return;
}
if (alliance.get() == Alliance.Blue) {
x = 4.6;
BotPose = limelight3g.getBotPoseBlue();
}
else {
x = 11.915394;
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[0];
boty = BotPose[1];
angle = BotPose[5];
calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(calcul > -5 && calcul < 5){
drivetrain.setControl(
drive.withRotationalRate(0));
}
else if(calcul > 5){
drivetrain.setControl(
drive.withRotationalRate(2));
}
else if(calcul < -5){
drivetrain.setControl(drive.withRotationalRate(-2));
}
// botx = BotPose[1];
// boty = BotPose[0];
// angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
// calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
// if(calcul < -5 && calcul > -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else if(calcul > 5 && calcul < 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul < -5){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul <= -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else{
// drivetrain.setControl(
// drive.withRotationalRate(0));
// }
// drivetrain.setControl(
// drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
// System.out.println(angle);
// if (calcul < 0.2 && calcul > -0.2) {
// drivetrain.setControl(drive.withRotationalRate(0));
// }
}
else{
drivetrain.setControl(drive.withRotationalRate(0));
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drivetrain.setControl(drive.withRotationalRate(0));
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

43
src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java
View File

@@ -1,43 +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.ModeAuto;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Balayeuse;
/* 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 AspirerAuto extends Command {
Balayeuse balayeuse;
/** Creates a new AspirerAuto. */
public AspirerAuto(Balayeuse balayeuse) {
this.balayeuse = balayeuse;
addRequirements(balayeuse);
// 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() {
balayeuse.BalayerEnbas(-0.5);
balayeuse.BalayerPadle(-0.2);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.BalayerEnbas(0);
balayeuse.BalayerPadle(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

80
src/main/java/frc/robot/commands/ModeAuto/BougerDroiteAuto.java
View File

@@ -1,80 +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.ModeAuto;
import static edu.wpi.first.units.Units.*;
import java.util.Optional;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
/* 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 BougerDroiteAuto extends Command {
CommandSwerveDrivetrain drivetrain;
Timer timer = new Timer();
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
Optional<Alliance> alliance;
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
/** Creates a new AvanceAuto. */
public BougerDroiteAuto(CommandSwerveDrivetrain drivetrain) {
this.drivetrain = drivetrain;
addRequirements(drivetrain);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
alliance = DriverStation.getAlliance();
timer.reset();
timer.start();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(alliance.get() == Alliance.Blue){
if(timer.get() < .75){
System.out.println("8765");
drivetrain.setControl(drive.withVelocityY(-1.5));
}
else{
drivetrain.setControl(drive.withVelocityY(0));
}
}
else{
if(timer.get() < 0.75){
drivetrain.setControl(drive.withVelocityY(1.5));
}
else{
drivetrain.setControl(drive.withVelocityY(0));
}
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drivetrain.setControl(drive.withVelocityY(0));
timer.stop();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return timer.get() > 1;
}
}

69
src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java
View File

@@ -1,69 +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.ModeAuto;
import static edu.wpi.first.units.Units.*;
import java.util.Optional;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
/* 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 BougerGaucheAuto extends Command {
CommandSwerveDrivetrain drivetrain;
Timer timer = new Timer();
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
Optional<Alliance> alliance;
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
/** Creates a new AvanceAuto. */
public BougerGaucheAuto(CommandSwerveDrivetrain drivetrain) {
this.drivetrain = drivetrain;
addRequirements(drivetrain);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
alliance = DriverStation.getAlliance();
timer.reset();
timer.start();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(timer.get() < 1.25){
drivetrain.setControl(drive.withVelocityY(1.5));
}
else{
drivetrain.setControl(drive.withVelocityY(0));
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drivetrain.setControl(drive.withVelocityY(0));
timer.stop();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return timer.get() > 1;
}
}

115
src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java
View File

@@ -1,115 +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.ModeAuto;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import java.util.Optional;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.LimeLight3;
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 GrimperMur extends Command {
LimeLight3 limeLight3;
CommandSwerveDrivetrain drivetrain;
double[] BotPose = new double[6];
double botx;
double boty;
double x;
double y;
double angle;
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
Optional<Alliance> alliance = DriverStation.getAlliance();
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
/** Creates a new GrimperMur. */
public GrimperMur(LimeLight3 limeLight3, CommandSwerveDrivetrain drivetrain) {
this.drivetrain = drivetrain;
this.limeLight3 = limeLight3;
addRequirements(limeLight3,drivetrain);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
// alliance = DriverStation.getAlliance();
// if(drivetrain.Equipe()){
// angle+=180;
// }
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(limeLight3.getV()){
BotPose = limeLight3.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble());
if(angle < 0){
angle = angle + 360;
}
if(alliance.get() == Alliance.Blue){
y = 5.4;
x = 1.11;
angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){
drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx));
}
else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI));
}
}
}
else{
x = 15.6;
y = 6.959326;
angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170 && drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190){
drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx));
}
else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(0.5*180/Math.PI));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(-0.5*180/Math.PI));
}
}
}
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05);
}
}

124
src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
View File

@@ -1,124 +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.ModeAuto;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import java.util.Optional;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.generated.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 GrimperReservoir extends Command {
Limelight3G limeLight3G;
CommandSwerveDrivetrain drivetrain;
double[] BotPose = new double[6];
double botx;
double boty;
double x;
double y;
double angle;
double pigeonAngle;
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
Optional<Alliance> alliance;
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
/** Creates a new GrimperMur. */
public GrimperReservoir(Limelight3G limeLight3G, CommandSwerveDrivetrain drivetrain) {
this.drivetrain = drivetrain;
this.limeLight3G = limeLight3G;
addRequirements(limeLight3G,drivetrain);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
alliance = DriverStation.getAlliance();
// if(drivetrain.Equipe()){
// drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180);
// }
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(limeLight3G.getV()){
BotPose = limeLight3G.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
if(angle < 0){
angle = angle + 360;
}
if(alliance.get() == Alliance.Red){
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
System.out.println(pigeonAngle);
y = 5.4;
x = 15.6;
angle = 180;
if(pigeonAngle< 190 && pigeonAngle> 170){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp((-(y-boty)*6.5), -2, 2)).withVelocityX(MathUtil.clamp((-(x-botx)*6.5),-2,2)));
}
else{
if(pigeonAngle>180){
drivetrain.setControl(drive.withRotationalRate(-1));
}
else{
drivetrain.setControl(drive.withRotationalRate(1));
}
}
}
else{
pigeonAngle = drivetrain.getPigeon2().getYaw().getValueAsDouble()+180;
System.out.println(pigeonAngle);
y = 2.6;
x = 1.11;
angle = 0;
if(pigeonAngle> 358 || pigeonAngle< 2){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5.5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5.5),-2,2)).withRotationalRate(0));
}
else{
if(pigeonAngle>0 && pigeonAngle<180){
drivetrain.setControl(drive.withRotationalRate(-1));
System.out.println("x");
}
else if(pigeonAngle>180){
System.out.println("e");
drivetrain.setControl(drive.withRotationalRate(1));
}
}
}
}
else{
drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (y-boty < 0.1 && y-boty >-0.1) && (x-botx < 0.1 && x-botx > -0.1);
}
}

93
src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java
View File

@@ -1,93 +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.ModeAuto;
import java.util.Optional;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur;
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 LancerAuto extends Command {
Lanceur lanceur;
Timer timer;
private PIDController pidController;
Limelight3G limelight3g;
double botx = 0;
double boty = 0;
Optional<Alliance> alliance = DriverStation.getAlliance();
/** Creates a new LancerAuto. */
public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) {
this.lanceur = lanceur;
timer = new Timer();
this.limelight3g = limelight3g;
addRequirements(lanceur, limelight3g);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
pidController = new PIDController(0.0007, 0, 0, 0.001);
timer.reset();
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double[] BotPose = new double[6];
if (limelight3g.getV()) {
if (!alliance.isPresent()) {
return;
}
if (alliance.get() == Alliance.Blue) {
BotPose = limelight3g.getBotPoseBlue();
} else {
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[0];
boty = BotPose[1];
}
double vitesse = 0.5;
if (limelight3g.getV()) {
vitesse = 410.57 * ((Math.sqrt(Math.pow(Math.abs(4.625594 - botx), 2) + Math.pow(Math.abs(4.034536 - boty), 2))))
+ 2250;
System.out.println("lancer");
double output = pidController.calculate(lanceur.Vitesse(), vitesse);
lanceur.Lancer(output);
if (lanceur.Vitesse() >= vitesse - 800) {
timer.start();
if (timer.get() > 1) {
lanceur.Demeler(1);
}
}
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
lanceur.Lancer(0);
lanceur.Demeler(0);
timer.reset();
timer.stop();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return timer.get() > 4;
// return false;
}
}

132
src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java
View File

@@ -1,132 +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.ModeAuto;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Limelight3G;
import static edu.wpi.first.units.Units.*;
import java.util.Optional;
/* 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 LimelighterAuto extends Command {
Limelight3G limelight3g;
CommandSwerveDrivetrain drivetrain;
private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
double botx;
double boty;
double angle;
double calcul;
double x;
Optional<Alliance> alliance = DriverStation.getAlliance();
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
/** Creates a new Limelighter. */
public LimelighterAuto(Limelight3G limelight3g, CommandSwerveDrivetrain drivetrain) {
this.limelight3g = limelight3g;
this.drivetrain = drivetrain;
addRequirements(drivetrain, limelight3g);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) {
return;
}
if (alliance.get() == Alliance.Blue) {
x = 4.6;
BotPose = limelight3g.getBotPoseBlue();
}
else {
x = 11.915394;
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[0];
boty = BotPose[1];
angle = BotPose[5];
calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(calcul > -5 && calcul < 5){
drivetrain.setControl(
drive.withRotationalRate(0));
}
else if(calcul > 5){
drivetrain.setControl(
drive.withRotationalRate(2));
}
else if(calcul < -5){
drivetrain.setControl(drive.withRotationalRate(-2));
}
// botx = BotPose[1];
// boty = BotPose[0];
// angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
// calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
// if(calcul < -5 && calcul > -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else if(calcul > 5 && calcul < 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul < -5){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul <= -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else{
// drivetrain.setControl(
// drive.withRotationalRate(0));
// }
// drivetrain.setControl(
// drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
// System.out.println(angle);
// if (calcul < 0.2 && calcul > -0.2) {
// drivetrain.setControl(drive.withRotationalRate(0));
// }
}
else{
drivetrain.setControl(drive.withRotationalRate(0));
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
drivetrain.setControl(drive.withRotationalRate(0));
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return calcul > -5 && calcul < 5;
}
}

4
src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java
View File

@@ -15,9 +15,9 @@ import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Limelight3G;
import frc.robot.generated.TunerConstants; import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain; 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 */ /* 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 RetourMilieuDroite extends Command { public class RetourMilieuDroite extends Command {
@@ -38,7 +38,7 @@ public class RetourMilieuDroite extends Command {
public RetourMilieuDroite(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) { public RetourMilieuDroite(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) {
this.drivetrain = drivetrain; this.drivetrain = drivetrain;
this.limelight3g = limelight3g; this.limelight3g = limelight3g;
addRequirements(drivetrain, limelight3g); addRequirements(drivetrain);
// Use addRequirements() here to declare subsystem dependencies. // Use addRequirements() here to declare subsystem dependencies.
} }

4
src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java
View File

@@ -15,9 +15,9 @@ import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Limelight3G;
import frc.robot.generated.TunerConstants; import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain; 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 */ /* 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 RetourMilieuGauche extends Command { public class RetourMilieuGauche extends Command {
@@ -38,7 +38,7 @@ public class RetourMilieuGauche extends Command {
public RetourMilieuGauche(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) { public RetourMilieuGauche(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) {
this.drivetrain = drivetrain; this.drivetrain = drivetrain;
this.limelight3g = limelight3g; this.limelight3g = limelight3g;
addRequirements(drivetrain, limelight3g); addRequirements(drivetrain);
// Use addRequirements() here to declare subsystem dependencies. // Use addRequirements() here to declare subsystem dependencies.
} }

4
src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java
View File

@@ -37,9 +37,7 @@ public class TournerVersMur extends Command {
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public void initialize() { public void initialize() {}
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override

15
src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java
View File

@@ -37,9 +37,7 @@ public class TournerVersReservoir extends Command {
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public void initialize() { public void initialize() {}
alliance = DriverStation.getAlliance();
}
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
@@ -54,18 +52,13 @@ public class TournerVersReservoir extends Command {
angle = 0; angle = 0;
} }
} }
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10){
drivetrain.setControl(drive.withRotationalRate(0));
}
else{
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){ if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >0 && drivetrain.getPigeon2().getYaw().getValueAsDouble() <180){
drivetrain.setControl(drive.withRotationalRate(-force*2)); drivetrain.setControl(drive.withRotationalRate(-force*180/Math.PI));
} }
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){ else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(force*2)); drivetrain.setControl(drive.withRotationalRate(force*180/Math.PI));
} }
} }
}
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.
@Override @Override
@@ -74,6 +67,6 @@ public class TournerVersReservoir extends Command {
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public boolean isFinished() { public boolean isFinished() {
return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10; return drivetrain.getPigeon2().getYaw().getValueAsDouble() > angle && drivetrain.getPigeon2().getYaw().getValueAsDouble() < angle + 10;
} }
} }

43
src/main/java/frc/robot/commands/ModeOposer.java
View File

@@ -1,43 +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 edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur;
/* 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 ModeOposer extends Command {
private Lanceur lanceur;
/** Creates a new Lancer. */
public ModeOposer(Lanceur lanceur) {
this.lanceur = lanceur;
addRequirements(lanceur);
// 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() {
lanceur.Lancer(-0.2);
lanceur.Demeler(-0.2);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
lanceur.Demeler(0);
lanceur.Lancer(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

43
src/main/java/frc/robot/commands/ModeOposerBalayeuse.java
View File

@@ -1,43 +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 edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Balayeuse;
/* 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 ModeOposerBalayeuse extends Command {
private Balayeuse balayeuse;
/** Creates a new Lancer. */
public ModeOposerBalayeuse(Balayeuse balayeuse) {
this.balayeuse = balayeuse;
addRequirements(balayeuse);
// 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() {
balayeuse.BalayerEnbas(0.5);
balayeuse.BalayerPadle(0.2);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.BalayerEnbas(0);
balayeuse.BalayerPadle(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

47
src/main/java/frc/robot/commands/MonterBalyeuse.java
View File

@@ -1,47 +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 edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Balayeuse;
/* 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 MonterBalyeuse extends Command {
private Balayeuse balayeuse;
/** Creates a new MonterBalyeuse. */
public MonterBalyeuse(Balayeuse balayeuse) {
this.balayeuse = balayeuse;
addRequirements(balayeuse);
// 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() {
if(!balayeuse.GetLimiSwtich()){
balayeuse.Pivoter(0.5);
}
else{
balayeuse.Reset();
balayeuse.Pivoter(0);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
balayeuse.Pivoter(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

47
src/main/java/frc/robot/commands/MonterGrimpeur.java
View File

@@ -1,47 +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 edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Grimpeur;
/* 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 MonterGrimpeur extends Command {
private Grimpeur grimpeur;
/** Creates a new MonterGrimpeur. */
public MonterGrimpeur(Grimpeur grimpeur) {
this.grimpeur = grimpeur;
// 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() {
if(Math.abs(grimpeur.Position()) < grimpeur.PositionFinal()){
grimpeur.Grimper(0.5);
System.out.println("monte");
}
else {
grimpeur.Grimper(0);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
grimpeur.Grimper(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return Math.abs(grimpeur.Position()) > grimpeur.PositionFinal();
}
}

25
src/main/java/frc/robot/commands/ModeOposerDemeleur.java → src/main/java/frc/robot/commands/RainBowAnim.java
View File

@@ -5,33 +5,32 @@
package frc.robot.commands; package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Lanceur; import frc.robot.subsystems.Led;
/* 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 */ /* 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 ModeOposerDemeleur extends Command { public class RainBowAnim extends Command {
private Lanceur lanceur; private Led led;
/** Creates a new Lancer. */ /** Creates a new RainBowAnim. */
public ModeOposerDemeleur(Lanceur lanceur) { public RainBowAnim(Led led) {
this.lanceur = lanceur; this.led = led;
addRequirements(lanceur); addRequirements(led);
// Use addRequirements() here to declare subsystem dependencies. // Use addRequirements() here to declare subsystem dependencies.
} }
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public void initialize() { public void initialize() {
led.RainBow();
} }
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { public void execute() {}
lanceur.Demeler(-0.2);
}
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.
@Override @Override
public void end(boolean interrupted) { public void end(boolean interrupted) {
lanceur.Demeler(0); led.RainBowStop();
lanceur.Lancer(0);
} }
// Returns true when the command should end. // Returns true when the command should end.

65
src/main/java/frc/robot/subsystems/Balayeuse.java
View File

@@ -5,61 +5,42 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Balayeuse extends SubsystemBase { public class Balayeuse extends SubsystemBase {
ShuffleboardTab teb = Shuffleboard.getTab("teb"); ShuffleboardTab teb = Shuffleboard.getTab("teb");
SparkFlex Balaye1 = new SparkFlex(2, MotorType.kBrushless); SparkFlex moteurBas = new SparkFlex(2, MotorType.kBrushless);
SparkFlex Balaye2 = new SparkFlex(20, MotorType.kBrushless); SparkFlex moteurHaut = new SparkFlex(20, MotorType.kBrushless);
SparkMax Pivot = new SparkMax(8, MotorType.kBrushless);
DigitalInput limit = new DigitalInput(9); private final double vitesseMoteurBas = -0.5;
private GenericEntry EncodeurBalayeuse = private final double vitesseMoteurHaut = -0.2;
teb.add("Position bas balayeuse", 1.8).getEntry();
private GenericEntry AmpBaleyeuse = public Command balaye(double vitesseBas, double vitesseHaut) {
teb.add("Ampérage Baleyeuse", 40).getEntry(); return startEnd(
public void BalayerEnbas(double vitesse){ () -> {
Balaye1.set(vitesse); moteurBas.set(vitesseBas);
moteurHaut.set(vitesseHaut);
}, () -> {
moteurBas.set(0);
moteurHaut.set(0);
});
} }
public void BalayerPadle(double vitesse){
Balaye2.set(vitesse); public Command aspirer() {
return balaye(vitesseMoteurBas, vitesseMoteurHaut);
} }
public void Pivoter(double vitesse){
Pivot.set(vitesse); public Command ejecter() {
} return balaye(-vitesseMoteurBas, -vitesseMoteurHaut);
public double Distance(){
return Pivot.getEncoder().getPosition();
}
public void Reset(){
Pivot.getEncoder().setPosition(0.6);
}
public boolean GetLimiSwtich(){
return !limit.get();
}
public double Amp(){
return Balaye2.getOutputCurrent();
}
public double AmpMax(){
return AmpBaleyeuse.getDouble(40);
}
public void Temps(){
Timer timer = new Timer();
timer.start();
}
public double EncodeurBalayeuse(){
return EncodeurBalayeuse.getDouble(1.8);
} }
/** Creates a new Balayeuse. */ /** Creates a new Balayeuse. */
public Balayeuse() { public Balayeuse() {
teb.addBoolean("limit balayeuse", this::GetLimiSwtich);
teb.addDouble("encodeur balayeuse", this::Distance);
} }
@Override @Override

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

@@ -1,6 +1,8 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Volts;
import java.util.Optional; import java.util.Optional;
import java.util.function.Supplier; import java.util.function.Supplier;
@@ -9,8 +11,10 @@ import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
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.ctre.phoenix6.swerve.SwerveRequest.FieldCentricFacingAngle;
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;
@@ -19,6 +23,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N3;
@@ -29,7 +34,7 @@ import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Coordinates;
import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
/** /**
@@ -55,107 +60,108 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
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. */
private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
),
new SysIdRoutine.Mechanism(
output -> setControl(m_translationCharacterization.withVolts(output)),
null,
this
)
);
/* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ /*
* SysId routine for characterizing translation. This is used to find PID gains
* for the drive motors.
*/
@SuppressWarnings("unused")
private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())),
new SysIdRoutine.Mechanism(
output -> setControl(m_translationCharacterization.withVolts(output)),
null,
this));
/*
* SysId routine for characterizing steer. This is used to find PID gains for
* the steer motors.
*/
@SuppressWarnings("unused")
private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
new SysIdRoutine.Config( new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s) null, // Use default ramp rate (1 V/s)
Volts.of(7), // Use dynamic voltage of 7 V Volts.of(7), // Use dynamic voltage of 7 V
null, // Use default timeout (10 s) null, // Use default timeout (10 s)
// Log state with SignalLogger class // Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) state -> SignalLogger.writeString("SysIdSteer_State", state.toString())),
), new SysIdRoutine.Mechanism(
new SysIdRoutine.Mechanism( volts -> setControl(m_steerCharacterization.withVolts(volts)),
volts -> setControl(m_steerCharacterization.withVolts(volts)), null,
null, this));
this
)
);
/* /*
* SysId routine for characterizing rotation. * SysId routine for characterizing rotation.
* This is used to find PID gains for the FieldCentricFacingAngle HeadingController. * This is used to find PID gains for the FieldCentricFacingAngle
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. * HeadingController.
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on
* importing the log to SysId.
*/ */
@SuppressWarnings("unused")
private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
new SysIdRoutine.Config( new SysIdRoutine.Config(
/* This is in radians per second², but SysId only supports "volts per second" */ /* This is in radians per second², but SysId only supports "volts per second" */
Volts.of(Math.PI / 6).per(Second), Volts.of(Math.PI / 6).per(Second),
/* This is in radians per second, but SysId only supports "volts" */ /* This is in radians per second, but SysId only supports "volts" */
Volts.of(Math.PI), Volts.of(Math.PI),
null, // Use default timeout (10 s) null, // Use default timeout (10 s)
// Log state with SignalLogger class // Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) state -> SignalLogger.writeString("SysIdRotation_State", state.toString())),
), new SysIdRoutine.Mechanism(
new SysIdRoutine.Mechanism( output -> {
output -> { /* output is actually radians per second, but SysId only supports "volts" */
/* output is actually radians per second, but SysId only supports "volts" */ setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); /* also log the requested output for SysId */
/* also log the requested output for SysId */ SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); },
}, null,
null, this));
this
)
);
/* The SysId routine to test */
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
private void configureAutoBuilder() {
private void configureAutoBuilder() {
try { try {
RobotConfig config = RobotConfig.fromGUISettings(); RobotConfig config = RobotConfig.fromGUISettings();
AutoBuilder.configure( AutoBuilder.configure(
() -> getState().Pose, () -> getState().Pose,
this::resetPose, this::resetPose,
() -> getState().Speeds, () -> getState().Speeds,
(speeds, feedforwards) -> setControl( (speeds, feedforwards) -> setControl(
m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())),
), new PPHolonomicDriveController(
new PPHolonomicDriveController( new PIDConstants(10, 0, 0),
new PIDConstants(10, 0, 0), new PIDConstants(7, 0, 0)),
new PIDConstants(7, 0, 0) config,
), () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
config, this);
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
this
);
} 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());
} }
} }
/** /**
* Constructs a CTRE SwerveDrivetrain using the specified constants. * Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p> * <p>
* This constructs the underlying hardware devices, so users should not construct * This constructs the underlying hardware devices, so users should not
* the devices themselves. If they need the devices, they can access them through * construct
* the devices themselves. If they need the devices, they can access them
* through
* getters in the classes. * getters in the classes.
* *
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param modules Constants for each specific module * @param modules Constants for each specific module
*/ */
public CommandSwerveDrivetrain( public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants, SwerveDrivetrainConstants drivetrainConstants,
SwerveModuleConstants<?, ?, ?>... modules SwerveModuleConstants<?, ?, ?>... modules) {
) {
super(drivetrainConstants, modules); super(drivetrainConstants, modules);
if (Utils.isSimulation()) { if (Utils.isSimulation()) {
startSimThread(); startSimThread();
@@ -166,8 +172,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
/** /**
* Constructs a CTRE SwerveDrivetrain using the specified constants. * Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p> * <p>
* This constructs the underlying hardware devices, so users should not construct * This constructs the underlying hardware devices, so users should not
* the devices themselves. If they need the devices, they can access them through * construct
* the devices themselves. If they need the devices, they can access them
* through
* getters in the classes. * getters in the classes.
* *
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
@@ -177,10 +185,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
* @param modules Constants for each specific module * @param modules Constants for each specific module
*/ */
public CommandSwerveDrivetrain( public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants, SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency, double odometryUpdateFrequency,
SwerveModuleConstants<?, ?, ?>... modules SwerveModuleConstants<?, ?, ?>... modules) {
) {
super(drivetrainConstants, odometryUpdateFrequency, modules); super(drivetrainConstants, odometryUpdateFrequency, modules);
if (Utils.isSimulation()) { if (Utils.isSimulation()) {
startSimThread(); startSimThread();
@@ -191,30 +198,38 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
/** /**
* Constructs a CTRE SwerveDrivetrain using the specified constants. * Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p> * <p>
* This constructs the underlying hardware devices, so users should not construct * This constructs the underlying hardware devices, so users should not
* the devices themselves. If they need the devices, they can access them through * construct
* the devices themselves. If they need the devices, they can access them
* through
* getters in the classes. * getters in the classes.
* *
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive * @param drivetrainConstants Drivetrain-wide constants for the swerve
* drive
* @param odometryUpdateFrequency The frequency to run the odometry loop. If * @param odometryUpdateFrequency The frequency to run the odometry loop. If
* unspecified or set to 0 Hz, this is 250 Hz on * unspecified or set to 0 Hz, this is 250 Hz
* on
* CAN FD, and 100 Hz on CAN 2.0. * CAN FD, and 100 Hz on CAN 2.0.
* @param odometryStandardDeviation The standard deviation for odometry calculation * @param odometryStandardDeviation The standard deviation for odometry
* in the form [x, y, theta]ᵀ, with units in meters * calculation
* in the form [x, y, theta]ᵀ, with units in
* meters
* and radians * and radians
* @param visionStandardDeviation The standard deviation for vision calculation * @param visionStandardDeviation The standard deviation for vision
* in the form [x, y, theta]ᵀ, with units in meters * calculation
* in the form [x, y, theta]ᵀ, with units in
* meters
* and radians * and radians
* @param modules Constants for each specific module * @param modules Constants for each specific module
*/ */
public CommandSwerveDrivetrain( public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants, SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency, double odometryUpdateFrequency,
Matrix<N3, N1> odometryStandardDeviation, Matrix<N3, N1> odometryStandardDeviation,
Matrix<N3, N1> visionStandardDeviation, Matrix<N3, N1> visionStandardDeviation,
SwerveModuleConstants<?, ?, ?>... modules SwerveModuleConstants<?, ?, ?>... modules) {
) { super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation,
super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); modules);
if (Utils.isSimulation()) { if (Utils.isSimulation()) {
startSimThread(); startSimThread();
} }
@@ -222,7 +237,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
} }
/** /**
* Returns a command that applies the specified control request to this swerve drivetrain. * Returns a command that applies the specified control request to this swerve
* drivetrain.
* *
* @param request Function returning the request to apply * @param request Function returning the request to apply
* @return Command to run * @return Command to run
@@ -230,43 +246,26 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
public Command applyRequest(Supplier<SwerveRequest> request) { public Command applyRequest(Supplier<SwerveRequest> request) {
return run(() -> this.setControl(request.get())); return run(() -> this.setControl(request.get()));
} }
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return m_sysIdRoutineToApply.quasistatic(direction);
}
/**
* Runs the SysId Dynamic test in the given direction for the routine
* specified by {@link #m_sysIdRoutineToApply}.
*
* @param direction Direction of the SysId Dynamic test
* @return Command to run
*/
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return m_sysIdRoutineToApply.dynamic(direction);
}
@Override @Override
public void periodic() { public void periodic() {
if(getPigeon2().getYaw().getValueAsDouble() > 360){
getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()-360);
}
else if(getPigeon2().getYaw().getValueAsDouble() < 0){
getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()+360);
}
/* /*
* Periodically try to apply the operator perspective. * Periodically try to apply the operator perspective.
* If we haven't applied the operator perspective before, then we should apply it regardless of DS state. * If we haven't applied the operator perspective before, then we should apply
* This allows us to correct the perspective in case the robot code restarts mid-match. * it regardless of DS state.
* Otherwise, only check and apply the operator perspective if the DS is disabled. * This allows us to correct the perspective in case the robot code restarts
* This ensures driving behavior doesn't change until an explicit disable event occurs during testing. * mid-match.
* Otherwise, only check and apply the operator perspective if the DS is
* disabled.
* This ensures driving behavior doesn't change until an explicit disable event
* occurs during testing.
*/ */
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
DriverStation.getAlliance().ifPresent(allianceColor -> { DriverStation.getAlliance().ifPresent(allianceColor -> {
setOperatorPerspectiveForward( setOperatorPerspectiveForward(
allianceColor == Alliance.Red allianceColor == Alliance.Red
? kRedAlliancePerspectiveRotation ? kRedAlliancePerspectiveRotation
: kBlueAlliancePerspectiveRotation : kBlueAlliancePerspectiveRotation);
);
m_hasAppliedOperatorPerspective = true; m_hasAppliedOperatorPerspective = true;
}); });
} }
@@ -288,11 +287,14 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
} }
/** /**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate * Adds a vision measurement to the Kalman Filter. This will correct the
* odometry pose estimate
* while still accounting for measurement noise. * while still accounting for measurement noise.
* *
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. * @param visionRobotPoseMeters The pose of the robot as measured by the vision
* @param timestampSeconds The timestamp of the vision measurement in seconds. * camera.
* @param timestampSeconds The timestamp of the vision measurement in
* seconds.
*/ */
@Override @Override
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
@@ -300,35 +302,74 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
} }
/** /**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate * Adds a vision measurement to the Kalman Filter. This will correct the
* odometry pose estimate
* while still accounting for measurement noise. * while still accounting for measurement noise.
* <p> * <p>
* Note that the vision measurement standard deviations passed into this method * Note that the vision measurement standard deviations passed into this method
* will continue to apply to future measurements until a subsequent call to * will continue to apply to future measurements until a subsequent call to
* {@link #setVisionMeasurementStdDevs(Matrix)} or this method. * {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
* *
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. * @param visionRobotPoseMeters The pose of the robot as measured by the
* @param timestampSeconds The timestamp of the vision measurement in seconds. * vision camera.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement * @param timestampSeconds The timestamp of the vision measurement in
* in the form [x, y, theta]ᵀ, with units in meters and radians. * seconds.
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement
* in the form [x, y, theta]ᵀ, with units in
* meters and radians.
*/ */
@Override @Override
public void addVisionMeasurement( public void addVisionMeasurement(
Pose2d visionRobotPoseMeters, Pose2d visionRobotPoseMeters,
double timestampSeconds, double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs Matrix<N3, N1> visionMeasurementStdDevs) {
) { super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds),
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); visionMeasurementStdDevs);
} }
/** /**
* Return the pose at a given timestamp, if the buffer is not empty. * Return the pose at a given timestamp, if the buffer is not empty.
* *
* @param timestampSeconds The timestamp of the pose in seconds. * @param timestampSeconds The timestamp of the pose in seconds.
* @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). * @return The pose at the given timestamp (or Optional.empty() if the buffer is
* empty).
*/ */
@Override @Override
public Optional<Pose2d> samplePoseAt(double timestampSeconds) { public Optional<Pose2d> samplePoseAt(double timestampSeconds) {
return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds)); return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds));
} }
private SwerveRequest.FieldCentricFacingAngle fieldCentricFacingAngle = new FieldCentricFacingAngle()
.withDriveRequestType(DriveRequestType.OpenLoopVoltage).withHeadingPID(7, 0, 0);
public Command viserReservoir(Supplier<Double> velocityX, Supplier<Double> velocityY) {
return runEnd(() -> {
Translation2d diff = Coordinates.diffFromHub(getState().Pose.getTranslation());
Rotation2d angle = diff.getAngle();
setControl(fieldCentricFacingAngle
.withTargetDirection(angle)
.withVelocityX(velocityX.get())
.withVelocityY(velocityY.get()));
}, () -> setControl(fieldCentricFacingAngle.withVelocityX(0).withVelocityY(0)));
}
public Command allerGrimper(Supplier<Translation2d> diffSupplier) {
return runEnd(() -> {
Translation2d diff = diffSupplier.get();
Rotation2d angle = diff.getAngle().rotateBy(new Rotation2d(Degrees.of(-90)));
setControl(fieldCentricFacingAngle
.withTargetDirection(angle)
.withVelocityX(diff.getX())
.withVelocityY(diff.getY()));
}, () -> setControl(fieldCentricFacingAngle.withVelocityX(0).withVelocityY(0)));
}
public Command allerGrimpeMur() {
return allerGrimper(() -> Coordinates.diffFromGrimpeMur(getState().Pose.getTranslation()));
}
public Command allerGrimpeDepot() {
return allerGrimper(() -> Coordinates.diffFromGrimpeDepot(getState().Pose.getTranslation()));
}
} }

33
src/main/java/frc/robot/subsystems/Demeleur.java Normal file
View File

@@ -0,0 +1,33 @@
// 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 com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Demeleur extends SubsystemBase {
SparkMax moteur = new SparkMax(19, MotorType.kBrushless);
/** Creates a new Demeleur. */
public Demeleur() {
}
public Command run() {
return startEnd(() -> moteur.set(1), () -> moteur.set(0));
}
public Command inverse() {
return startEnd(() -> moteur.set(-0.2), () -> moteur.set(0));
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}

82
src/main/java/frc/robot/subsystems/Grimpeur.java
View File

@@ -4,48 +4,76 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import com.revrobotics.spark.SparkMax; import com.revrobotics.PersistMode;
import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.RelativeEncoder;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SoftLimitConfig;
import com.revrobotics.spark.config.SparkBaseConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Grimpeur extends SubsystemBase { public class Grimpeur extends SubsystemBase {
ShuffleboardTab teb = Shuffleboard.getTab("teb"); ShuffleboardTab teb = Shuffleboard.getTab("teb");
SparkMax grimpeur1 = new SparkMax(3,MotorType.kBrushless); SparkMax moteur1 = new SparkMax(3, MotorType.kBrushless);
SparkMax grimpeur2 = new SparkMax(12,MotorType.kBrushless); SparkMax moteur2 = new SparkMax(12, MotorType.kBrushless);
SparkMaxConfig slaveConfig = new SparkMaxConfig(); RelativeEncoder encoder1 = moteur1.getEncoder();
RelativeEncoder encoder2 = moteur2.getEncoder();
private double vitesseSortir = 0.5;
private double vitesseEntrer = -0.4;
private GenericEntry ntEncodeurGrimpeur = teb.add("Position haut grimpeur", 101).getEntry();
private double maxEncodeur = ntEncodeurGrimpeur.getDouble(101);
DigitalInput limit = new DigitalInput(0); DigitalInput limit = new DigitalInput(0);
private GenericEntry EncodeurGrimpeur =
teb.add("Position haut grimpeur", 101).getEntry();
public void Grimper(double vitesse){
grimpeur1.set(vitesse);
grimpeur2.set(vitesse);
}
public double Position(){
return grimpeur1.getEncoder().getPosition();
}
public void Reset(){
grimpeur1.getEncoder().setPosition(0);
}
public boolean Limit(){
return limit.get();
}
public double PositionFinal(){
return EncodeurGrimpeur.getDouble(101);
}
/** Creates a new Grimpeur. */
public Grimpeur() { public Grimpeur() {
teb.addDouble("encodeur grimpeur", this::Position); teb.addDouble("encodeur grimpeur", encoder1::getPosition);
teb.addBoolean("limit grimpeur", this::Limit); teb.addBoolean("limit grimpeur", limit::get);
configMoteurs();
}
private void configMoteurs() {
SparkBaseConfig configMoteur = new SparkMaxConfig().apply(
new SoftLimitConfig().forwardSoftLimit(maxEncodeur).forwardSoftLimitEnabled(true)
.reverseSoftLimit(0).reverseSoftLimitEnabled(true))
.openLoopRampRate(0.1)
.idleMode(IdleMode.kBrake);
moteur1.configure(configMoteur, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
moteur2.configure(configMoteur.follow(moteur1), ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
}
public Command sortir() {
return startEnd(() -> moteur1.set(vitesseSortir), () -> moteur1.set(0))
.until(() -> moteur1.getForwardSoftLimit().isReached() || moteur2.getForwardSoftLimit().isReached());
}
public Command rentrer() {
return startEnd(() -> moteur1.set(vitesseEntrer), () -> moteur1.set(0))
.until(() -> moteur1.getReverseSoftLimit().isReached() || moteur2.getReverseSoftLimit().isReached());
} }
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run if (limit.get()) {
encoder1.setPosition(0);
encoder2.setPosition(0);
}
double newEncoderMax = ntEncodeurGrimpeur.getDouble(maxEncodeur);
if (newEncoderMax != maxEncodeur) {
maxEncodeur = newEncoderMax;
configMoteurs();
}
} }
} }

142
src/main/java/frc/robot/subsystems/LEDSubsystem.java
View File

@@ -1,142 +0,0 @@
/* Generated by Phoenix Tuner X */
package frc.robot.subsystems;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.controls.SolidColor;
import com.ctre.phoenix6.hardware.CANdle;
import com.ctre.phoenix6.signals.RGBWColor;
/**
* Subsystem that controls an addressable LED strip using a CANdle.
*/
public class LEDSubsystem extends SubsystemBase {
Timer _timer;
ShuffleboardTab teb = Shuffleboard.getTab("teb");
private GenericEntry equipe =
teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
private final CANBus kCANBus = new CANBus("rio");
private final CANdle m_candle = new CANdle(17, kCANBus);
public void Bleu(){
m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(255, 0, 0, 0)));
}
public void Rouge(){
m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 255, 0)));
}
public void Vert(){
m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 255, 0, 0)));
}
public void Noir(){
m_candle.setControl(new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 0, 0)));
}
public void Flash(boolean couleur){
if(_timer.get() <0.5){
if(couleur){
Bleu();
}
else{
Rouge();
}
}
else{
Noir();
_timer.reset();
}
}
public LEDSubsystem() {
setDefaultCommand(updateLEDs());
_timer = new Timer();
_timer.reset();
_timer.start();
}
public boolean Equipe(){
return equipe.getBoolean(false);
}
/**
* Updates the animations and LEDs of the CANdle.
*
* @return Command to run
*/
public Command updateLEDs() {
double temps = DriverStation.getMatchTime();
System.out.println(temps);
return run(() -> {
if(Equipe()){
if(temps > 110){
Vert();
}
else if(temps > 88){
Bleu();
}
else if(temps > 85){
Flash(true);
}
else if(temps > 63){
Rouge();
}
else if(temps > 60){
Flash(false);
}
else if(temps > 33){
Bleu();
}
else if(temps > 30){
Flash(true);
}
else if(temps > 13){
Rouge();
}
else if(temps > 10){
Flash(false);
}
else if(temps < 10){
Vert();
}
}
else{
if(temps > 110){
Vert();
}
else if(temps > 88){
Rouge();
}
else if(temps > 85){
Flash(false);
}
else if(temps > 63){
Bleu();
}
else if(temps > 60){
Flash(true);
}
else if(temps > 33){
Rouge();
}
else if(temps > 30){
Flash(false);
}
else if(temps > 13){
Bleu();
}
else if(temps > 10){
Flash(true);
}
else if(temps < 10){
Vert();
}
}
});
}
}

80
src/main/java/frc/robot/subsystems/Lanceur.java
View File

@@ -4,48 +4,71 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import java.util.function.Supplier;
import com.revrobotics.PersistMode;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.ClosedLoopConfig;
import com.revrobotics.spark.config.SparkBaseConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Lanceur extends SubsystemBase { public class Lanceur extends SubsystemBase {
ShuffleboardTab teb = Shuffleboard.getTab("teb"); ShuffleboardTab teb = Shuffleboard.getTab("teb");
SparkFlex moteur1 = new SparkFlex(15, MotorType.kBrushless); SparkFlex moteur1 = new SparkFlex(15, MotorType.kBrushless);
SparkFlex moteur2 = new SparkFlex(18, MotorType.kBrushless); SparkFlex moteur2 = new SparkFlex(18, MotorType.kBrushless);
SparkMax Demeleur = new SparkMax(19, MotorType.kBrushless);
GenericEntry vitesse = RelativeEncoder encoder = moteur1.getEncoder();
teb.add("vitesse lanceur",4000).getEntry();
GenericEntry AmpLanceur = GenericEntry vitesse = teb.add("vitesse lanceur", 4000).getEntry();
teb.add("ampérage lanceur",30).getEntry(); GenericEntry AmpLanceur = teb.add("ampérage lanceur", 30).getEntry();
public void Lancer(double vitesse){
moteur1.set(vitesse);
moteur2.set(vitesse);
}
public void Demeler(double vitesse){
Demeleur.set(vitesse);
}
public double Vitesse(){
return moteur1.getEncoder().getVelocity();
}
public double Amp(){
return moteur1.getOutputCurrent();
}
public double AmpBas(){
return AmpLanceur.getDouble(30);
}
public double vitesseDemander(){
return vitesse.getDouble(4000);
}
/** Creates a new Lanceur. */ /** Creates a new Lanceur. */
public Lanceur() { public Lanceur() {
teb.addDouble("amperage lanceur", this::Amp); teb.addDouble("amperage lanceur", moteur1::getOutputCurrent);
teb.addDouble("vitesse actuelle",this::Vitesse); teb.addDouble("vitesse actuelle", encoder::getVelocity);
configMoteurs();
}
private void configMoteurs() {
SparkBaseConfig configMoteur = new SparkMaxConfig().apply(
new ClosedLoopConfig().pid(0.0007, 0, 0))
.openLoopRampRate(0.1)
.idleMode(IdleMode.kCoast);
moteur1.configure(configMoteur, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
moteur2.configure(configMoteur.follow(moteur1), ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
}
public Command lancer(Supplier<Double> vitesse) {
return runEnd(() -> moteur1.getClosedLoopController().setSetpoint(vitesse.get(), ControlType.kVelocity),
() -> moteur1.set(0));
}
public Command inverse() {
return startEnd(() -> moteur1.set(-0.2), () -> moteur1.set(0));
}
public double getVitesse() {
return encoder.getVelocity();
}
public boolean setPointAtteint() {
return getVitesse() >= moteur1.getClosedLoopController().getSetpoint() * 0.95;
}
public double ntBasseVitesse() {
return vitesse.getDouble(4000);
} }
@Override @Override
@@ -53,4 +76,3 @@ public class Lanceur extends SubsystemBase {
// This method will be called once per scheduler run // This method will be called once per scheduler run
} }
} }

145
src/main/java/frc/robot/subsystems/Led.java Normal file
View File

@@ -0,0 +1,145 @@
// 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 com.ctre.phoenix.led.CANdle;
import com.ctre.phoenix.led.RainbowAnimation;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Led extends SubsystemBase {
ShuffleboardTab teb = Shuffleboard.getTab("teb");
private GenericEntry equipe =
teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
@SuppressWarnings("removal")
CANdle CANDle = new CANdle(17);
@SuppressWarnings("removal")
RainbowAnimation rainbowAnim = new RainbowAnimation();
@SuppressWarnings("removal")
public void bleu(){
CANDle.setLEDs(0, 0, 255,0,0,8);
CANDle.setLEDs(0, 0, 255,0,16,8);
CANDle.setLEDs(0, 0, 255,0,32,8);
// CANDle.setLEDs(0, 0, 255,0,56,8);
// CANDle.setLEDs(0, 0, 255,0,72,8);
// CANDle.setLEDs(0, 0, 255,0,88,8);
// CANDle.setLEDs(0, 0, 255,0,104,8);
// CANDle.setLEDs(0, 0, 255,0,120,8);
// CANDle.setLEDs(0, 0, 255,0,136,8);
}
@SuppressWarnings("removal")
public void Vert1(){
CANDle.setLEDs(0, 255, 0,0,0,8);
CANDle.setLEDs(0, 255, 0,0,16,8);
CANDle.setLEDs(0, 255, 0,0,32,8);
// CANDle.setLEDs(0, 255, 0,0,56,8);
// CANDle.setLEDs(0, 255, 0,0,72,8);
// CANDle.setLEDs(0, 255, 0,0,88,8);
// CANDle.setLEDs(0, 255, 0,0,104,8);
// CANDle.setLEDs(0, 255, 0,0,120,8);
// CANDle.setLEDs(0, 255, 0,0,136,8);
}
@SuppressWarnings("removal")
public void Rouge(){
CANDle.setLEDs(255, 0, 0,0,0,8);
CANDle.setLEDs(255, 0, 0,0,16,8);
CANDle.setLEDs(255, 0, 0,0,32,8);
// CANDle.setLEDs(255, 0, 0,0,48,8);
// CANDle.setLEDs(255, 0, 0,0,64,8);
// CANDle.setLEDs(255, 0, 0,0,80,8);
// CANDle.setLEDs(255, 0, 0,0,96,8);
// CANDle.setLEDs(255, 0, 0,0,112,8);
// CANDle.setLEDs(255, 0, 0,0,128,8);
}
@SuppressWarnings("removal")
public void Jaune2(){
CANDle.setLEDs(255, 255, 0,0,8,8);
CANDle.setLEDs(255, 255, 0,0,24,8);
CANDle.setLEDs(255, 255, 0,0,40,8);
// CANDle.setLEDs(255, 255, 0,0,56,8);
// CANDle.setLEDs(255, 255, 0,0,72,8);
// CANDle.setLEDs(255, 255, 0,0,88,8);
// CANDle.setLEDs(255, 255, 0,0,104,8);
// CANDle.setLEDs(255, 255, 0,0,120,8);
// CANDle.setLEDs(255, 255, 0,0,136,8);
}
@SuppressWarnings("removal")
public void Rouge2(){
CANDle.setLEDs(255, 0, 0,0,8,8);
CANDle.setLEDs(255, 0, 0,0,24,8);
CANDle.setLEDs(255, 0, 0,0,40,8);
// CANDle.setLEDs(255, 0, 0,0,56,8);
// CANDle.setLEDs(255, 0, 0,0,72,8);
// CANDle.setLEDs(255, 0, 0,0,88,8);
// CANDle.setLEDs(255, 0, 0,0,104,8);
// CANDle.setLEDs(255, 0, 0,0,120,8);
// CANDle.setLEDs(255, 0, 0,0,136,8);
}
@SuppressWarnings("removal")
public void RainBow(){
CANDle.animate(rainbowAnim);
}
@SuppressWarnings("removal")
public void RainBowStop(){
CANDle.animate(null);
}
public boolean Equipe(){
return equipe.getBoolean(true);
}
/** Creates a new Led. */
public Led() {}
@Override
public void periodic() {
double temps = DriverStation.getMatchTime();
if(temps > 20 && temps < 30){
Vert1();
}
if(Equipe()){
if(temps > 30 && temps < 55){
bleu();
}
else if(temps > 30 && temps < 80){
Rouge();
}
else if(temps > 80 && temps < 105){
bleu();
}
else if(temps > 105 && temps < 130){
Rouge();
}
else{
RainBow();
}
}
else{
if(temps > 30 && temps < 55){
Rouge();
}
else if(temps > 30 && temps < 80){
bleu();
}
else if(temps > 80 && temps < 105){
Rouge();
}
else if(temps > 105 && temps < 130){
bleu();
}
else{
RainBow();
}
}
// This method will be called once per scheduler run
}
}

74
src/main/java/frc/robot/subsystems/LimeLight3.java
View File

@@ -1,74 +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-balaie");
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[] getBotPoseBlue(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
public double[] getBotPoseRed(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
public double getTx(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry tx = table.getEntry("tx");
return tx.getDouble(0.0);
}
public double getTId(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry tid = table.getEntry("tid");
return tid.getDouble(0.0);
}
public double getTA(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
NetworkTableEntry ta = table.getEntry("ta");
return ta.getDouble(0.0);
}
public boolean getV(){
return LimelightHelpers.getTV("limelight-balaie");
}
public void AprilTag(){
pipeline.setNumber(0);
}
public void Forme(){
pipeline.setNumber(1);
}
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
}
}

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

@@ -1,82 +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 Limelight3G extends SubsystemBase {
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry pipeline = table.getEntry("pipeline");
/** Creates a new LimeLight3. */
public Limelight3G() {
for(int port = 5800; port <=5807; port++){
PortForwarder.add(port, "limelight.local", port);
}
}
public double[] getBotPoseBlue(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
public double[] getBotPoseRed(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose;
}
public double getTx(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry tx = table.getEntry("tx");
return tx.getDouble(0.0);
}
public double getTId(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry tid = table.getEntry("tid");
return tid.getDouble(0.0);
}
public double getTA(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
NetworkTableEntry ta = table.getEntry("ta");
return ta.getDouble(0.0);
}
public boolean getV(){
return LimelightHelpers.getTV("limelight-tag");
}
public double Calcule(double x1, double x2, double y1, double y2, double angle)
{
// if(x1 > x2){
// if(y1 > y2){
// return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle;
// }
// else{
// return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle;
// }
// }
// else{
// if(y1 > y2){
// return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle;
// }
// else{
// return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle;
// }
// }
if(y1 > y2){
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
}
else{
return -Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
}
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}

82
src/main/java/frc/robot/subsystems/PivotBalayeuse.java Normal file
View File

@@ -0,0 +1,82 @@
// 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 com.revrobotics.PersistMode;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SoftLimitConfig;
import com.revrobotics.spark.config.SparkBaseConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class PivotBalayeuse extends SubsystemBase {
ShuffleboardTab teb = Shuffleboard.getTab("teb");
SparkMax moteur = new SparkMax(8, MotorType.kBrushless);
RelativeEncoder encoder = moteur.getEncoder();
DigitalInput limit = new DigitalInput(9);
private GenericEntry ntEncodeurBalayeuse = teb.add("Position bas balayeuse", 1.8).getEntry();
private double maxEncodeur = ntEncodeurBalayeuse.getDouble(1.8);
private double vitesseMonter = 0.5;
private double vitesseDescendre = -0.5;
/** Creates a new PivotBalayeuse. */
public PivotBalayeuse() {
configMoteurs();
}
private void configMoteurs() {
SparkBaseConfig configMoteur = new SparkMaxConfig().apply(
new SoftLimitConfig().forwardSoftLimit(maxEncodeur).forwardSoftLimitEnabled(true)
.reverseSoftLimit(0.6).reverseSoftLimitEnabled(true))
.openLoopRampRate(0.1)
.idleMode(IdleMode.kBrake);
moteur.configure(configMoteur, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
}
public Command monter() {
return startEnd(() -> moteur.set(vitesseMonter), () -> moteur.set(0))
.until(() -> moteur.getForwardSoftLimit().isReached());
}
public Command descendre() {
return startEnd(() -> moteur.set(vitesseDescendre), () -> moteur.set(0))
.until(() -> moteur.getReverseSoftLimit().isReached());
}
public double EncodeurBalayeuse() {
return ntEncodeurBalayeuse.getDouble(1.8);
}
@Override
public void periodic() {
teb.addBoolean("limit balayeuse", limit::get);
teb.addDouble("encodeur balayeuse", encoder::getPosition);
if (!limit.get()) {
encoder.setPosition(0.6);
}
double newEncoderMax = ntEncodeurBalayeuse.getDouble(maxEncodeur);
if (newEncoderMax != maxEncodeur) {
maxEncodeur = newEncoderMax;
configMoteurs();
}
}
}