diff --git a/src/main/java/frc/robot/command/GrimpeurHaut.java b/src/main/java/frc/robot/command/GrimpeurHaut.java index 840c475..a14a6fb 100644 --- a/src/main/java/frc/robot/command/GrimpeurHaut.java +++ b/src/main/java/frc/robot/command/GrimpeurHaut.java @@ -18,17 +18,46 @@ public class GrimpeurHaut extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + grimpeur.resetencodeurd(); + grimpeur.resetencodeurg(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - + if(grimpeur.droite()){ + grimpeur.resetencodeurd(); + } + else if(grimpeur.gauche()){ + grimpeur.resetencodeurg(); + } + else if(grimpeur.encoderd()>261){ + grimpeur.droit(0); + } + else if(grimpeur.getpitch()<-15){ + grimpeur.droit(0.6); + grimpeur.gauche(-0.6); + } + else if(grimpeur.getpitch()>15){ + grimpeur.gauche(0.6); + grimpeur.droit(-0.6); + } + else if(grimpeur.encoderg()>261){ + grimpeur.gauche(0.6); + } + else{ + grimpeur.gauche(0); + grimpeur.gauche(0); + } } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + grimpeur.droit(0); + grimpeur.gauche(0); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/subsystem/Grimpeur.java b/src/main/java/frc/robot/subsystem/Grimpeur.java index ed02bda..4f595c4 100644 --- a/src/main/java/frc/robot/subsystem/Grimpeur.java +++ b/src/main/java/frc/robot/subsystem/Grimpeur.java @@ -45,7 +45,12 @@ public void resetencodeurd(){ public void resetencodeurg(){ grimpeurg.getEncoder().setPosition(0); } - +public double encoderd(){ + return grimpeurd.getEncoder().getPosition(); +} +public double encoderg(){ + return grimpeurg.getEncoder().getPosition(); +} public AHRS gyroscope = new AHRS(); public double getpitch(){ return gyroscope.getPitch();