// 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.PneumaticsModuleType;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class Grimpeur extends SubsystemBase {
  /** Creates a new Acrocheur. */
  // moteur
  
  ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
  ShuffleboardLayout layout = Shuffleboard.getTab("dashboard")
  .getLayout("grimpeur", BuiltInLayouts.kList)
  .withSize(2,2)
  .withPosition(0,2);
  final CANSparkMax grimpeurd = new CANSparkMax(Constants.grimpeurd,MotorType.kBrushless);
  final CANSparkMax grimpeurg = new CANSparkMax(Constants.grimpeurg,MotorType.kBrushless); 
  // limit switch
  final Solenoid pistondroite= new Solenoid(PneumaticsModuleType.REVPH, Constants.pistondroiteouvre);
  //fonction
  public Grimpeur() {
    pistonouvre();
    layout.addDouble("grimpeurencodeurd", this::encoderd);
    layout.addDouble("grimpeurencodeurg", this::encoderg);
    layout.addDouble("pitchgyrogrimpeur", this::getpitch);
  }
public void droit(double vitesse){
  grimpeurd.set(vitesse);
} 
public void gauche(double vitesse){
  grimpeurg.set(vitesse);
} 
public void resetencodeurd(){
  grimpeurd.getEncoder().setPosition(0);
}
public void resetencodeurg(){
  grimpeurg.getEncoder().setPosition(0);
}
public double encoderd(){
 return grimpeurd.getEncoder().getPosition();
}
public double encoderg(){
  return grimpeurg.getEncoder().getPosition();
}
public AHRS gyroscope = new AHRS();
  public double getpitch(){
    return gyroscope.getPitch();
  }
  public void pistonferme(){
     pistondroite.set(true);
  }
  public void pistonouvre(){
    pistondroite.set(false);
  }
  public boolean piston(){
    return pistondroite.get();
  }

 @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
}