PLS5618/robot-2024
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Commit

Browse Source
This commit is contained in:
samuel desharnais
2024-02-19 19:59:59 -05:00
parent e0ee680ff7
commit b0830bbe39
2 changed files with 74 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
src/main/java/frc/robot/subsystem/Lanceur.java
View File

@@ -7,11 +7,22 @@ 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");
@@ -32,6 +43,11 @@ public class Lanceur extends SubsystemBase {
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 pidspeaker(){
lanceur1.getPIDController().setP(0.5);
lanceur1.getPIDController().setI(0.5);
@@ -46,6 +62,30 @@ public class Lanceur extends SubsystemBase {
lanceur4.getPIDController().setI(0.5);
lanceur4.getPIDController().setD(0.5);
}
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) -> {
lanceur1.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(
lanceur1.get() * RobotController.getBatteryVoltage(), Volts))
.angularPosition(angle.mut_replace(lanceur1.getEncoder().getPosition(), Rotations))
.angularVelocity(
velocity.mut_replace(lanceur1.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);
@@ -64,7 +104,18 @@ public class Lanceur extends SubsystemBase {
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