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

Merge branch 'main' of https://git.demerso.net/sdesharnais/Rebuilt-2026

Browse Source
This commit is contained in:
Olivier Dubois
2026-04-01 04:48:25 +02:00
parent a429b936d2 3b06cbd447
commit 75922f581c
19 changed files with 467 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -0,0 +1,99 @@
{
"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
}

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

@@ -14,6 +14,12 @@
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 3.0
}
},
{
"type": "path",
"data": {
@@ -72,9 +78,9 @@
}
},
{
"type": "path",
"type": "named",
"data": {
"pathName": "MonterMur"
"name": "droite"
}
},
{

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

@@ -14,6 +14,12 @@
"type": "deadline",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 3.0
}
},
{
"type": "path",
"data": {
@@ -72,9 +78,9 @@
}
},
{
"type": "path",
"type": "named",
"data": {
"pathName": "MonterReservoir"
"name": "gauche"
}
},
{

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

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

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

@@ -20,6 +20,7 @@ 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;
@@ -30,6 +31,8 @@ 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;
@@ -73,6 +76,8 @@ public class RobotContainer {
public RobotContainer() {
NamedCommands.registerCommand("droite", new BougerDroiteAuto(drivetrain));
NamedCommands.registerCommand("gauche", new BougerGaucheAuto(drivetrain));
NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain));
NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain));
NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
@@ -126,6 +131,9 @@ public class RobotContainer {
manette1.b().whileTrue(new ModeOposer(lanceur));
manette1.a().whileTrue(new ModeOposerDemeleur(lanceur));
manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse));
manette1.povRight().whileTrue(new BougerDroiteAuto(drivetrain));
manette1.povLeft().whileTrue(new BougerGaucheAuto(drivetrain));
manette1.start().whileTrue(new Inverser(drivetrain));
}
public Command getAutonomousCommand() {

42
src/main/java/frc/robot/commands/Inverser.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.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) {
addRequirements(drivetrain);
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(!inverse){
drivetrain.getPigeon2().setYaw(drivetrain.getPigeon2().getYaw().getValueAsDouble()+180);
inverse = true;
}
}
// 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;
}
}

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

@@ -48,7 +48,9 @@ public class Lancer extends Command {
public void execute() {
double[] BotPose = new double[6];
if(limeLight3G.getV()){
if(!alliance.isPresent()){return;}
if(!alliance.isPresent()){
return;
}
if(alliance.get() == Alliance.Blue){
BotPose = limeLight3G.getBotPoseBlue();
}
@@ -60,10 +62,11 @@ public class Lancer extends Command {
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()){
System.out.println(vitesse);
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){

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

@@ -53,7 +53,7 @@ public class Limelighter extends Command {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
// BotPose = limelight3g.getBotPoseBlue();
BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) {
return;
}
@@ -65,25 +65,24 @@ public class Limelighter extends Command {
x = 11.915394;
BotPose = limelight3g.getBotPoseRed();
}
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
botx = BotPose[0];
boty = BotPose[1];
calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(angle > 180){
angle -= 360;
}
if(calcul > -5 && calcul < 5){
drivetrain.setControl(
drive.withRotationalRate(0));
}
else if(calcul > 5){
drivetrain.setControl(
drive.withRotationalRate(0.5*(2*Math.PI)));
drive.withRotationalRate(0.5*(2*Math.PI)));
}
else if(calcul < -5){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
drivetrain.setControl(drive.withRotationalRate(-0.5*(2*Math.PI)));
}
// 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)));
@@ -92,7 +91,7 @@ public class Limelighter extends Command {
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul >= 180){
// else if(calcul < -5){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }

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

@@ -0,0 +1,80 @@
// 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;
}
}

79
src/main/java/frc/robot/commands/ModeAuto/BougerGaucheAuto.java Normal file
View File

@@ -0,0 +1,79 @@
// 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(alliance.get() == Alliance.Blue){
if(timer.get() < 0.75){
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;
}
}

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

@@ -48,7 +48,10 @@ public class GrimperMur extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {
alliance = DriverStation.getAlliance();
// alliance = DriverStation.getAlliance();
// if(drivetrain.Equipe()){
// angle+=180;
// }
}
// Called every time the scheduler runs while the command is scheduled.
@@ -62,7 +65,7 @@ public class GrimperMur extends Command {
angle = angle + 360;
}
if(alliance.get() == Alliance.Blue){
y = 2.961328;
y = 5;
x = 1.11;
angle = 0;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() > 350 || drivetrain.getPigeon2().getYaw().getValueAsDouble() < 10){

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

@@ -14,6 +14,7 @@ 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;
@@ -31,6 +32,7 @@ public class GrimperReservoir extends Command {
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 = DriverStation.getAlliance();
@@ -48,52 +50,62 @@ public class GrimperReservoir extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {
alliance = DriverStation.getAlliance();
// 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() {
BotPose = limeLight3G.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
System.out.println(drivetrain.getPigeon2().getYaw().getValueAsDouble());
if(angle < 0){
angle = angle + 360;
}
if(alliance.get() == Alliance.Red){
y = 6.959326;
x = 13.57966;
angle = 180;
if(drivetrain.getPigeon2().getYaw().getValueAsDouble() < 190 && drivetrain.getPigeon2().getYaw().getValueAsDouble() > 170){
drivetrain.setControl(drive.withVelocityY(y-boty).withVelocityX(x-botx));
if(limeLight3G.getV()){
BotPose = limeLight3G.getBotPoseBlue();
botx = BotPose[0];
boty = BotPose[1];
if(angle < 0){
angle = angle + 360;
}
if(alliance.get() == Alliance.Red){
y = 6.959326;
x = 13.57966;
angle = 180;
if(pigeonAngle< 190 && pigeonAngle> 170){
drivetrain.setControl(drive.withVelocityY(MathUtil.clamp(((y-boty)*5), -2, 2)).withVelocityX(MathUtil.clamp(((x-botx)*5),-2,2)));
}
else{
if(pigeonAngle>0 && pigeonAngle<180){
drivetrain.setControl(drive.withRotationalRate(1));
}
else if(pigeonAngle>180){
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{
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));
}
drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
}
}
else{
y = 5.081328;
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));
}
}
}
}
// Called once the command ends or is interrupted.
@@ -105,6 +117,6 @@ public class GrimperReservoir extends Command {
// 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);
return (y-boty < 0.1 && y-boty >-0.1) && (x-botx < 0.1 && x-botx > -0.1);
}
}

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

@@ -78,7 +78,7 @@ public class LimelighterAuto extends Command {
}
else if(calcul > 5){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
drive.withRotationalRate(0.5*(2*Math.PI)));
}
else if(calcul < -5){
drivetrain.setControl(

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

@@ -54,13 +54,18 @@ public class TournerVersReservoir extends Command {
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){
drivetrain.setControl(drive.withRotationalRate(-force*180/Math.PI));
drivetrain.setControl(drive.withRotationalRate(-force*2));
}
else if(drivetrain.getPigeon2().getYaw().getValueAsDouble() >180){
drivetrain.setControl(drive.withRotationalRate(force*180/Math.PI));
drivetrain.setControl(drive.withRotationalRate(force*2));
}
}
}
// Called once the command ends or is interrupted.
@Override
@@ -69,6 +74,6 @@ public class TournerVersReservoir extends Command {
// Returns true when the command should end.
@Override
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;
}
}

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

@@ -42,6 +42,6 @@ public class MonterGrimpeur extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return Math.abs(grimpeur.Position()) > grimpeur.PositionFinal();
}
}

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

@@ -22,10 +22,14 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
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.Subsystem;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
@@ -54,8 +58,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
/* Swerve requests to apply during SysId characterization */
private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
ShuffleboardTab teb = Shuffleboard.getTab("teb");
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
private GenericEntry equipe =
teb.add("equipe commence (bleu = ouvert, rouge = fermé)", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
/* 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(
@@ -221,7 +227,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
configureAutoBuilder();
}
public boolean Equipe(){
return equipe.getBoolean(false);
}
/**
* Returns a command that applies the specified control request to this swerve drivetrain.
*
@@ -254,7 +262,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
else if(getPigeon2().getYaw().getValueAsDouble() < 0){
getPigeon2().setYaw(getPigeon2().getYaw().getValueAsDouble()+360);
}
/*
* 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.

39
src/main/java/frc/robot/subsystems/LEDSubsystem.java Normal file
View File

@@ -0,0 +1,39 @@
/* Generated by Phoenix Tuner X */
package frc.robot.subsystems;
import static edu.wpi.first.units.Units.*;
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 {
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(){
new SolidColor(0, 80).withColor(new RGBWColor(0, 0, 255, 0));
}
public LEDSubsystem() {
setDefaultCommand(updateLEDs());
}
/**
* Updates the animations and LEDs of the CANdle.
*
* @return Command to run
*/
public Command updateLEDs() {
return run(() -> {});
}
}

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

@@ -16,13 +16,8 @@ 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);
@@ -47,7 +42,6 @@ public class Led extends SubsystemBase {
// 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);
@@ -59,7 +53,6 @@ public class Led extends SubsystemBase {
// 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);
@@ -91,9 +84,6 @@ public class Led extends SubsystemBase {
public void RainBowStop(){
CANDle.animate(null);
}
public boolean Equipe(){
return equipe.getBoolean(true);
}
/** Creates a new Led. */
public Led() {}
@@ -104,7 +94,7 @@ public class Led extends SubsystemBase {
if(temps > 20 && temps < 30){
Vert1();
}
if(Equipe()){
// if(Equipe()){
if(temps > 30 && temps < 55){
bleu();
}
@@ -120,8 +110,8 @@ public class Led extends SubsystemBase {
else{
RainBow();
}
}
else{
// }
// else{
if(temps > 30 && temps < 55){
Rouge();
}
@@ -140,6 +130,6 @@ public class Led extends SubsystemBase {
}
// This method will be called once per scheduler run
}
// }
}

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

@@ -72,7 +72,7 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
}
else{
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)-360 - angle;
return -Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
}
}
@Override