diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07aa04f..a1f1808 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -100,20 +100,20 @@ public class RobotContainer { drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> - drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*manette1.getLeftY()*MaxSpeed, 0.1)) // Drive forward with negative Y (forward) - .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*manette1.getLeftX()*MaxSpeed, 0.10000)) // Drive left with negative X (left) - .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*manette1.getRightX()*MaxAngularRate, 0.1)) // Drive counterclockwise with negative X (left) + drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftX()*manette1.getLeftX()*manette1.getLeftX()*MaxSpeed, 0.05)) // Drive forward with negative Y (forward) + .withVelocityY(MathUtil.applyDeadband(manette1.getLeftY()*manette1.getLeftY()*manette1.getLeftY()*MaxSpeed, 0.05)) // Drive left with negative X (left) + .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*manette1.getRightX()*manette1.getRightX()*MaxAngularRate, 0.05)) // Drive counterclockwise with negative X (left) ) ); // Elevateur manuel drivetrain.registerTelemetry(logger::telemeterize); elevateur.setDefaultCommand(new RunCommand(()->{ - elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY(), 0.1)); + elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05)); }, elevateur)); //Pince manuel pince.setDefaultCommand(new RunCommand(()->{ - pince.pivote(MathUtil.applyDeadband(manette2.getRightY()*manette2.getRightY(), 0.1)); + pince.pivote(MathUtil.applyDeadband(manette2.getRightY()*manette2.getRightY()*manette2.getRightY(), 0.05)); }, pince)); @@ -130,7 +130,7 @@ public class RobotContainer { //manette2 manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie)); manette2.a().whileTrue(new CorailAspir(pince)); - manette2.start().whileTrue(new reset(elevateur)); + manette2.start().whileTrue(new reset(elevateur,pince)); manette2.b().whileTrue(new Algue_inspire(pince)); manette2.leftBumper().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); manette2.rightBumper().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); diff --git a/src/main/java/frc/robot/commands/reset.java b/src/main/java/frc/robot/commands/reset.java index d6ba79b..9a61223 100644 --- a/src/main/java/frc/robot/commands/reset.java +++ b/src/main/java/frc/robot/commands/reset.java @@ -15,7 +15,7 @@ public class reset extends Command { /** Creates a new reset. */ private Elevateur elevateur; private Pince pince; - public reset(Elevateur elevateur) { + public reset(Elevateur elevateur, Pince pince) { // Use addRequirements() here to declare subsystem dependencies. this.elevateur = elevateur; this.pince = pince;