This commit is contained in:
Olivier Dubois 2023-02-22 19:10:44 -05:00
commit b7ea7412d9
2 changed files with 8 additions and 4 deletions

View File

@ -75,6 +75,10 @@ public RobotContainer() {
manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince)); manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince));
manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable)); manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable));
manette1.y().whileTrue(gyro); manette1.y().whileTrue(gyro);
<<<<<<< HEAD
// manette1.b().toggleOnTrue(Commands.startEnd(gratte::baiser, gratte::Lever,gratte));
=======
>>>>>>> 4b22cac30fd2b3c5e7b1af3c7f890c4def2d21f7
manette1.start().toggleOnTrue(Commands.startEnd(basePilotable::resetGyro, basePilotable::resetGyro, basePilotable)); manette1.start().toggleOnTrue(Commands.startEnd(basePilotable::resetGyro, basePilotable::resetGyro, basePilotable));
manette1.b().toggleOnTrue(Commands.startEnd(gratte::baiser, gratte::Lever,gratte)); manette1.b().toggleOnTrue(Commands.startEnd(gratte::baiser, gratte::Lever,gratte));
manette2.a().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasHaut, brasTelescopique::pivoteBrasHaut)); manette2.a().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasHaut, brasTelescopique::pivoteBrasHaut));

View File

@ -19,11 +19,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants; import frc.robot.Constants;
public class BasePilotable extends SubsystemBase { public class BasePilotable extends SubsystemBase {
final CANSparkMax avantdroit = new CANSparkMax(Constants.avantdroit, MotorType.kBrushless); final CANSparkMax avantd = new CANSparkMax(Constants.avantdroit, MotorType.kBrushless);
final CANSparkMax avantgauche = new CANSparkMax(Constants.avantgauche, MotorType.kBrushless); final CANSparkMax avantgauche = new CANSparkMax(Constants.avantgauche, MotorType.kBrushless);
final CANSparkMax arrieredroit = new CANSparkMax(Constants.arrieredroit, MotorType.kBrushless); final CANSparkMax arrieredroit = new CANSparkMax(Constants.arrieredroit, MotorType.kBrushless);
final CANSparkMax arrieregauche = new CANSparkMax(Constants.arrieregauche, MotorType.kBrushless); final CANSparkMax arrieregauche = new CANSparkMax(Constants.arrieregauche, MotorType.kBrushless);
final MotorControllerGroup droit = new MotorControllerGroup(avantdroit, arrieredroit); final MotorControllerGroup droit = new MotorControllerGroup(avantd, arrieredroit);
final MotorControllerGroup gauche = new MotorControllerGroup(avantgauche, arrieregauche); final MotorControllerGroup gauche = new MotorControllerGroup(avantgauche, arrieregauche);
final DifferentialDrive drive = new DifferentialDrive(gauche, droit); final DifferentialDrive drive = new DifferentialDrive(gauche, droit);
@ -41,13 +41,13 @@ public class BasePilotable extends SubsystemBase {
drive.arcadeDrive(xSpeed, zRotation); drive.arcadeDrive(xSpeed, zRotation);
} }
public double distance(){ public double distance(){
return (-avantdroit.getEncoder().getPosition() return (-avantd.getEncoder().getPosition()
+avantgauche.getEncoder().getPosition() +avantgauche.getEncoder().getPosition()
-arrieredroit.getEncoder().getPosition() -arrieredroit.getEncoder().getPosition()
+arrieregauche.getEncoder().getPosition()) / 4; +arrieregauche.getEncoder().getPosition()) / 4;
} }
public void Reset() { public void Reset() {
avantdroit.getEncoder().setPosition(0); avantd.getEncoder().setPosition(0);
avantgauche.getEncoder().setPosition(0); avantgauche.getEncoder().setPosition(0);
arrieredroit.getEncoder().setPosition(0); arrieredroit.getEncoder().setPosition(0);
arrieregauche.getEncoder().setPosition(0); arrieregauche.getEncoder().setPosition(0);