diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 923373b..9893719 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.commands.L4; import frc.robot.commands.PinceManuel; import frc.robot.commands.PinceManuel2; import frc.robot.commands.StationPince; +import frc.robot.commands.reset; import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Pince; import frc.robot.subsystems.Bougie; @@ -42,7 +43,7 @@ public class RobotContainer { public RobotContainer() { configureBindings(); elevateur.setDefaultCommand(new RunCommand(()->{ - elevateur.vitesse(MathUtil.applyDeadband(-manette2.getLeftY(), 0.2)); + elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.2)); }, elevateur)); pince.setDefaultCommand(new RunCommand(()->{ pince.pivote(MathUtil.applyDeadband(manette2.getRightY()/5, 0.2)); @@ -72,6 +73,7 @@ public class RobotContainer { manette2.povLeft().whileTrue(new Algue1Test(pince)); manette2.povRight().whileTrue(new Algue2Test(pince)); manette2.x().whileTrue(new CorailTest(pince)); + manette2.start().whileTrue(new reset(elevateur)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Depart.java b/src/main/java/frc/robot/commands/Depart.java index 4d53236..f5322dd 100644 --- a/src/main/java/frc/robot/commands/Depart.java +++ b/src/main/java/frc/robot/commands/Depart.java @@ -32,17 +32,9 @@ public class Depart extends Command { elevateur.reset(); } else{ - elevateur.vitesse(-.5); - } - if(pince.encodeurpivot()>=900 && pince.encodeurpivot()<=910){ - pince.pivote(0); - } - else if(pince.encodeurpivot()>=910){ - pince.pivote(-0.5); - } - else{ - pince.pivote(0.5); + elevateur.vitesse(.5); } + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/L2.java b/src/main/java/frc/robot/commands/L2.java index abae929..0cc7048 100644 --- a/src/main/java/frc/robot/commands/L2.java +++ b/src/main/java/frc/robot/commands/L2.java @@ -32,7 +32,7 @@ public class L2 extends Command { elevateur.vitesse(0); } else if(elevateur.position()>=510){ - elevateur.vitesse(-0.3); + elevateur.vitesse(0.3); } else{ elevateur.vitesse(.3); @@ -42,10 +42,10 @@ public class L2 extends Command { } else if(pince.encodeurpivot()>=510){ - pince.pivote(-0.2); + pince.pivote(0.2); } else{ - pince.pivote(0.2); + pince.pivote(-0.2); } } diff --git a/src/main/java/frc/robot/commands/L3.java b/src/main/java/frc/robot/commands/L3.java index 8786079..a0fc189 100644 --- a/src/main/java/frc/robot/commands/L3.java +++ b/src/main/java/frc/robot/commands/L3.java @@ -32,19 +32,19 @@ public class L3 extends Command { elevateur.vitesse(0); } else if(elevateur.position()>=510){ - elevateur.vitesse(-0.5); + elevateur.vitesse(0.5); } else{ - elevateur.vitesse(.5); + elevateur.vitesse(-.5); } if(pince.encodeurpivot()>=700 && pince.encodeurpivot()<=710){ pince.pivote(0); } else if(pince.encodeurpivot()>=710){ - pince.pivote(-0.2); + pince.pivote(0.2); } else{ - pince.pivote(0.2); + pince.pivote(-0.2); } } diff --git a/src/main/java/frc/robot/commands/L4.java b/src/main/java/frc/robot/commands/L4.java index 7794fc3..5bec009 100644 --- a/src/main/java/frc/robot/commands/L4.java +++ b/src/main/java/frc/robot/commands/L4.java @@ -32,7 +32,7 @@ public class L4 extends Command { elevateur.vitesse(0); } else if(elevateur.position()>=810){ - elevateur.vitesse(-0.5); + elevateur.vitesse(0.5); } else{ elevateur.vitesse(.5); @@ -41,10 +41,10 @@ public class L4 extends Command { pince.pivote(0); } else if(pince.encodeurpivot()>=810){ - pince.pivote(-0.2); + pince.pivote(0.2); } else{ - pince.pivote(0.2); + pince.pivote(-0.2); } } diff --git a/src/main/java/frc/robot/commands/reset.java b/src/main/java/frc/robot/commands/reset.java new file mode 100644 index 0000000..f49fe97 --- /dev/null +++ b/src/main/java/frc/robot/commands/reset.java @@ -0,0 +1,41 @@ +// 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.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 reset extends Command { + /** Creates a new reset. */ + private Elevateur elevateur; + public reset(Elevateur elevateur) { + // Use addRequirements() here to declare subsystem dependencies. + this.elevateur = elevateur; + addRequirements(elevateur); + } + + // 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() { + elevateur.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; + } +}