diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4fe2fad..1b29981 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.GrimperHaut; import frc.robot.commands.GrimpeurBas; import frc.robot.commands.GrimpeurManuel; +import frc.robot.commands.ResetGrimpeur; import frc.robot.subsystems.Bougie; import frc.robot.subsystems.Grimpeur; @@ -20,20 +21,25 @@ public class RobotContainer { CommandXboxController manette2 = new CommandXboxController(1); Grimpeur grimpeur = new Grimpeur(); Bougie bougie = new Bougie(); - GrimpeurManuel grimpeurManuel = new GrimpeurManuel(grimpeur, manette1::getLeftY); + GrimpeurManuel grimpeurManuel = new GrimpeurManuel(grimpeur, manette2::getLeftY); public RobotContainer() { grimpeur.setDefaultCommand(new RunCommand(()->{ - grimpeur.grimpe(MathUtil.applyDeadband(manette2.getLeftY(), 0.2)); - }, grimpeur)); + if(grimpeur.stop()){ + grimpeur.grimpe(0); + } + else{ + grimpeur.grimpe(MathUtil.applyDeadband(manette2.getLeftY(), 0.2)); + }}, grimpeur)); configureBindings(); } - + private void configureBindings() { - manette1.leftBumper().whileTrue(new GrimperHaut(grimpeur,bougie)); - manette1.rightBumper().whileTrue(new GrimpeurBas(grimpeur)); + manette2.leftBumper().whileTrue(new GrimperHaut(grimpeur,bougie)); + manette2.rightBumper().whileTrue(new GrimpeurBas(grimpeur)); + manette2.a().whileTrue(new ResetGrimpeur(grimpeur)); } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/GrimpeurBas.java b/src/main/java/frc/robot/commands/GrimpeurBas.java index 73cd93f..d6d27f7 100644 --- a/src/main/java/frc/robot/commands/GrimpeurBas.java +++ b/src/main/java/frc/robot/commands/GrimpeurBas.java @@ -24,13 +24,13 @@ public class GrimpeurBas extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(grimpeur.encodeur()>=500 && grimpeur.encodeur()<=510){ + if(grimpeur.encodeur()>=-38.5 && grimpeur.encodeur()<=-39.19){ grimpeur.grimpe(0); } - else if(grimpeur.encodeur()>=510){ - grimpeur.grimpe(0.5); + else if(grimpeur.encodeur()>=-38.5){ + grimpeur.grimpe(-0.5); } - else{grimpeur.grimpe(-0.5); + else{grimpeur.grimpe(0.5); } } diff --git a/src/main/java/frc/robot/commands/ResetGrimpeur.java b/src/main/java/frc/robot/commands/ResetGrimpeur.java new file mode 100644 index 0000000..b4e6978 --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetGrimpeur.java @@ -0,0 +1,39 @@ +// 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 ResetGrimpeur extends Command { + private Grimpeur grimpeur; + /** Creates a new ResetGrimpeur. */ + public ResetGrimpeur(Grimpeur grimpeur) { + this.grimpeur = grimpeur; + addRequirements(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() { + grimpeur.reset(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Grimpeur.java b/src/main/java/frc/robot/subsystems/Grimpeur.java index d8c06d5..759c4b3 100644 --- a/src/main/java/frc/robot/subsystems/Grimpeur.java +++ b/src/main/java/frc/robot/subsystems/Grimpeur.java @@ -8,14 +8,19 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; 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; public class Grimpeur extends SubsystemBase { /** Creates a new Grimpeur. */ - public Grimpeur() {} + ShuffleboardTab teb = Shuffleboard.getTab("teb"); + public Grimpeur() { + teb.addBoolean("limit grimpeur", this::stop); + teb.addDouble("encodeur grimpeur", this::encodeur); + } final SparkMax grimpeur = new SparkMax(21,MotorType.kBrushless); - final DigitalInput limit1 = new DigitalInput(3); + final DigitalInput limit1 = new DigitalInput(2); public void grimpe(double vitesse){ grimpeur.set(vitesse); }