102 lines
3.1 KiB
Java
102 lines
3.1 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.networktables.GenericEntry;
|
|
import edu.wpi.first.wpilibj.DigitalInput;
|
|
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
|
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.ShuffleboardTab;
|
|
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
|
|
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");
|
|
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);
|
|
final Solenoid pistondroite= new Solenoid(PneumaticsModuleType.CTREPCM, Constants.pistondroiteouvre);
|
|
final Solenoid pistondgauche = new Solenoid(PneumaticsModuleType.CTREPCM, Constants.pistondgaucheouvre);
|
|
//fonction
|
|
public Grimpeur() {
|
|
dashboard.add("limitgrimpeurd", droite())
|
|
.withSize(1, 1)
|
|
.withPosition(1, 5);
|
|
dashboard.add("limitgrimpeurd", droite())
|
|
.withSize(1, 1)
|
|
.withPosition(1, 4);
|
|
dashboard.add("grimpeurencodeurd", encoderd())
|
|
.withSize(1, 1)
|
|
.withPosition(1, 3);
|
|
dashboard.add("grimpeurencodeurg", encoderg())
|
|
.withSize(1, 1)
|
|
.withPosition(1, 2);
|
|
dashboard.add("pitchgyrogrimpeur", getpitch())
|
|
.withSize(1, 1)
|
|
.withPosition(1, 1);
|
|
}
|
|
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 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);
|
|
pistondgauche.set(true);
|
|
}
|
|
public void pistonouvre(){
|
|
pistondgauche.set(false);
|
|
pistondroite.set(false);
|
|
}
|
|
public boolean piston(){
|
|
return pistondgauche.get();
|
|
}
|
|
@Override
|
|
public void periodic() {
|
|
// This method will be called once per scheduler run
|
|
if(droite()) {
|
|
resetencodeurd();
|
|
}
|
|
if(gauche()) {
|
|
resetencodeurg();
|
|
}
|
|
}
|
|
}
|