diff --git a/src/main/java/frc/robot/Commands/FollowAprilTag.java b/src/main/java/frc/robot/Commands/FollowAprilTag.java index 86ce2bd..89fd4e0 100644 --- a/src/main/java/frc/robot/Commands/FollowAprilTag.java +++ b/src/main/java/frc/robot/Commands/FollowAprilTag.java @@ -32,11 +32,11 @@ public class FollowAprilTag extends Command { if (enlignement.getv()==1) { - drive.drive(0, 0, enlignement.getx()/30); + lanceur.tourelRotation(0,0, enlignement.getx()/30); } else { - drive.drive(0, 0, 0); + lanceur.tourelRotation(0, 0, 0); } } @@ -44,7 +44,7 @@ public class FollowAprilTag extends Command { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - drive.drive(0, 0, 0); + lanceur.tourelRotation(0, 0, 0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/Subsystems/Lanceur.java b/src/main/java/frc/robot/Subsystems/Lanceur.java index 7d73f1d..ecb14c8 100644 --- a/src/main/java/frc/robot/Subsystems/Lanceur.java +++ b/src/main/java/frc/robot/Subsystems/Lanceur.java @@ -36,8 +36,8 @@ public class Lanceur extends SubsystemBase { public void lance(double vitesse){ lanceur1.set(vitesse); } - public void tourelRotation(double vitesse){ - tourelle.set(vitesse); + public void tourelRotation(double x, double y, double rotation){ + tourelle.set(rotation); } public double distancetourel(){ return(tourelle.getEncoder().getPosition());