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

touche, amperage, distance

Browse Source
This commit is contained in:
Antoine PerreaultE 2025-03-03 09:22:48 -05:00
parent 37452f0b05
commit 264bbd003f
17 changed files with 227 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,9 +44,7 @@ import frc.robot.commands.Pince.PinceManuel;
import frc.robot.commands.grimpeur.GrimperHaut;
import frc.robot.commands.grimpeur.GrimpeurBas;
import frc.robot.commands.grimpeur.GrimpeurManuelhaut;
import frc.robot.commands.grimpeur.ResetGrimpeur;
import frc.robot.commands.requin.BalayeuseAlgue;
import frc.robot.commands.requin.BalayeuseBas;
import frc.robot.commands.requin.BalayeuseCoral;
import frc.robot.commands.requin.BalayeuseHaut;
import frc.robot.commands.requin.ExpireCorail;
@ -60,13 +58,16 @@ import frc.robot.subsystems.Limelight3G;
import frc.robot.subsystems.Pince;
import frc.robot.subsystems.Requin;
import frc.robot.commands.requin.exspire;
import frc.robot.commands.Pince.DepartPince;
import frc.robot.commands.Elevateur.balonL2;
import frc.robot.commands.Elevateur.balonL3;
public class RobotContainer {
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
/* Setting up bindings for necessary control of the swerve drive platform */
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
private final SwerveRequest.RobotCentric drive = new SwerveRequest.RobotCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
.withDriveRequestType(DriveRequestType.OpenLoopVoltage
); // Use open-loop control for drive motors
@ -106,34 +107,31 @@ public class RobotContainer {
private void configureBindings() {
drivetrain.registerTelemetry(logger::telemeterize);
drivetrain.setDefaultCommand(
// Drivetrain will execute this command periodically
// Drivetrain will execute this command periodically
drivetrain.applyRequest(() ->
drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftX()*manette1.getLeftX()*manette1.getLeftX()*MaxSpeed, 0.05)) // Drive forward with negative Y (forward)
.withVelocityY(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)
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)
)
);
/* Manette 1 */
// reset the field-centric heading on start press
manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
//pince
manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
manette1.rightBumper().whileTrue(new StationPince(pince, elevateur,bougie));
manette1.leftTrigger().whileTrue(new AlgueExpire(pince, 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));
//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.leftTrigger().whileTrue(new DepartPince(pince));
manette1.povDown().whileTrue(new Algue_inspire(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));
/* Manette 2 */
manette2.a().whileTrue(new CorailAspir(pince,bougie));
manette2.b().whileTrue(new Algue_inspire(pince,bougie));
//requin
manette2.rightBumper().whileTrue(new BalayeuseAlgue(requin,bougie));
manette2.leftBumper().whileTrue(new L1Requin(requin, bougie));
@ -141,12 +139,15 @@ public class RobotContainer {
manette2.leftTrigger().whileTrue(new BalayeuseCoral(requin, bougie));
manette2.y().whileTrue(new exspire(requin, bougie));
manette2.x().whileTrue(new ExpireCorail(requin, bougie));
//limelight
manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
//grimpeur
manette2.povDown().whileTrue(new GrimpeurManuelhaut(Grimpeur, bougie));
manette2.povLeft().whileTrue(new GrimperHaut(Grimpeur, bougie));
manette2.povRight().whileTrue(new GrimpeurBas(Grimpeur));
//Pince manuel
pince.setDefaultCommand(new RunCommand(()->{
pince.pivote(MathUtil.applyDeadband((manette2.getRightY()*manette2.getRightY()*manette2.getRightY())/3, 0.05));
@ -157,8 +158,6 @@ public class RobotContainer {
elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05));
}, elevateur));
//limelight
manette2.povUp().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
//Reset encodeur
manette2.start().whileTrue(new reset(elevateur, pince));
}

15
src/main/java/frc/robot/commands/Elevateur/Depart.java
View File

@ -11,10 +11,12 @@ import frc.robot.subsystems.Pince;
/* 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 Depart extends Command {
private Elevateur elevateur;
private Pince pince;
/** Creates a new L2. */
public Depart(Elevateur elevateur, Pince pince) {
this.elevateur = elevateur;
addRequirements(elevateur);
this.pince = pince;
addRequirements(elevateur, pince);
// Use addRequirements() here to declare subsystem dependencies.
}
@ -25,20 +27,27 @@ public class Depart extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(pince.position()){
pince.pivote(0);
pince.reset();
}
else{
pince.pivote(-0.2);
}
if(elevateur.limit2()==true){
elevateur.vitesse(0);
elevateur.reset();
}
else{
elevateur.vitesse(.3);
elevateur.vitesse(.5);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
elevateur.vitesse(0);
pince.pivote(0);
}
// Returns true when the command should end.

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

@ -26,11 +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()>=15.6 && pince.encodeurpivot()<=16){
if(pince.encodeurpivot()>=16 && pince.encodeurpivot()<=17){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=16){
else if(pince.encodeurpivot()>=17){
pince.pivote(-0.1);
}
else{

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

@ -30,22 +30,23 @@ public class L3 extends Command {
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.5);
elevateur.vitesse(-0.7);
}
else{
elevateur.vitesse(0.5);
elevateur.vitesse(0.25);
}
if(pince.encodeurpivot()>=15.6 && pince.encodeurpivot()<=16){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=16){
pince.pivote(-0.1);
}
else{
pince.pivote(0.1);
}
}
// Called once the command ends or is interrupted.

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

@ -28,24 +28,24 @@ public class L4 extends Command {
@Override
public void execute() {
if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){
elevateur.vitesse(0);
elevateur.vitesse(0);
pince.pivote(-0.1);
if(pince.encodeurpivot()>=22 && pince.encodeurpivot()<=23){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=22){
pince.pivote(-0.15);
}
else{
pince.pivote(0.15);
}
}
else if(elevateur.position()>=elevateur.encodeurelevateurL4bas()){
elevateur.vitesse(-0.5);
elevateur.vitesse(-0.7);
}
else{
elevateur.vitesse(.5);
elevateur.vitesse(.25);
}
if(pince.encodeurpivot()>=21.76 && pince.encodeurpivot()<=22.3){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=22.3){
pince.pivote(-0.1);
}
else{
pince.pivote(0.1);
}
}
// Called once the command ends or is interrupted.

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

@ -28,26 +28,37 @@ public class StationPince extends Command {
// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(pince.emperagecoral() > 60){
pince.aspirecoral(0);bougie.Bleu();
}
else{
pince.aspirecoral(0.5);
}
if(pince.encodeurpivot()<=11 && pince.encodeurpivot()>=12.6){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=9.8){
pince.pivote(-0.1);
if(pince.emperagecoral() >= 12){
pince.x = true;
}
if(pince.x){
pince.aspirecoral(0);
bougie.Bleu();
pince.pivote(-0.2);
if(pince.position()){
pince.pivote(0);
}
}
else{
pince.pivote(0.1);
pince.aspirecoral(0.1);
}
if (!pince.x){
if(pince.encodeurpivot()>=10.5 && pince.encodeurpivot()<=11.5){
pince.pivote(0);
}
else if(pince.encodeurpivot()>=11){
pince.pivote(-0.1);
}
else{
pince.pivote(0.1);
}
}
}
@ -56,6 +67,7 @@ public class StationPince extends Command {
public void end(boolean interrupted) {
pince.pivote(0);
pince.aspirecoral(0);
pince.x =false;
}
// Returns true when the command should end.

49
src/main/java/frc/robot/commands/Elevateur/balonL2.java Normal file
View File

@ -0,0 +1,49 @@
// 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.Elevateur;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Elevateur;
/* 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 balonL2 extends Command {
private Elevateur elevateur;
/** Creates a new L2. */
public balonL2(Elevateur elevateur) {
this.elevateur = elevateur;
addRequirements(elevateur);
// 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(elevateur.position()<=-2 && elevateur.position()>=-2.2){
elevateur.vitesse(0);
}
else if(elevateur.position()>= -1.95){
elevateur.vitesse(-0.7);
}
else{
elevateur.vitesse(0.25);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
elevateur.vitesse(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

49
src/main/java/frc/robot/commands/Elevateur/balonL3.java Normal file
View File

@ -0,0 +1,49 @@
// 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.Elevateur;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Elevateur;
/* 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 balonL3 extends Command {
private Elevateur elevateur;
/** Creates a new L2. */
public balonL3(Elevateur elevateur) {
this.elevateur = elevateur;
addRequirements(elevateur);
// 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(elevateur.position()<=-4 && elevateur.position()>=-4.1){
elevateur.vitesse(0);
}
else if(elevateur.position()>= -4){
elevateur.vitesse(-0.7);
}
else{
elevateur.vitesse(0.25);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
elevateur.vitesse(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}

6
src/main/java/frc/robot/commands/Limelight/AprilTag3G.java
View File

@ -49,8 +49,8 @@ public class AprilTag3G extends Command {
if(limelight3g.getV() == true){
drivetrain.setControl(drive.
withRotationalRate(-a/7).
withVelocityX(x.getAsDouble()).
withVelocityY(y.getAsDouble()));
withVelocityY(x.getAsDouble()).
withVelocityX(-y.getAsDouble()));
System.out.println(a/7);
}
else{
@ -73,6 +73,6 @@ public class AprilTag3G extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return limelight3g.getX()<1 && limelight3g.getX()>-1;
return false;
}
}

1
src/main/java/frc/robot/commands/Pince/Algue_inspire.java
View File

@ -37,7 +37,6 @@ public class Algue_inspire extends Command {
}
else{
pince.aspirealgue(0.5);
}
}

2
src/main/java/frc/robot/commands/Pince/DepartPince.java
View File

@ -29,7 +29,7 @@ public class DepartPince extends Command {
pince.reset();
}
else{
pince.pivote(.2);
pince.pivote(-0.2);
}
}

2
src/main/java/frc/robot/commands/requin/BalayeuseBas.java
View File

@ -24,7 +24,7 @@ public class BalayeuseBas extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
requin.rotationer(0.5);
requin.rotationer(0.2);
}
// Called once the command ends or is interrupted.
@Override

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

@ -29,28 +29,26 @@ public class BalayeuseCoral extends Command {
public void execute() {
if(requin.encodeur()<=1200 && requin.encodeur()>=1025){
requin.rotationer(0);
if(requin.amp()>60){
requin.balaye(0);
bougie.Vert();
if(requin.enHaut()){
requin.rotationer(0);
}
else{
requin.rotationer(-0.5);
}
}
else{
if(requin.amp()>=12){
requin.xRequin = true;
}
if(requin.xRequin){
requin.balaye(0);
bougie.Vert();
}
else{
requin.balaye(0.5);
}
}
else if(requin.encodeur()>=1200){
requin.rotationer(-0.5);
}
else{
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.
@Override

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

@ -29,7 +29,14 @@ public class exspire extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
requin.balaye(0.5);
if(requin.amp()> 15)
{
requin.balaye(0.5);
}
else{
bougie.Rouge();
requin.balaye(0.5);
}
}
// Called once the command ends or is interrupted.

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

@ -18,13 +18,13 @@ public class Elevateur extends SubsystemBase {
private GenericEntry encodeurelevateurL2haut =
teb.add("encodeurelevateurL2haut", -0.9).getEntry();
private GenericEntry encodeurelevateurL3bas =
teb.add("encodeurelevateurL3bas", -1.9).getEntry();
teb.add("encodeurelevateurL3bas", -3).getEntry();
private GenericEntry encodeurelevateurL3haut =
teb.add("encodeurelevateurL3haut", -2.11).getEntry();
teb.add("encodeurelevateurL3haut", -3.1).getEntry();
private GenericEntry encodeurelevateurL4bas =
teb.add("encodeurelevateurL4bas", -6.6).getEntry();
teb.add("encodeurelevateurL4bas", -6.3).getEntry();
private GenericEntry encodeurelevateurL4haut =
teb.add("encodeurelevateurL4haut", -6.2).getEntry();
teb.add("encodeurelevateurL4haut", -6.5).getEntry();
private GenericEntry encodeurelevateurstationbas =
teb.add("encodeurelevateursationbas", -0.5).getEntry();
private GenericEntry encodeurelevateurstationhaut =
@ -67,16 +67,16 @@ public class Elevateur extends SubsystemBase {
return encodeurelevateurL2haut.getDouble(-0.9);
}
public double encodeurelevateurL3bas(){
return encodeurelevateurL3bas.getDouble(-1.9);
return encodeurelevateurL3bas.getDouble(-2.8);
}
public double encodeurelevateurL3haut(){
return encodeurelevateurL3haut.getDouble(-2.11);
return encodeurelevateurL3haut.getDouble(-3);
}
public double encodeurelevateurL4bas(){
return encodeurelevateurL4bas.getDouble(-6.6);
return encodeurelevateurL4bas.getDouble(-6.3);
}
public double encodeurelevateurL4haut(){
return encodeurelevateurL4haut.getDouble(-6.2);
return encodeurelevateurL4haut.getDouble(-6.5);
}
public double encodeurelevateurstationbas(){
return encodeurelevateurstationbas.getDouble(-0.5);

1
src/main/java/frc/robot/subsystems/Pince.java
View File

@ -62,6 +62,7 @@ public double emperagecoral(){
public double emperagealgue(){
return algue1.getOutputCurrent();
}
public boolean x = false;
@Override
public void periodic() {

1
src/main/java/frc/robot/subsystems/Requin.java
View File

@ -43,6 +43,7 @@ public class Requin extends SubsystemBase {
public double amp(){
return balaye.getOutputCurrent();
}
public boolean xRequin = false;
@Override
public void periodic() {
// This method will be called once per scheduler run