From b0830bbe398cbf4073283eceb49c02bff4d0c499 Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Mon, 19 Feb 2024 19:59:59 -0500 Subject: [PATCH] Commit --- src/main/java/frc/robot/RobotContainer.java | 27 ++++++++-- .../java/frc/robot/subsystem/Lanceur.java | 53 ++++++++++++++++++- 2 files changed, 74 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2429243..5173ae4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,7 +18,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; // Manettes import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; - +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; // Commands import frc.robot.command.AllumeLED; import frc.robot.command.Balayer; @@ -78,7 +78,7 @@ public class RobotContainer { autoChooser = AutoBuilder.buildAutoChooser(); CameraServer.startAutomaticCapture(); - manette.a().whileTrue(new GuiderBas(guideur)); + /*manette.a().whileTrue(new GuiderBas(guideur)); manette.b().whileTrue(new GuiderHaut(guideur)); manette.x().whileTrue(new PistonFerme(grimpeur)); manette.y().whileTrue(new PistonOuvre(grimpeur)); @@ -86,11 +86,11 @@ public class RobotContainer { joystick.button(1).whileTrue(new LancerNote(lanceur, accumulateur)); joystick.button(5).whileTrue(new LancerAmp(lanceur, accumulateur)); joystick.button(4).whileTrue(new ParallelCommandGroup(new Lancer(lanceur,limelight),new Limelight_tracker(limelight,drive))); - joystick.button(2).whileTrue(new ParallelCommandGroup(new Lancerampli(lanceur,limelight),new Limelight_tracker(limelight,drive),new GuiderHaut(guideur))); + joystick.button(2).whileTrue(new ParallelCommandGroup(new Lancerampli(lanceur,limelight),new Limelight_tracker(limelight,drive),new GuiderHaut(guideur)));*/ // deplacement configureBindings(); - drive.setDefaultCommand(new RunCommand(()->{ + /* drive.setDefaultCommand(new RunCommand(()->{ drive.drive(MathUtil.applyDeadband(joystick.getY(),0.2), MathUtil.applyDeadband(joystick.getX(),0.2), MathUtil.applyDeadband(joystick.getZ(), 0.2)); },drive)); @@ -103,7 +103,24 @@ public class RobotContainer { LED.setDefaultCommand(allumeLED); dashboard.add("autochooser",autoChooser) .withSize(2,1) - .withPosition(1,1); + .withPosition(1,1);*/ + manette + .a() + .and(manette.rightBumper()) + .whileTrue(lanceur.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + manette + .b() + .and(manette.rightBumper()) + .whileTrue(lanceur.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + manette + .x() + .and(manette.rightBumper()) + .whileTrue(lanceur.sysIdDynamic(SysIdRoutine.Direction.kForward)); + manette + .y() + .and(manette.rightBumper()) + .whileTrue(lanceur.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } private void configureBindings() {} diff --git a/src/main/java/frc/robot/subsystem/Lanceur.java b/src/main/java/frc/robot/subsystem/Lanceur.java index 60285a4..1e251fc 100644 --- a/src/main/java/frc/robot/subsystem/Lanceur.java +++ b/src/main/java/frc/robot/subsystem/Lanceur.java @@ -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 appliedVoltage = mutable(Volts.of(0)); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutableMeasure angle = mutable(Rotations.of(0)); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutableMeasure> 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 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