102 lines
3.4 KiB
Java
102 lines
3.4 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.subsystems;
|
|
import edu.wpi.first.networktables.GenericEntry;
|
|
import edu.wpi.first.wpilibj.DigitalInput;
|
|
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
|
|
|
import com.revrobotics.spark.SparkMax;
|
|
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
|
public class Elevateur extends SubsystemBase {
|
|
ShuffleboardTab teb = Shuffleboard.getTab("teb");
|
|
/** Creates a new Elevateur. */
|
|
private GenericEntry encodeurelevateurL2bas =
|
|
teb.add("encodeurelevateurL2bas", -1).getEntry();
|
|
private GenericEntry encodeurelevateurL2haut =
|
|
teb.add("encodeurelevateurL2haut", -0.9).getEntry();
|
|
private GenericEntry encodeurelevateurL3bas =
|
|
teb.add("encodeurelevateurL3bas", -3).getEntry();
|
|
private GenericEntry encodeurelevateurL3haut =
|
|
teb.add("encodeurelevateurL3haut", -3.1).getEntry();
|
|
private GenericEntry encodeurelevateurL4bas =
|
|
teb.add("encodeurelevateurL4bas", -6.3).getEntry();
|
|
private GenericEntry encodeurelevateurL4haut =
|
|
teb.add("encodeurelevateurL4haut", -6.5).getEntry();
|
|
private GenericEntry encodeurelevateurstationbas =
|
|
teb.add("encodeurelevateursationbas", -0.5).getEntry();
|
|
private GenericEntry encodeurelevateurstationhaut =
|
|
teb.add("encodeurelevateursationhaut", -0.4).getEntry();
|
|
private GenericEntry distanceDeploiePince =
|
|
teb.add("encodeurDeploiePince", 0.2).getEntry();
|
|
public Elevateur() {
|
|
teb.addDouble("encodeur elevateur",this::position);
|
|
teb.addBoolean("limit elevateur", this::limit2);
|
|
teb.addDouble("amperage elevateur", this::amp);
|
|
}
|
|
final SparkMax monte = new SparkMax(22, MotorType.kBrushless);
|
|
final DigitalInput limit2 = new DigitalInput(0);
|
|
public double position(){
|
|
return monte.getEncoder().getPosition();
|
|
}
|
|
|
|
public void vitesse(double vitesse){
|
|
if (limit2()) {
|
|
if (vitesse > 0) {
|
|
monte.set(0);
|
|
}
|
|
else{
|
|
monte.set(vitesse);
|
|
}
|
|
}
|
|
else{
|
|
monte.set(vitesse);
|
|
}
|
|
}
|
|
public boolean limit2(){
|
|
return limit2.get();
|
|
}
|
|
public void reset(){
|
|
monte.getEncoder().setPosition(0);
|
|
}
|
|
public double encodeurelevateurL2bas(){
|
|
return encodeurelevateurL2bas.getDouble(-1);
|
|
}
|
|
public double encodeurelevateurL2haut(){
|
|
return encodeurelevateurL2haut.getDouble(-0.9);
|
|
}
|
|
public double encodeurelevateurL3bas(){
|
|
return encodeurelevateurL3bas.getDouble(-2.8);
|
|
}
|
|
public double encodeurelevateurL3haut(){
|
|
return encodeurelevateurL3haut.getDouble(-3);
|
|
}
|
|
public double encodeurelevateurL4bas(){
|
|
return encodeurelevateurL4bas.getDouble(-6.3);
|
|
}
|
|
public double encodeurelevateurL4haut(){
|
|
return encodeurelevateurL4haut.getDouble(-6.5);
|
|
}
|
|
public double encodeurelevateurstationbas(){
|
|
return encodeurelevateurstationbas.getDouble(-0.5);
|
|
}
|
|
public double encodeurelevateurstationhaut(){
|
|
return encodeurelevateurstationhaut.getDouble(-0.4);
|
|
}
|
|
public double distanceDeploiePince(){
|
|
return distanceDeploiePince.getDouble(0.2);
|
|
}
|
|
public double amp(){
|
|
return monte.getOutputCurrent();
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
// This method will be called once per scheduler run
|
|
}
|
|
}
|