From bcac578e539bd45ad68dbc645ab68eed7c557595 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sat, 17 Feb 2024 16:14:16 -0500 Subject: [PATCH] SysID information gathering setup, no controls setup yet --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/Intake.java | 47 +++++++ .../java/frc/robot/subsystems/Shooter.java | 129 +++++++++++++++++- 3 files changed, 176 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a08c4df..83a4b84 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -136,7 +136,7 @@ public class RobotContainer { intake.setDefaultCommand(intake.manualPivot(operator::getLeftY)); // shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0)); - shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY); + shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY)); climber.setDefaultCommand(climber.stop()); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 5e80fe9..330e445 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -9,8 +9,16 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.RobotController; 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.IntakeConstants; public class Intake extends SubsystemBase{ @@ -23,6 +31,12 @@ public class Intake extends SubsystemBase{ private ArmFeedforward intakeFeedForward; + private MutableMeasure appliedVoltage; + private MutableMeasure distance; + private MutableMeasure> velocity; + + private SysIdRoutine intakePivotRoutine; + public Intake(){ intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless); @@ -52,6 +66,31 @@ public class Intake extends SubsystemBase{ IntakeConstants.kDIntake ); + appliedVoltage = MutableMeasure.mutable(Units.Volts.of(0)); + distance = MutableMeasure.mutable(Units.Meters.of(0)); + velocity = MutableMeasure.mutable(Units.MetersPerSecond.of(0)); + + intakePivotRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + intakePivot.setVoltage(volts.in(Units.Volts)); + }, + (log) -> { + log.motor("intakePivot") + .voltage(appliedVoltage.mut_replace( + intakePivot.get() * RobotController.getBatteryVoltage(), Units.Volts + )) + .linearPosition( + distance.mut_replace(intakeEncoder.getPosition(), Units.Meters) + ) + .linearVelocity( + velocity.mut_replace(intakeEncoder.getVelocity(), Units.MetersPerSecond) + ); + }, + this + ) + ); } public Command intakeDownCommand() { @@ -107,4 +146,12 @@ public class Intake extends SubsystemBase{ ); }); } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return intakePivotRoutine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return intakePivotRoutine.dynamic(direction); + } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 04bfc75..a1a8bb3 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -10,13 +10,27 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; 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.ShooterConstants; -public class Shooter extends SubsystemBase{ +public class Shooter extends SubsystemBase { + public enum ShooterSysIDMode { + PIVOT, + TOP, + BOTTOM + } + private CANSparkMax topShooter; private CANSparkMax bottomShooter; @@ -38,6 +52,14 @@ public class Shooter extends SubsystemBase{ private DigitalInput shooterBeamBreak; + private MutableMeasure appliedVoltage; + private MutableMeasure distance; + private MutableMeasure> velocity; + + private SysIdRoutine shooterPivotRoutine; + private SysIdRoutine shooterTopRoutine; + private SysIdRoutine shooterBottomRoutine; + public Shooter(){ topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless); bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless); @@ -87,6 +109,76 @@ public class Shooter extends SubsystemBase{ ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF ); + + appliedVoltage = MutableMeasure.mutable(Units.Volts.of(0)); + distance = MutableMeasure.mutable(Units.Meters.of(0)); + velocity = MutableMeasure.mutable(Units.MetersPerSecond.of(0)); + + shooterPivotRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + shooterPivot.setVoltage(volts.in(Units.Volts)); + }, + (log) -> { + log.motor("shooterPivot") + .voltage(appliedVoltage.mut_replace( + shooterPivot.get() * RobotController.getBatteryVoltage(), Units.Volts + )) + .linearPosition( + distance.mut_replace(pivotEncoder.getDistance(), Units.Meters) + ) + .linearVelocity( + velocity.mut_replace(pivotEncoder.getRate(), Units.MetersPerSecond) + ); + }, + this + ) + ); + + shooterTopRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + topShooter.setVoltage(volts.in(Units.Volts)); + }, + (log) -> { + log.motor("shooterTop") + .voltage(appliedVoltage.mut_replace( + topShooter.get() * RobotController.getBatteryVoltage(), Units.Volts + )) + .linearPosition( + distance.mut_replace(topEncoder.getPosition(), Units.Meters) + ) + .linearVelocity( + velocity.mut_replace(topEncoder.getVelocity(), Units.MetersPerSecond) + ); + }, + this + ) + ); + + shooterBottomRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + bottomShooter.setVoltage(volts.in(Units.Volts)); + }, + (log) -> { + log.motor("shooterBottom") + .voltage(appliedVoltage.mut_replace( + bottomShooter.get() * RobotController.getBatteryVoltage(), Units.Volts + )) + .linearPosition( + distance.mut_replace(bottomEncoder.getPosition(), Units.Meters) + ) + .linearVelocity( + velocity.mut_replace(bottomEncoder.getVelocity(), Units.MetersPerSecond) + ); + }, + this + ) + ); } public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){ @@ -138,4 +230,39 @@ public class Shooter extends SubsystemBase{ }); } + public Command sysIdQuasistatic(ShooterSysIDMode mode, SysIdRoutine.Direction direction) { + switch(mode) { + case PIVOT: + shooterPivotRoutine.quasistatic(direction); + break; + case TOP: + shooterTopRoutine.quasistatic(direction); + break; + case BOTTOM: + default: + shooterBottomRoutine.quasistatic(direction); + break; + } + + //It'll never get here, because the switch above has a default case + return null; + } + + public Command sysIdDynamic(ShooterSysIDMode mode, SysIdRoutine.Direction direction) { + switch(mode) { + case PIVOT: + shooterPivotRoutine.dynamic(direction); + break; + case TOP: + shooterTopRoutine.dynamic(direction); + break; + case BOTTOM: + default: + shooterBottomRoutine.dynamic(direction); + break; + } + + //It'll never get here, because the switch above has a default case + return null; + } }