diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a1966a9..8ecee90 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.subsystems.BasePilotable; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -17,12 +19,19 @@ import edu.wpi.first.wpilibj2.command.Command; */ public class RobotContainer { // The robot's subsystems and commands are defined here... - - + XboxController manette = new XboxController(0); + + BasePilotable basePilotable = new BasePilotable(); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the button bindings configureButtonBindings(); + + basePilotable.setDefaultCommand( + new RunCommand( + ()-> basePilotable.drive(manette.getLeftY(), manette.getLeftX(), manette.getLeftTriggerAxis()-manette.getRightTriggerAxis()), basePilotable) + ); } /** 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 } + }