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

big stuff

Browse Source
This commit is contained in:
Olivier Demers
2026-04-22 10:21:28 -04:00
parent f7e88619e9
commit 4ecaef3d1d
34 changed files with 779 additions and 1580 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
build.gradle
View File

@@ -33,7 +33,7 @@ deploy {
frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
files = project.fileTree('src/main/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
}
}

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;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.TimedRobot;
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);
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) {
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds);
m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds,
limelightStdDevs);
}
}
@Override
public void disabledInit() {}
public void disabledInit() {
}
@Override
public void disabledPeriodic() {}
public void disabledPeriodic() {
}
@Override
public void disabledExit() {}
public void disabledExit() {
}
@Override
public void autonomousInit() {
@@ -51,10 +61,12 @@ public class Robot extends TimedRobot {
}
@Override
public void autonomousPeriodic() {}
public void autonomousPeriodic() {
}
@Override
public void autonomousExit() {}
public void autonomousExit() {
}
@Override
public void teleopInit() {
@@ -64,10 +76,12 @@ public class Robot extends TimedRobot {
}
@Override
public void teleopPeriodic() {}
public void teleopPeriodic() {
}
@Override
public void teleopExit() {}
public void teleopExit() {
}
@Override
public void testInit() {
@@ -75,8 +89,10 @@ public class Robot extends TimedRobot {
}
@Override
public void testPeriodic() {}
public void testPeriodic() {
}
@Override
public void testExit() {}
public void testExit() {
}
}

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

@@ -4,7 +4,9 @@
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.SwerveRequest;
@@ -12,28 +14,11 @@ import com.pathplanner.lib.auto.AutoBuilder;
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.Commands;
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.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.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.RetourMilieuGauche;
import frc.robot.commands.ModeAuto.TournerVersMur;
@@ -41,91 +26,89 @@ import frc.robot.commands.ModeAuto.TournerVersReservoir;
import frc.robot.generated.TunerConstants;
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.Led;
import frc.robot.subsystems.LimeLight3;
import frc.robot.subsystems.Limelight3G;
import frc.robot.subsystems.PivotBalayeuse;
public class RobotContainer {
private final SendableChooser<Command> autoChooser;
Balayeuse balayeuse = new Balayeuse();
Grimpeur grimpeur = new Grimpeur();
Lanceur lanceur = new Lanceur();
LimeLight3 limeLight3 = new LimeLight3();
Limelight3G limeLight3G = new Limelight3G();
Led led = new Led();
CommandXboxController manette = new CommandXboxController(0);
CommandXboxController manette1 = new CommandXboxController(1);
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
private final Balayeuse balayeuse = new Balayeuse();
private final PivotBalayeuse pivotBalayeuse = new PivotBalayeuse();
private final Demeleur demeleur = new Demeleur();
private final Grimpeur grimpeur = new Grimpeur();
private final Lanceur lanceur = new Lanceur();
private final Commandes commandes = new Commandes(drivetrain, lanceur, demeleur, balayeuse, grimpeur, pivotBalayeuse);
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 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 */
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
private final Telemetry logger = new Telemetry(MaxSpeed);
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
/* Setting up bindings for necessary control of the swerve drive platform */
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
private final Telemetry logger = new Telemetry(MaxSpeed);
public RobotContainer() {
NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain));
NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain));
NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain,limeLight3G));
NamedCommands.registerCommand("GrimperMur", drivetrain.allerGrimpeMur());
NamedCommands.registerCommand("GrimperReservoir", drivetrain.allerGrimpeDepot());
NamedCommands.registerCommand("Lancer", commandes.lancer());
NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain, limeLight3G));
NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche(drivetrain, limeLight3G));
NamedCommands.registerCommand("Limelighter", new LimelighterAuto(limeLight3G,drivetrain));
NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse));
NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse));
NamedCommands.registerCommand("Limelighter", commandes.viserLancer());
NamedCommands.registerCommand("DescendreBalayeuse", pivotBalayeuse.descendre());
NamedCommands.registerCommand("Aspirer", balayeuse.aspirer());
NamedCommands.registerCommand("TournerA180", new TournerVersReservoir(drivetrain));
NamedCommands.registerCommand("TournerAZero", new TournerVersMur(drivetrain));
NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur));
NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur));
NamedCommands.registerCommand("MonterGrimpeur", grimpeur.sortir());
NamedCommands.registerCommand("DescendreGrimpeur", grimpeur.rentrer());
autoChooser = AutoBuilder.buildAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser);
CameraServer.startAutomaticCapture();
configureBindings();
}
private void configureBindings() {
// Manette 0 (pilote)
drivetrain.setDefaultCommand(
drivetrain.applyRequest(() ->
drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed*0.7, 0.05))
.withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*Math.abs(-manette.getLeftY())*MaxSpeed*0.7, 0.05))
.withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05))
)
);
//manette 1
// 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));
drivetrain.applyRequest(() -> drive
.withVelocityY(-manette.getLeftX() * MaxSpeed * 0.7)
.withVelocityX(-manette.getLeftY() * MaxSpeed * 0.7)
.withRotationalRate(-manette.getRightX() * MaxAngularRate)));
manette.rightBumper().whileTrue(balayeuse.aspirer());
manette.povUp().whileTrue(grimpeur.sortir());
manette.povDown().whileTrue(grimpeur.rentrer());
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
manette1.rightTrigger().whileTrue(new Aspirer(balayeuse));
manette1.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));
manette1.leftTrigger().whileTrue(new LancerBaseVitesse(lanceur));
manette1.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
manette1.x().whileTrue(new LancerAspirer(lanceur, balayeuse, limeLight3G));
manette1.b().whileTrue(new ModeOposer(lanceur));
manette1.a().whileTrue(new ModeOposerDemeleur(lanceur));
manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse));
// Manette 1 (co-pilote)
manette1.rightTrigger().whileTrue(balayeuse.aspirer());
manette1.rightBumper().whileTrue(pivotBalayeuse.monter());
manette1.leftTrigger().whileTrue(commandes.lancer(() -> lanceur.ntBasseVitesse()));
manette1.leftBumper().whileTrue(pivotBalayeuse.descendre());
manette1.x().whileTrue(commandes.lancerAspirer());
manette1.b().whileTrue(Commands.parallel(lanceur.inverse(), demeleur.inverse()));
manette1.a().whileTrue(demeleur.inverse());
manette1.y().whileTrue(balayeuse.ejecter());
drivetrain.registerTelemetry(logger::telemeterize);
}
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;
}
}

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

@@ -1,90 +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();
}
// 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()){
System.out.println(vitesse);
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.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;
}
}

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

@@ -1,103 +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;
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() {}
// 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();
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle);
if(calcul < -5 && calcul > -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/Math.PI));
}
else if(calcul > 5 && calcul < 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
}
else if(calcul >= 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/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;
}
}

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

@@ -1,108 +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;
/* 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() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
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 = 2.961328;
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 = 11.45966;
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);
}
}

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

@@ -1,108 +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.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;
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 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() {}
// 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));
}
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{
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.
@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);
}
}

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

@@ -1,92 +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();
}
// 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;
}
}

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

@@ -1,104 +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;
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() {}
// 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();
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle);
if(calcul < -5 && calcul > -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/Math.PI));
}
else if(calcul > 5 && calcul < 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
}
else if(calcul >= 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/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.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Limelight3G;
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 RetourMilieuDroite extends Command {
@@ -38,7 +38,7 @@ public class RetourMilieuDroite extends Command {
public RetourMilieuDroite(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) {
this.drivetrain = drivetrain;
this.limelight3g = limelight3g;
addRequirements(drivetrain, limelight3g);
addRequirements(drivetrain);
// 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.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Limelight3G;
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 RetourMilieuGauche extends Command {
@@ -38,7 +38,7 @@ public class RetourMilieuGauche extends Command {
public RetourMilieuGauche(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) {
this.drivetrain = drivetrain;
this.limelight3g = limelight3g;
addRequirements(drivetrain, limelight3g);
addRequirements(drivetrain);
// Use addRequirements() here to declare subsystem dependencies.
}

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;
}
}

42
src/main/java/frc/robot/commands/ModeOposerDemeleur.java
View File

@@ -1,42 +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 ModeOposerDemeleur extends Command {
private Lanceur lanceur;
/** Creates a new Lancer. */
public ModeOposerDemeleur(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.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;
}
}

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 false;
}
}

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

@@ -5,61 +5,42 @@
package frc.robot.subsystems;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkMax;
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.ShuffleboardTab;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Balayeuse extends SubsystemBase {
ShuffleboardTab teb = Shuffleboard.getTab("teb");
SparkFlex Balaye1 = new SparkFlex(2, MotorType.kBrushless);
SparkFlex Balaye2 = new SparkFlex(20, MotorType.kBrushless);
SparkMax Pivot = new SparkMax(8, MotorType.kBrushless);
DigitalInput limit = new DigitalInput(9);
private GenericEntry EncodeurBalayeuse =
teb.add("Position bas balayeuse", 1.8).getEntry();
private GenericEntry AmpBaleyeuse =
teb.add("Ampérage Baleyeuse", 40).getEntry();
public void BalayerEnbas(double vitesse){
Balaye1.set(vitesse);
SparkFlex moteurBas = new SparkFlex(2, MotorType.kBrushless);
SparkFlex moteurHaut = new SparkFlex(20, MotorType.kBrushless);
private final double vitesseMoteurBas = -0.5;
private final double vitesseMoteurHaut = -0.2;
public Command balaye(double vitesseBas, double vitesseHaut) {
return startEnd(
() -> {
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 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);
public Command ejecter() {
return balaye(-vitesseMoteurBas, -vitesseMoteurHaut);
}
/** Creates a new Balayeuse. */
public Balayeuse() {
teb.addBoolean("limit balayeuse", this::GetLimiSwtich);
teb.addDouble("encodeur balayeuse", this::Distance);
}
@Override

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

@@ -1,6 +1,8 @@
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.function.Supplier;
@@ -9,8 +11,10 @@ import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveRequest;
import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentricFacingAngle;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants;
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.geometry.Pose2d;
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.numbers.N1;
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.Subsystem;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Coordinates;
import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
/**
@@ -56,107 +61,107 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
/*
* SysId routine for characterizing translation. This is used to find PID gains
* for the drive motors.
*/
@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
)
);
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 steer. This is used to find PID gains for
* the steer motors.
*/
@SuppressWarnings("unused")
private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(7), // Use dynamic voltage of 7 V
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
),
new SysIdRoutine.Mechanism(
volts -> setControl(m_steerCharacterization.withVolts(volts)),
null,
this
)
);
new SysIdRoutine.Config(
null, // Use default ramp rate (1 V/s)
Volts.of(7), // Use dynamic voltage of 7 V
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdSteer_State", state.toString())),
new SysIdRoutine.Mechanism(
volts -> setControl(m_steerCharacterization.withVolts(volts)),
null,
this));
/*
* SysId routine for characterizing rotation.
* This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
* This is used to find PID gains for the FieldCentricFacingAngle
* HeadingController.
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on
* importing the log to SysId.
*/
@SuppressWarnings("unused")
private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
new SysIdRoutine.Config(
/* This is in radians per second², but SysId only supports "volts per second" */
Volts.of(Math.PI / 6).per(Second),
/* This is in radians per second, but SysId only supports "volts" */
Volts.of(Math.PI),
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
),
new SysIdRoutine.Mechanism(
output -> {
/* output is actually radians per second, but SysId only supports "volts" */
setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
/* also log the requested output for SysId */
SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
},
null,
this
)
);
/* The SysId routine to test */
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
new SysIdRoutine.Config(
/* This is in radians per second², but SysId only supports "volts per second" */
Volts.of(Math.PI / 6).per(Second),
/* This is in radians per second, but SysId only supports "volts" */
Volts.of(Math.PI),
null, // Use default timeout (10 s)
// Log state with SignalLogger class
state -> SignalLogger.writeString("SysIdRotation_State", state.toString())),
new SysIdRoutine.Mechanism(
output -> {
/* output is actually radians per second, but SysId only supports "volts" */
setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
/* also log the requested output for SysId */
SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
},
null,
this));
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
private void configureAutoBuilder() {
private void configureAutoBuilder() {
try {
RobotConfig config = RobotConfig.fromGUISettings();
AutoBuilder.configure(
() -> getState().Pose,
this::resetPose,
() -> getState().Speeds,
(speeds, feedforwards) -> setControl(
m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
),
new PPHolonomicDriveController(
new PIDConstants(10, 0, 0),
new PIDConstants(7, 0, 0)
),
config,
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
this
);
() -> getState().Pose,
this::resetPose,
() -> getState().Speeds,
(speeds, feedforwards) -> setControl(
m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())),
new PPHolonomicDriveController(
new PIDConstants(10, 0, 0),
new PIDConstants(7, 0, 0)),
config,
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
this);
} 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.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* This constructs the underlying hardware devices, so users should not
* construct
* the devices themselves. If they need the devices, they can access them
* through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param modules Constants for each specific module
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
* @param modules Constants for each specific module
*/
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
SwerveModuleConstants<?, ?, ?>... modules
) {
SwerveDrivetrainConstants drivetrainConstants,
SwerveModuleConstants<?, ?, ?>... modules) {
super(drivetrainConstants, modules);
if (Utils.isSimulation()) {
startSimThread();
@@ -167,8 +172,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* This constructs the underlying hardware devices, so users should not
* construct
* the devices themselves. If they need the devices, they can access them
* through
* getters in the classes.
*
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
@@ -178,10 +185,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
* @param modules Constants for each specific module
*/
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
SwerveModuleConstants<?, ?, ?>... modules
) {
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
SwerveModuleConstants<?, ?, ?>... modules) {
super(drivetrainConstants, odometryUpdateFrequency, modules);
if (Utils.isSimulation()) {
startSimThread();
@@ -192,30 +198,38 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
* This constructs the underlying hardware devices, so users should not construct
* the devices themselves. If they need the devices, they can access them through
* This constructs the underlying hardware devices, so users should not
* construct
* the devices themselves. If they need the devices, they can access them
* through
* 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
* 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.
* @param odometryStandardDeviation The standard deviation for odometry calculation
* in the form [x, y, theta]ᵀ, with units in meters
* @param odometryStandardDeviation The standard deviation for odometry
* calculation
* in the form [x, y, theta]ᵀ, with units in
* meters
* and radians
* @param visionStandardDeviation The standard deviation for vision calculation
* in the form [x, y, theta]ᵀ, with units in meters
* @param visionStandardDeviation The standard deviation for vision
* calculation
* in the form [x, y, theta]ᵀ, with units in
* meters
* and radians
* @param modules Constants for each specific module
*/
public CommandSwerveDrivetrain(
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
Matrix<N3, N1> odometryStandardDeviation,
Matrix<N3, N1> visionStandardDeviation,
SwerveModuleConstants<?, ?, ?>... modules
) {
super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules);
SwerveDrivetrainConstants drivetrainConstants,
double odometryUpdateFrequency,
Matrix<N3, N1> odometryStandardDeviation,
Matrix<N3, N1> visionStandardDeviation,
SwerveModuleConstants<?, ?, ?>... modules) {
super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation,
modules);
if (Utils.isSimulation()) {
startSimThread();
}
@@ -223,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
* @return Command to run
@@ -231,44 +246,26 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
public Command applyRequest(Supplier<SwerveRequest> request) {
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
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.
* If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
* This allows us to correct the perspective in case the robot code restarts 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 we haven't applied the operator perspective before, then we should apply
* it regardless of DS state.
* This allows us to correct the perspective in case the robot code restarts
* 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()) {
DriverStation.getAlliance().ifPresent(allianceColor -> {
setOperatorPerspectiveForward(
allianceColor == Alliance.Red
? kRedAlliancePerspectiveRotation
: kBlueAlliancePerspectiveRotation
);
allianceColor == Alliance.Red
? kRedAlliancePerspectiveRotation
: kBlueAlliancePerspectiveRotation);
m_hasAppliedOperatorPerspective = true;
});
}
@@ -290,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.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds.
* @param visionRobotPoseMeters The pose of the robot as measured by the vision
* camera.
* @param timestampSeconds The timestamp of the vision measurement in
* seconds.
*/
@Override
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
@@ -302,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.
* <p>
* Note that the vision measurement standard deviations passed into this method
* will continue to apply to future measurements until a subsequent call to
* {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
* in the form [x, y, theta]ᵀ, with units in meters and radians.
* @param visionRobotPoseMeters The pose of the robot as measured by the
* vision camera.
* @param timestampSeconds The timestamp of the vision measurement in
* seconds.
* @param visionMeasurementStdDevs Standard deviations of the vision pose
* measurement
* in the form [x, y, theta]ᵀ, with units in
* meters and radians.
*/
@Override
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs
) {
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs) {
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds),
visionMeasurementStdDevs);
}
/**
* Return the pose at a given timestamp, if the buffer is not empty.
*
* @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
public Optional<Pose2d> samplePoseAt(double 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;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkMaxConfig;
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 Grimpeur extends SubsystemBase {
ShuffleboardTab teb = Shuffleboard.getTab("teb");
SparkMax grimpeur1 = new SparkMax(3,MotorType.kBrushless);
SparkMax grimpeur2 = new SparkMax(12,MotorType.kBrushless);
SparkMaxConfig slaveConfig = new SparkMaxConfig();
SparkMax moteur1 = new SparkMax(3, MotorType.kBrushless);
SparkMax moteur2 = new SparkMax(12, MotorType.kBrushless);
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);
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() {
teb.addDouble("encodeur grimpeur", this::Position);
teb.addBoolean("limit grimpeur", this::Limit);
teb.addDouble("encodeur grimpeur", encoder1::getPosition);
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
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();
}
}
}

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

@@ -4,48 +4,71 @@
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.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.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 Lanceur extends SubsystemBase {
ShuffleboardTab teb = Shuffleboard.getTab("teb");
SparkFlex moteur1 = new SparkFlex(15, MotorType.kBrushless);
SparkFlex moteur2 = new SparkFlex(18, MotorType.kBrushless);
SparkMax Demeleur = new SparkMax(19, MotorType.kBrushless);
GenericEntry vitesse =
teb.add("vitesse lanceur",4000).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);
}
RelativeEncoder encoder = moteur1.getEncoder();
GenericEntry vitesse = teb.add("vitesse lanceur", 4000).getEntry();
GenericEntry AmpLanceur = teb.add("ampérage lanceur", 30).getEntry();
/** Creates a new Lanceur. */
public Lanceur() {
teb.addDouble("amperage lanceur", this::Amp);
teb.addDouble("vitesse actuelle",this::Vitesse);
teb.addDouble("amperage lanceur", moteur1::getOutputCurrent);
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
@@ -53,4 +76,3 @@ public class Lanceur extends SubsystemBase {
// 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
}
}

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

@@ -1,76 +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(90-((x2 - x1) / (y2 - y1)))* (180 / Math.PI)+270 - angle;
}
else{
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+180 - 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();
}
}
}