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

valeurs encodeurs, sequance, touche, quelque test amperage

Browse Source
This commit is contained in:
Antoine PerreaultE
2025-03-03 20:32:11 -05:00
parent 263caa4d85
commit 9f4142d7aa
15 changed files with 168 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,14 +12,20 @@ import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.commands.PathPlannerAuto;
import com.pathplanner.lib.util.FlippingUtil;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
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.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -81,7 +87,7 @@ public class RobotContainer {
public double getAngle() {
return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot
}
Elevateur elevateur = new Elevateur();
Pince pince = new Pince();
ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY);
@@ -94,44 +100,49 @@ public class RobotContainer {
Requin requin = new Requin();
CorailAspir corailAspir = new CorailAspir(pince, bougie);
public RobotContainer() {
autoChooser = AutoBuilder.buildAutoChooser("New Auto");
SmartDashboard.putData("Auto Mode", autoChooser);
configureBindings();
NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null));
NamedCommands.registerCommand("Station",new StationPince(pince, elevateur,bougie));
NamedCommands.registerCommand("L4", new L4(elevateur, pince));
NamedCommands.registerCommand("L3", new L3(elevateur, pince));
NamedCommands.registerCommand("CoralExpire",new CoralExpire(pince,bougie));
NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie));
NamedCommands.registerCommand("baleeuse",new L1Requin(requin, bougie));
NamedCommands.registerCommand("baleeuse sort", new ExpireCorail(requin, bougie));
}
CameraServer.startAutomaticCapture();
configureBindings();
NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null));
NamedCommands.registerCommand("Station",new StationPince(pince, elevateur,bougie));
NamedCommands.registerCommand("L4", new L4(elevateur, pince));
NamedCommands.registerCommand("L3", new L3(elevateur, pince));
NamedCommands.registerCommand("CoralExpire",new CoralExpire(pince,bougie));
NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie));
NamedCommands.registerCommand("baleeuse",new L1Requin(requin, bougie));
NamedCommands.registerCommand("baleeuse sort", new ExpireCorail(requin, bougie));
autoChooser = AutoBuilder.buildAutoChooser("New Auto");
SmartDashboard.putData("Auto Mode", autoChooser);
}
private void configureBindings() {
drivetrain.registerTelemetry(logger::telemeterize);
drivetrain.setDefaultCommand(
drivetrain.setDefaultCommand(
// Drivetrain will execute this command periodically
drivetrain.applyRequest(() ->
drive.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*-manette1.getLeftX()*-manette1.getLeftX()*MaxSpeed, 0.05)) // Drive forward with negative Y (forward)
.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*-manette1.getLeftY()*-manette1.getLeftY()*MaxSpeed, 0.05)) // Drive left with negative X (left)
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*-manette1.getRightX()*-manette1.getRightX()*MaxAngularRate, 0.05)) // Drive counterclockwise with negative X (left)
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*-manette1.getRightX()*-manette1.getRightX()*MaxAngularRate, 0.05)) // Drive counterclockwise with negative X (left)
)
);
/* Manette 1 */
//pince
manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
manette1.rightBumper().whileTrue(new StationPince(pince, elevateur,bougie));
manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie));
manette1.povLeft().whileTrue(new AlgueExpire(pince, bougie));
manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
manette1.povRight().whileTrue(new CoralExpire(pince, bougie));
manette1.leftTrigger().whileTrue(new DepartPince(pince));
manette1.povDown().whileTrue(new Algue_inspire(pince,bougie));
manette1.leftStick().whileTrue(new CorailAspir(pince, bougie));
//elevateur
manette1.a().whileTrue(new Depart(elevateur, pince));
manette1.b().whileTrue(new L2(elevateur,pince));
manette1.x().whileTrue(new L3(elevateur, pince));
manette1.y().whileTrue(new L4(elevateur, pince));
manette1.povUp().whileTrue(new balonL2(elevateur));
manette1.start().whileTrue(new balonL3(elevateur));
manette1.a().toggleOnTrue(new Depart(elevateur, pince));
manette1.b().toggleOnTrue(new L2(elevateur,pince));
manette1.x().toggleOnTrue(new L3(elevateur, pince));
manette1.y().toggleOnTrue(new L4(elevateur, pince));
manette1.povUp().toggleOnTrue(new balonL2(elevateur));
manette1.start().toggleOnTrue(new balonL3(elevateur));
/* Manette 2 */
//requin
@@ -144,6 +155,7 @@ public class RobotContainer {
//limelight
manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
//grimpeur
manette2.povDown().whileTrue(new GrimpeurManuelhaut(Grimpeur, bougie));
@@ -161,7 +173,7 @@ public class RobotContainer {
}, elevateur));
//Reset encodeur
manette2.start().whileTrue(new reset(elevateur, pince));
manette2.start().whileTrue(new reset(elevateur, pince, requin));
}
public Command getAutonomousCommand() {

7
src/main/java/frc/robot/commands/Elevateur/L2.java
View File

@@ -26,12 +26,11 @@ public class L2 extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(pince.encodeurpivot()>=16 && pince.encodeurpivot()<=17){
if(pince.encodeurpivot()>=14 && pince.encodeurpivot()<=15){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=17){
pince.pivote(-0.1);
else if(pince.encodeurpivot()>=14){
pince.pivote(-0.2);
}
else{
pince.pivote(0.1);

32
src/main/java/frc/robot/commands/Elevateur/L3.java
View File

@@ -28,26 +28,26 @@ public class L3 extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){
elevateur.vitesse(0);
pince.pivote(-0.15);
if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=20){
pince.pivote(-0.15);
}
else{
pince.pivote(0.15);
}
}
else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){
elevateur.vitesse(-0.7);
if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){
elevateur.vitesse(0);
pince.pivote(-0.15);
if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=20){
pince.pivote(-0.15);
}
else{
elevateur.vitesse(0.25);
pince.pivote(0.15);
}
}
else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){
elevateur.vitesse(-0.7);
}
else{
elevateur.vitesse(0.25);
}
}
// Called once the command ends or is interrupted.
@Override

6
src/main/java/frc/robot/commands/Elevateur/L4.java
View File

@@ -29,11 +29,11 @@ public class L4 extends Command {
public void execute() {
if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){
elevateur.vitesse(0);
pince.pivote(-0.1);
if(pince.encodeurpivot()>=22 && pince.encodeurpivot()<=23){
pince.pivote(-0.15);
if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=22){
else if(pince.encodeurpivot()>=20){
pince.pivote(-0.15);
}
else{

28
src/main/java/frc/robot/commands/Elevateur/StationPince.java
View File

@@ -22,7 +22,7 @@ public class StationPince extends Command {
this.elevateur = elevateur;
this.pince = pince;
this.bougie = bougie;
addRequirements(pince,elevateur, bougie);
addRequirements(pince, elevateur, bougie);
// Use addRequirements() here to declare subsystem dependencies.
}
@@ -35,9 +35,18 @@ public class StationPince extends Command {
@Override
public void execute() {
if(pince.emperagecoral() >= 12){
if(pince.emperagecoral() >= 18){
pince.x = true;
}
if(elevateur.position()<=-0.1 && elevateur.position()>= -0.2){
elevateur.vitesse(0);
}
else if(elevateur.position()>=-0.1){
elevateur.vitesse(-0.7);
}
else{
elevateur.vitesse(0.25);
}
if(pince.x){
pince.aspirecoral(0);
bougie.Bleu();
@@ -47,19 +56,21 @@ public class StationPince extends Command {
}
}
else{
pince.aspirecoral(0.1);
pince.aspirecoral(0.25);
}
if (!pince.x){
if(pince.encodeurpivot()>=10.5 && pince.encodeurpivot()<=11.5){
pince.pivote(-0.15);
if(pince.encodeurpivot()>=10 && pince.encodeurpivot()<=11){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=11){
pince.pivote(-0.1);
else if(pince.encodeurpivot()>=10.5){
pince.pivote(-0.15);
}
else{
pince.pivote(0.1);
pince.pivote(0.15);
}
}
}
}
// Called once the command ends or is interrupted.
@@ -67,6 +78,7 @@ public class StationPince extends Command {
public void end(boolean interrupted) {
pince.pivote(0);
pince.aspirecoral(0);
elevateur.vitesse(0);
pince.x =false;
}

4
src/main/java/frc/robot/commands/Pince/CoralExpire.java
View File

@@ -26,13 +26,13 @@ public class CoralExpire extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
/* je crois que ce nest pas necessaire
if(pince.emperagecoral() > 60){
pince.aspirecoral(0);
}
else{
*/
pince.aspirecoral(-.5);
bougie.Jaune();
}
}
// Called once the command ends or is interrupted.

36
src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java
View File

@@ -27,31 +27,28 @@ public class BalayeuseAlgue extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(requin.amp()>=12){
requin.xRequin = true;
}
double cibleMin = 700;
double cibleMax = 900;
if(requin.amp()>=78.2){
requin.xRequin = true;
}
if(requin.xRequin){
bougie.Vert();
requin.balaye(0);
}
else{
requin.balaye(.2);
}
if(!requin.xRequin){
if(requin.encodeur()>=700){
requin.rotationer(-0.5);
}
if(!requin.xRequin){
if(requin.encodeur()<=cibleMax && requin.encodeur()>=cibleMin){
requin.rotationer(0);
requin.balaye(-0.4);
}
else if(requin.encodeur()>=cibleMax){
requin.rotationer(-0.1);
}
else{
requin.rotationer(0.5);
requin.rotationer(0.3);
}
if(requin.encodeur()<=715 && requin.encodeur()>=670){
requin.rotationer(0);
}
}
}
}
// Called once the command ends or is interrupted.
@@ -59,6 +56,7 @@ public class BalayeuseAlgue extends Command {
public void end(boolean interrupted) {
requin.rotationer(0);
requin.balaye(0);
requin.xRequin = false;
}
// Returns true when the command should end.

25
src/main/java/frc/robot/commands/requin/BalayeuseCoral.java
View File

@@ -27,27 +27,27 @@ public class BalayeuseCoral extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(requin.encodeur()<=1200 && requin.encodeur()>=1025){
requin.rotationer(0);
if(requin.amp()>=12){
double cibleMax = 1100;
double cibleMin = 900;
if(requin.amp()>=78.2){
requin.xRequin = true;
}
if(requin.xRequin){
requin.balaye(0);
bougie.Vert();
}
if (!requin.xRequin) {
if(requin.encodeur()<=cibleMax && requin.encodeur()>=cibleMin){
requin.rotationer(0);
requin.balaye(0.7);
}
else if(requin.encodeur()>=cibleMax){
requin.rotationer(-0.5);
}
else{
requin.balaye(0.5);
requin.rotationer(0.5);
}
}
if (!requin.xRequin) {
if(requin.encodeur()>=1200){
requin.rotationer(-0.5);
}
else{
requin.rotationer(0.5);
}
}
}
// Called once the command ends or is interrupted.
@@ -55,6 +55,7 @@ public class BalayeuseCoral extends Command {
public void end(boolean interrupted) {
requin.rotationer(0);
requin.balaye(0);
requin.xRequin = false;
}
// Returns true when the command should end.

12
src/main/java/frc/robot/commands/requin/ExpireCorail.java
View File

@@ -28,14 +28,14 @@ public class ExpireCorail extends Command {
@Override
public void execute() {
if(requin.amp()> 60){
requin.balaye(-0.5);
requin.balaye(-0.4);
}
else
{
bougie.Rouge();
requin.balaye(-0.5);
}
else
{
bougie.Rouge();
requin.balaye(-0.4);
}
}
// Called once the command ends or is interrupted.
@Override

6
src/main/java/frc/robot/commands/requin/L1Requin.java
View File

@@ -27,11 +27,11 @@ public class L1Requin extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(requin.encodeur()<=645 && requin.encodeur()>=600){
bougie.Bleu();
if(requin.encodeur()<=530 && requin.encodeur()>=430){
requin.rotationer(0);
}
else if(requin.encodeur()>=645){
else if(requin.encodeur()>=530){
requin.rotationer(-0.5);
}
else{

4
src/main/java/frc/robot/commands/requin/exspire.java
View File

@@ -31,11 +31,11 @@ public class exspire extends Command {
public void execute() {
if(requin.amp()> 15)
{
requin.balaye(0.5);
requin.balaye(0.4);
}
else{
bougie.Rouge();
requin.balaye(0.5);
requin.balaye(0.4);
}
}

10
src/main/java/frc/robot/commands/reset.java
View File

@@ -7,19 +7,20 @@ package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Elevateur;
import frc.robot.subsystems.Pince;
import frc.robot.subsystems.Requin;
/* 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 reset extends Command {
/** Creates a new reset. */
private Elevateur elevateur;
private Pince pince;
public reset(Elevateur elevateur, Pince pince) {
private Requin requin;
public reset(Elevateur elevateur, Pince pince, Requin requin) {
// Use addRequirements() here to declare subsystem dependencies.
this.elevateur = elevateur;
this.pince = pince;
addRequirements(elevateur,pince);
this.requin = requin;
addRequirements(elevateur,pince, requin);
}
// Called when the command is initially scheduled.
@@ -31,6 +32,7 @@ public class reset extends Command {
public void execute() {
elevateur.reset();
pince.reset();
requin.reset();
}
// Called once the command ends or is interrupted.

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

@@ -41,6 +41,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
private void configureAutoBuilder() {
try {
@@ -58,8 +61,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
new PPHolonomicDriveController(
// PID constants for translation
new PIDConstants(63.167, 0, 0.54521),
// // PID constants for rotation
// new PIDConstants(7.9735, 0, 0.038499)
// PID constants for rotation
new PIDConstants(7.9735, 0, 0.038499)
new PIDConstants(43.502,0,2.7353)
),
config,
// Assume the path needs to be flipped for Red vs Blue, this is normally the case

6
src/main/java/frc/robot/subsystems/Elevateur.java
View File

@@ -29,6 +29,8 @@ public class Elevateur extends SubsystemBase {
teb.add("encodeurelevateursationbas", -0.5).getEntry();
private GenericEntry encodeurelevateurstationhaut =
teb.add("encodeurelevateursationhaut", -0.4).getEntry();
private GenericEntry distanceDeploiePince =
teb.add("encodeurDeploiePince", 0.2).getEntry();
public Elevateur() {
teb.addDouble("encodeur elevateur",this::position);
@@ -84,6 +86,10 @@ public class Elevateur extends SubsystemBase {
public double encodeurelevateurstationhaut(){
return encodeurelevateurstationhaut.getDouble(-0.4);
}
public double distanceDeploiePince(){
return distanceDeploiePince.getDouble(0.2);
}
@Override
public void periodic() {