diff --git a/src/main/java/frc/robot/subsystems/BasePilotable.java b/src/main/java/frc/robot/subsystems/BasePilotable.java index 156cc09..b39f27b 100644 --- a/src/main/java/frc/robot/subsystems/BasePilotable.java +++ b/src/main/java/frc/robot/subsystems/BasePilotable.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.drive.MecanumDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -23,8 +24,12 @@ private WPI_TalonSRX arriereDroit = new WPI_TalonSRX(Constants.MoteurArriereDroi private MecanumDrive mecanum = new MecanumDrive(avantGauche, arriereGauche, avantDroit, arriereDroit); -private void drive(double y, double x, double rot){ - mecanum.driveCartesian(y, x, rot, 0); +private AHRS gyroscope = new AHRS(); +public double getangle() { + return gyroscope.getAngle(); +} +public void drive(double y, double x, double rot){ + mecanum.driveCartesian(y, x, rot, getangle()); } /** Creates a new BasePilotable. */ public BasePilotable() { @@ -36,4 +41,5 @@ private void drive(double y, double x, double rot){ public void periodic() { // This method will be called once per scheduler run } + }