119 lines
4.8 KiB
Java
119 lines
4.8 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.revrobotics.CANSparkMax;
|
|
import com.revrobotics.CANSparkLowLevel.MotorType;
|
|
import edu.wpi.first.networktables.GenericEntry;
|
|
import edu.wpi.first.units.Angle;
|
|
import edu.wpi.first.units.Measure;
|
|
import edu.wpi.first.units.MutableMeasure;
|
|
import edu.wpi.first.units.Velocity;
|
|
import edu.wpi.first.units.Voltage;
|
|
import edu.wpi.first.wpilibj.RobotController;
|
|
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
|
import frc.robot.Constants;
|
|
import static edu.wpi.first.units.Units.Volts;
|
|
import static edu.wpi.first.units.Units.Rotations;
|
|
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
|
import static edu.wpi.first.units.MutableMeasure.mutable;
|
|
public class Lanceur extends SubsystemBase {
|
|
/** Creates a new Lanceur. */
|
|
ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
|
|
private GenericEntry vitesse =
|
|
dashboard.add("vitesse", 0.8)
|
|
.withSize(1, 1)
|
|
.withPosition(2,1)
|
|
.getEntry();
|
|
private GenericEntry vitesseamp =
|
|
dashboard.add("vitesseamp", 0.1)
|
|
.withSize(1, 1)
|
|
.withPosition(2,3)
|
|
.getEntry();
|
|
public Lanceur() {
|
|
|
|
}
|
|
final CANSparkMax lanceur1 = new CANSparkMax(Constants.lanceur, MotorType.kBrushless);
|
|
final CANSparkMax lanceur2 = new CANSparkMax(Constants.lancer2, MotorType.kBrushless);
|
|
final CANSparkMax lanceur3 = new CANSparkMax(Constants.lancer3, MotorType.kBrushless);
|
|
final CANSparkMax lanceur4 = new CANSparkMax(Constants.lancer4, MotorType.kBrushless);
|
|
private final MutableMeasure<Voltage> appliedVoltage = mutable(Volts.of(0));
|
|
// Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
|
|
private final MutableMeasure<Angle> angle = mutable(Rotations.of(0));
|
|
// Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
|
|
private final MutableMeasure<Velocity<Angle>> velocity = mutable(RotationsPerSecond.of(0));
|
|
public void pid(){
|
|
lanceur1.getPIDController().setP(0.000006824);
|
|
lanceur2.getPIDController().setP(0.0051335);
|
|
lanceur3.getPIDController().setP(0.00079046);
|
|
lanceur4.getPIDController().setP(0.0000049791);
|
|
}
|
|
private final SysIdRoutine sysIdRoutine =
|
|
new SysIdRoutine(
|
|
// Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
|
|
new SysIdRoutine.Config(),
|
|
new SysIdRoutine.Mechanism(
|
|
// Tell SysId how to plumb the driving voltage to the motor(s).
|
|
(Measure<Voltage> volts) -> {
|
|
lanceur4.setVoltage(volts.in(Volts));
|
|
},
|
|
// Tell SysId how to record a frame of data for each motor on the mechanism being
|
|
// characterized.
|
|
log -> {
|
|
// Record a frame for the shooter motor.
|
|
log.motor("shooter-wheel")
|
|
.voltage(
|
|
appliedVoltage.mut_replace(
|
|
lanceur4.getAppliedOutput() * lanceur4.getBusVoltage(), Volts))
|
|
.angularPosition(angle.mut_replace(lanceur4.getEncoder().getPosition(), Rotations))
|
|
.angularVelocity(
|
|
velocity.mut_replace(lanceur4.getEncoder().getVelocity(), RotationsPerSecond));
|
|
},
|
|
// Tell SysId to make generated commands require this subsystem, suffix test state in
|
|
// WPILog with this subsystem's name ("shooter")
|
|
this));
|
|
public void lancer(double vitesse){
|
|
lanceur1.set(-vitesse);
|
|
lanceur2.set(-vitesse);
|
|
lanceur3.set(vitesse);
|
|
lanceur4.set(-vitesse);
|
|
}
|
|
public void lancerspeaker(){
|
|
lancer(vitesse.getDouble(0.8));
|
|
}
|
|
public void Lancertrape(){
|
|
lancer(vitesse.getDouble(0.8));
|
|
}
|
|
public void lanceramp(){
|
|
lancer(vitesseamp.getDouble(0.1));
|
|
}
|
|
public double vitesse(double vitesse){
|
|
return lanceur3.getEncoder().getVelocity();
|
|
}
|
|
public void lanceur(){
|
|
|
|
}
|
|
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
|
|
return sysIdRoutine.quasistatic(direction);
|
|
}
|
|
|
|
/**
|
|
* Returns a command that will execute a dynamic test in the given direction.
|
|
*
|
|
* @param direction The direction (forward or reverse) to run the test in
|
|
*/
|
|
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
|
|
return sysIdRoutine.dynamic(direction);
|
|
}
|
|
@Override
|
|
public void periodic() {}
|
|
// This method will be called once per scheduler run
|
|
|
|
}
|