diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bb49f9f..0e01c85 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,18 +21,26 @@ 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.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.TunerConstants.TunerConstants; import frc.robot.commands.AprilTag3; import frc.robot.commands.AprilTag3G; +import frc.robot.commands.ElevateurManuel; import frc.robot.commands.Forme3; +import frc.robot.commands.L2; +import frc.robot.commands.PinceManuel; +import frc.robot.commands.PinceManuel2; import frc.robot.commands.RainBow; +import frc.robot.commands.reset; import frc.robot.subsystems.Bougie; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Limelight3; import frc.robot.subsystems.Limelight3G; +import frc.robot.subsystems.Pince; public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed @@ -49,7 +57,11 @@ public class RobotContainer { private final CommandXboxController manette2 = new CommandXboxController(1); public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - + Elevateur elevateur = new Elevateur(); + Pince pince = new Pince(); + ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY); + PinceManuel pinceManuel = new PinceManuel(pince); + PinceManuel2 pinceManuel2 = new PinceManuel2(pince); private final SendableChooser autoChooser; Bougie bougie = new Bougie(); Limelight3G limelight3g = new Limelight3G(); @@ -65,8 +77,13 @@ public class RobotContainer { configureBindings(); NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null)); } - private void configureBindings() { + elevateur.setDefaultCommand(new RunCommand(()->{ + elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.2)); + }, elevateur)); + pince.setDefaultCommand(new RunCommand(()->{ + pince.pivote(MathUtil.applyDeadband(manette2.getRightY()/5, 0.2)); + }, pince)); // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. drivetrain.setDefaultCommand( @@ -78,15 +95,19 @@ public class RobotContainer { ) ); + // reset the field-centric heading on left bumper press - manette1.y().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - manette1.a().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - manette1.b().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward)); - manette1.x().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse)); manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); + manette1.a().whileTrue(new reset(elevateur)); + manette1.b().whileTrue(new L2(elevateur, pince)); + + manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); + + + drivetrain.registerTelemetry(logger::telemeterize); } diff --git a/src/main/java/frc/robot/commands/L2.java b/src/main/java/frc/robot/commands/L2.java index 0cc7048..3a201a8 100644 --- a/src/main/java/frc/robot/commands/L2.java +++ b/src/main/java/frc/robot/commands/L2.java @@ -28,25 +28,25 @@ public class L2 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(elevateur.position()>=500 && elevateur.position()<=510){ + if(elevateur.position()<=-1 && elevateur.position()>=-0.9){ elevateur.vitesse(0); } - else if(elevateur.position()>=510){ - elevateur.vitesse(0.3); + else if(elevateur.position()>=-1){ + elevateur.vitesse(-0.2); } else{ - elevateur.vitesse(.3); - } - if(pince.encodeurpivot()>=500 && pince.encodeurpivot()<=510){ - pince.pivote(0); + elevateur.vitesse(.2); + // } + // if(pince.encodeurpivot()>=500 && pince.encodeurpivot()<=510){ + // pince.pivote(0); - } - else if(pince.encodeurpivot()>=510){ - pince.pivote(0.2); - } - else{ - pince.pivote(-0.2); - } + // } + // else if(pince.encodeurpivot()>=510){ + // pince.pivote(0.2); + // } + // else{ + // pince.pivote(-0.2); + } } diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index 49928f3..ef76d3f 100644 --- a/src/main/java/frc/robot/subsystems/Elevateur.java +++ b/src/main/java/frc/robot/subsystems/Elevateur.java @@ -7,17 +7,19 @@ package frc.robot.subsystems; 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.SubsystemBase; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; public class Elevateur extends SubsystemBase { + ShuffleboardTab teb = Shuffleboard.getTab("teb"); /** Creates a new Elevateur. */ - public Elevateur() {} + public Elevateur() { + teb.addDouble("encodeur elevateur",this::position); + } final SparkMax monte = new SparkMax(22, MotorType.kBrushless); final DigitalInput limit2 = new DigitalInput(1); - GenericEntry teb = Shuffleboard.getTab("teb") - .add("elevateur encodeur",position()) - .getEntry(); + public double position(){ return monte.getEncoder().getPosition(); }