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

mode auto

Browse Source
This commit is contained in:
samuel desharnais
2026-03-24 17:52:09 -04:00
parent a7fb3108ea
commit 7d2b2ea139
18 changed files with 277 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
src/main/deploy/pathplanner/autos/Copy of TirSimple.auto Normal file
View File

@@ -0,0 +1,31 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "TirerSimple"
}
},
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "named",
"data": {
"name": "Lancer"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

12
src/main/deploy/pathplanner/autos/DepotTirer.auto
View File

@@ -4,6 +4,12 @@
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "deadline",
"data": {
@@ -14,12 +20,6 @@
"pathName": "Depot"
}
},
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "named",
"data": {

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

@@ -4,6 +4,12 @@
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "deadline",
"data": {
@@ -14,12 +20,6 @@
"pathName": "Depot"
}
},
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "named",
"data": {

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

@@ -4,6 +4,12 @@
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "deadline",
"data": {
@@ -14,12 +20,6 @@
"pathName": "Depot"
}
},
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "named",
"data": {

12
src/main/deploy/pathplanner/autos/MilieuDroite.auto
View File

@@ -4,6 +4,12 @@
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "deadline",
"data": {
@@ -14,12 +20,6 @@
"pathName": "ChercherMilieuDroite"
}
},
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "named",
"data": {

12
src/main/deploy/pathplanner/autos/MilieuDroiteProche.auto
View File

@@ -4,6 +4,12 @@
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "deadline",
"data": {
@@ -14,12 +20,6 @@
"pathName": "ChercherMilieuDroiteProche"
}
},
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "named",
"data": {

12
src/main/deploy/pathplanner/autos/MilieuGauche.auto
View File

@@ -4,6 +4,12 @@
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "deadline",
"data": {
@@ -14,12 +20,6 @@
"pathName": "ChercherMilieuGauche"
}
},
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "named",
"data": {

12
src/main/deploy/pathplanner/autos/MilieuGaucheProche.auto
View File

@@ -4,6 +4,12 @@
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "deadline",
"data": {
@@ -14,12 +20,6 @@
"pathName": "ChercherMilieuGaucheProche"
}
},
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "named",
"data": {

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

@@ -10,6 +10,12 @@
"pathName": "TirerSimple"
}
},
{
"type": "named",
"data": {
"name": "DescendreBalayeuse"
}
},
{
"type": "named",
"data": {

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

@@ -1,6 +1,6 @@
{
"robotWidth": 0.9,
"robotLength": 0.9,
"robotWidth": 0.83,
"robotLength": 0.87,
"holonomicMode": true,
"pathFolders": [
"Milieu"
@@ -13,23 +13,23 @@
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0,
"robotMass": 74.088,
"robotMass": 54.5,
"robotMOI": 6.883,
"robotTrackwidth": 0.546,
"driveWheelRadius": 0.048,
"driveWheelRadius": 0.1,
"driveGearing": 5.143,
"maxDriveSpeed": 5.45,
"maxDriveSpeed": 5.374,
"driveMotorType": "krakenX60",
"driveCurrentLimit": 60.0,
"wheelCOF": 1.2,
"flModuleX": 0.273,
"flModuleY": 0.273,
"frModuleX": 0.273,
"frModuleY": -0.273,
"blModuleX": -0.273,
"blModuleY": 0.273,
"brModuleX": -0.273,
"brModuleY": -0.273,
"flModuleX": 0.288925,
"flModuleY": 0.269875,
"frModuleX": 0.288925,
"frModuleY": -0.27,
"blModuleX": -0.288925,
"blModuleY": 0.269875,
"brModuleX": -0.288925,
"brModuleY": -0.27,
"bumperOffsetX": 0.0,
"bumperOffsetY": 0.0,
"robotFeatures": []

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

@@ -14,6 +14,7 @@ import com.pathplanner.lib.auto.NamedCommands;
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.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
@@ -72,12 +73,9 @@ public class RobotContainer {
public RobotContainer() {
autoChooser = AutoBuilder.buildAutoChooser();
CameraServer.startAutomaticCapture();
configureBindings();
NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain));
NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain));
NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur));
NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite());
NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche());
NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain));
@@ -87,6 +85,12 @@ public class RobotContainer {
NamedCommands.registerCommand("TournerA180", new TournerA180(drivetrain));
NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur));
NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur));
autoChooser = AutoBuilder.buildAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser);
CameraServer.startAutomaticCapture();
configureBindings();
}
private void configureBindings() {
@@ -127,5 +131,6 @@ public class RobotContainer {
public Command getAutonomousCommand() {
return autoChooser.getSelected();
// return getAutonomousCommand();
}
}

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

@@ -41,6 +41,6 @@ public class DescendreBalyeuse extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return Math.abs(balayeuse.Distance()) > balayeuse.EncodeurBalayeuse();
}
}

42
src/main/java/frc/robot/commands/DitanceLancer.java Normal file
View File

@@ -0,0 +1,42 @@
// 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.Led;
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 DitanceLancer extends Command {
private Led led;
private Limelight3G limelight3g;
/** Creates a new DitanceLancer. */
public DitanceLancer(Limelight3G limelight3g, Led led) {
this.led = led;
this.limelight3g = limelight3g;
addRequirements(led,limelight3g);
// 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() {
}
// 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;
}
}

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

@@ -25,6 +25,7 @@ public class Lancer extends Command {
this.timer = new Timer();
this.limeLight3G = new Limelight3G();
addRequirements(lanceur, limeLight3G);
this.temp = 0;
// Use addRequirements() here to declare subsystem dependencies.
}
@@ -50,7 +51,7 @@ public class Lancer extends Command {
}
double vitesse = 0.5;
if(limeLight3G.getV()){
vitesse = 3100 + 475 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2))) - 2);
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(vitesse);
double output = pidController.calculate(lanceur.Vitesse(),vitesse);

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

@@ -24,6 +24,7 @@ public class Limelighter extends Command {
double botx;
double boty;
double angle;
double calcul;
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
.withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -55,7 +56,7 @@ public class Limelighter extends Command {
if(angle >180){
angle =- 360;
}
double calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 20;
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5;
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(calcul);

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

@@ -4,34 +4,58 @@
package frc.robot.commands.ModeAuto;
import edu.wpi.first.math.controller.PIDController;
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;
/** Creates a new LancerAuto. */
public LancerAuto(Lanceur lanceur) {
public LancerAuto(Lanceur lanceur, Limelight3G limelight3g) {
this.lanceur = lanceur;
timer = new Timer();
addRequirements(lanceur);
this.limelight3g = limelight3g;
addRequirements(lanceur, limelight3g);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
timer.start();
pidController = new PIDController(0.0007, 0,0, 0.001);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
lanceur.Lancer(0.5);
if(timer.get() > 1){
lanceur.Demeler(0.1);
double[] BotPose = new double[6];
if(limelight3g.getV()){
BotPose = limelight3g.getBotPoseBlue();
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(vitesse);
double output = pidController.calculate(lanceur.Vitesse(),vitesse);
lanceur.Lancer(output);
if(lanceur.Vitesse() >= vitesse-800){
timer.start();
if(timer.get() >0.5){
lanceur.Demeler(1);
}
}
}
}
@@ -47,7 +71,7 @@ public class LancerAuto extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return timer.get() > 3;
return timer.get() > 4;
//return false;
}
}

91
src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java Normal file
View File

@@ -0,0 +1,91 @@
// 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.Timer;
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.*;
/* 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 {
Timer timer;
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;
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);
timer = new Timer();
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
timer.reset();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double[] BotPose = new double[6];
if (limelight3g.getV()) {
BotPose = limelight3g.getBotPoseBlue();
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
if(angle >180){
angle =- 360;
}
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5;
drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
System.out.println(calcul);
if (calcul < 0.2 && calcul > -0.2) {
// timer.start();
// } else {
// timer.reset();
// }
// } else {
// timer.stop();
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));
timer.stop();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return calcul < 0.1 && calcul > -0.1;
}
}

3
src/main/java/frc/robot/subsystems/Led.java
View File

@@ -4,11 +4,14 @@
package frc.robot.subsystems;
import java.util.Optional;
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.DriverStation.Alliance;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;