// 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.subsystem;

import com.kauailabs.navx.frc.AHRS;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class Grimpeur extends SubsystemBase {
  /** Creates a new Acrocheur. */
  // moteur
  public Grimpeur() {}
  final CANSparkMax grimpeurd = new CANSparkMax(Constants.grimpeurd,MotorType.kBrushless);
  final CANSparkMax grimpeurg = new CANSparkMax(Constants.grimpeurg,MotorType.kBrushless); 
  // limit switch
  final DigitalInput limitdroite = new DigitalInput(Constants.limithaut);
  final DigitalInput limitgauche = new DigitalInput(Constants.limitbas);
  //fonction
public void droit(double vitesse){
  grimpeurd.set(vitesse);
} 
public void gauche(double vitesse){
  grimpeurg.set(vitesse);
} 
public boolean droite(){
  return limitdroite.get();
}
public boolean gauche(){
  return limitgauche.get();
}
public void resetencodeurd(){
  grimpeurd.getEncoder().setPosition(0);
}
public void resetencodeurg(){
  grimpeurg.getEncoder().setPosition(0);
}
public void grimpeur(){
  grimpeurg.follow(grimpeurd);
}
public AHRS gyroscope = new AHRS();
  public double getpitch(){
    return gyroscope.getPitch();
  }
  @Override
  public void periodic() {
    // This method will be called once per scheduler run
    if(droite()) {
      resetencodeurd();
    }
    if(gauche()) {
      resetencodeurg();
    }
  }
}