58 lines
1.6 KiB
Java
58 lines
1.6 KiB
Java
// 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 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();
|
|
}
|
|
}
|
|
}
|