diff --git a/src/main/deploy/pathplanner/autos/baleeuse.auto b/src/main/deploy/pathplanner/autos/baleeuse.auto index e826bd1..c8d8f22 100644 --- a/src/main/deploy/pathplanner/autos/baleeuse.auto +++ b/src/main/deploy/pathplanner/autos/baleeuse.auto @@ -17,21 +17,41 @@ } }, { - "type": "named", + "type": "deadline", "data": { - "name": "baleeuse" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "baleeuse" + } + } + ] } }, { - "type": "wait", + "type": "deadline", "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "baleeuse sort" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "baleeuse sort" + } + } + ] } } ] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d7ab742..05d146d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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() { diff --git a/src/main/java/frc/robot/commands/Elevateur/L2.java b/src/main/java/frc/robot/commands/Elevateur/L2.java index 279cd5e..99b1a3a 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L2.java +++ b/src/main/java/frc/robot/commands/Elevateur/L2.java @@ -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); diff --git a/src/main/java/frc/robot/commands/Elevateur/L3.java b/src/main/java/frc/robot/commands/Elevateur/L3.java index f6634e7..cb940c0 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L3.java +++ b/src/main/java/frc/robot/commands/Elevateur/L3.java @@ -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 diff --git a/src/main/java/frc/robot/commands/Elevateur/L4.java b/src/main/java/frc/robot/commands/Elevateur/L4.java index 99c8499..4b8dcbc 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L4.java +++ b/src/main/java/frc/robot/commands/Elevateur/L4.java @@ -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{ diff --git a/src/main/java/frc/robot/commands/Elevateur/StationPince.java b/src/main/java/frc/robot/commands/Elevateur/StationPince.java index b149051..885c300 100644 --- a/src/main/java/frc/robot/commands/Elevateur/StationPince.java +++ b/src/main/java/frc/robot/commands/Elevateur/StationPince.java @@ -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; } diff --git a/src/main/java/frc/robot/commands/Pince/CoralExpire.java b/src/main/java/frc/robot/commands/Pince/CoralExpire.java index f20ab57..4db46df 100644 --- a/src/main/java/frc/robot/commands/Pince/CoralExpire.java +++ b/src/main/java/frc/robot/commands/Pince/CoralExpire.java @@ -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. diff --git a/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java b/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java index 0cc8453..f6130df 100644 --- a/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java +++ b/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java @@ -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. diff --git a/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java b/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java index d4e7c3b..61a8434 100644 --- a/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java +++ b/src/main/java/frc/robot/commands/requin/BalayeuseCoral.java @@ -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. diff --git a/src/main/java/frc/robot/commands/requin/ExpireCorail.java b/src/main/java/frc/robot/commands/requin/ExpireCorail.java index c206644..3dd81c1 100644 --- a/src/main/java/frc/robot/commands/requin/ExpireCorail.java +++ b/src/main/java/frc/robot/commands/requin/ExpireCorail.java @@ -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 diff --git a/src/main/java/frc/robot/commands/requin/L1Requin.java b/src/main/java/frc/robot/commands/requin/L1Requin.java index 73e3bac..8087309 100644 --- a/src/main/java/frc/robot/commands/requin/L1Requin.java +++ b/src/main/java/frc/robot/commands/requin/L1Requin.java @@ -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{ diff --git a/src/main/java/frc/robot/commands/requin/exspire.java b/src/main/java/frc/robot/commands/requin/exspire.java index 33a0fc0..f3a77c6 100644 --- a/src/main/java/frc/robot/commands/requin/exspire.java +++ b/src/main/java/frc/robot/commands/requin/exspire.java @@ -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); } } diff --git a/src/main/java/frc/robot/commands/reset.java b/src/main/java/frc/robot/commands/reset.java index 9a61223..be304e6 100644 --- a/src/main/java/frc/robot/commands/reset.java +++ b/src/main/java/frc/robot/commands/reset.java @@ -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. diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 6e77bc6..8016a25 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -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 diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index 5b0dc10..1ef7681 100644 --- a/src/main/java/frc/robot/subsystems/Elevateur.java +++ b/src/main/java/frc/robot/subsystems/Elevateur.java @@ -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() {