// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

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;
import frc.robot.Constants;

public class BasePilotable extends SubsystemBase {
     

private WPI_TalonSRX avantGauche = new WPI_TalonSRX(Constants.MoteurAvantGauche);

private WPI_TalonSRX avantDroit = new WPI_TalonSRX(Constants.MoteurAvantDroit);

private WPI_TalonSRX arriereGauche = new WPI_TalonSRX(Constants.MoteurArriereGauche);

private WPI_TalonSRX arriereDroit = new WPI_TalonSRX(Constants.MoteurArriereDroit);

private MecanumDrive mecanum = new MecanumDrive(avantGauche, arriereGauche, avantDroit, arriereDroit);

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() {
    avantDroit.setInverted(true);
    arriereDroit.setInverted(true);
  }
   
  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
  
}