From 865ba29152eaa8da7df87cd4626730325012fa64 Mon Sep 17 00:00:00 2001 From: NoahLacks63 Date: Mon, 13 Jan 2025 13:42:48 +0000 Subject: [PATCH] minor subsystem changes --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/constants/ArmConstants.java | 2 + .../constants/ClimberPivotConstants.java | 2 + .../robot/constants/ElevatorConstants.java | 42 +++++++++++++ src/main/java/frc/robot/subsystems/Arm.java | 6 +- .../frc/robot/subsystems/ClimberPivot.java | 12 ++-- .../java/frc/robot/subsystems/Elevator.java | 17 +++-- .../java/frc/robot/subsystems/Indexer.java | 3 +- .../frc/robot/subsystems/Manipulator.java | 6 +- .../robot/subsystems/sysid/ElevatorSysID.java | 62 +++++++++++++++++++ 10 files changed, 134 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e9bbe31..f3d7d3d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,7 +61,7 @@ public class RobotContainer { ); climberPivot.setDefaultCommand( - climberPivot.goToAngle(0) + climberPivot.goToAngle(0, 1) ); climberRollers.setDefaultCommand( diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 8ab9aa3..1dfe976 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -3,4 +3,6 @@ package frc.robot.constants; public class ArmConstants { public static final int kArmMotorID = 0; public static final int kCANcoderID = 0; + + public static final double kEncoderConversionFactor = 0; } diff --git a/src/main/java/frc/robot/constants/ClimberPivotConstants.java b/src/main/java/frc/robot/constants/ClimberPivotConstants.java index 557d574..0138ad2 100644 --- a/src/main/java/frc/robot/constants/ClimberPivotConstants.java +++ b/src/main/java/frc/robot/constants/ClimberPivotConstants.java @@ -5,6 +5,8 @@ public class ClimberPivotConstants { public static final int kClimberLimitSwitchID = 0; + public static final double kEncoderConversionFactor = 0; + public static final double kPIDControllerP = 0; public static final double kPIDControllerI = 0; public static final double kPIDControllerD = 0; diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 687edf3..deb4332 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -1,8 +1,22 @@ package frc.robot.constants; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Seconds; + +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; + public class ElevatorConstants { public static final int kElevatorMotorID = 0; + public static final double kEncoderConversionFactor = 0; + + public static final int kMotorAmpsMax = 0; + public static final double kPIDControllerP = 0; public static final double kPIDControllerI = 0; public static final double kPIDControllerD = 0; @@ -10,4 +24,32 @@ public class ElevatorConstants { public static final double kFeedForwardS = 0; public static final double kFeedForwardG = 0; public static final double kFeedForwardV = 0; + + + // 1, 7, 10 are the defaults for these, change as necessary + public static final double kSysIDRampRate = 1; + public static final double kSysIDStepVolts = 7; + public static final double kSysIDTimeout = 10; + + // This should be kCoast for SysID, any other time it can be kBrake if you want + public static final IdleMode kIdleMode = IdleMode.kCoast; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM + + public static final SysIdRoutine.Config kSysIDConfig = new Config( + Volts.of(kSysIDRampRate).per(Second), + Volts.of(kSysIDStepVolts), + Seconds.of(kSysIDTimeout) + ); + + public static final SparkMaxConfig motorConfig = new SparkMaxConfig(); + + static { + motorConfig + .smartCurrentLimit(kMotorAmpsMax) + .idleMode(kIdleMode); + motorConfig.encoder + .positionConversionFactor(kEncoderConversionFactor) + .velocityConversionFactor(kEncoderConversionFactor / 60.0); + } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 610930a..66fb977 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ArmConstants; public class Arm extends SubsystemBase { - private SparkMax ArmMotor; + private SparkMax armMotor; private CANcoder canCoder; @@ -19,7 +19,7 @@ public class Arm extends SubsystemBase { private ArmFeedforward feedForward; public Arm() { - ArmMotor = new SparkMax( + armMotor = new SparkMax( ArmConstants.kArmMotorID, MotorType.kBrushless ); @@ -37,7 +37,7 @@ public class Arm extends SubsystemBase { canCoder.getVelocity().getValueAsDouble() ); - ArmMotor.setVoltage(voltsOut); + armMotor.setVoltage(voltsOut); }).until(pidController::atSetpoint).withTimeout(timeout); } } diff --git a/src/main/java/frc/robot/subsystems/ClimberPivot.java b/src/main/java/frc/robot/subsystems/ClimberPivot.java index 694105f..791b6a6 100644 --- a/src/main/java/frc/robot/subsystems/ClimberPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimberPivot.java @@ -1,6 +1,6 @@ package frc.robot.subsystems; -import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -13,7 +13,7 @@ import frc.robot.constants.ClimberPivotConstants; public class ClimberPivot extends SubsystemBase { private SparkMax pivotMotor; - private AbsoluteEncoder neoEncoder; + private RelativeEncoder neoEncoder; private DigitalInput cageLimitSwitch; @@ -25,9 +25,9 @@ public class ClimberPivot extends SubsystemBase { MotorType.kBrushless ); - neoEncoder = pivotMotor.getAbsoluteEncoder(); + neoEncoder = pivotMotor.getEncoder(); - cageLimitSwitch = new DigitalInput(0); + cageLimitSwitch = new DigitalInput(ClimberPivotConstants.kClimberLimitSwitchID); pidController = new PIDController( ClimberPivotConstants.kPIDControllerP, @@ -42,7 +42,7 @@ public class ClimberPivot extends SubsystemBase { }); } - public Command goToAngle(double setpoint) { + public Command goToAngle(double setpoint, double timeout) { return run(() -> { pivotMotor.set( pidController.calculate( @@ -50,7 +50,7 @@ public class ClimberPivot extends SubsystemBase { setpoint ) ); - }); + }).withTimeout(timeout); } public boolean getCageLimitSwitch() { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index baf11b4..b111743 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,7 +1,9 @@ package frc.robot.subsystems; -import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.math.controller.ElevatorFeedforward; @@ -11,9 +13,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ElevatorConstants; public class Elevator extends SubsystemBase { - private SparkMax elevatorMotor1; + protected SparkMax elevatorMotor1; - private AbsoluteEncoder encoder; + protected RelativeEncoder encoder; private PIDController pidController; @@ -25,7 +27,14 @@ public class Elevator extends SubsystemBase { MotorType.kBrushless ); - encoder = elevatorMotor1.getAbsoluteEncoder(); + elevatorMotor1.configure( + ElevatorConstants.motorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + + encoder = elevatorMotor1.getEncoder(); pidController = new PIDController( ElevatorConstants.kPIDControllerP, diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index e114c63..c163749 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -31,7 +31,6 @@ public class Indexer extends SubsystemBase { public Command indexCoral(double speed) { return run(() -> { indexerMotor.set(speed); - }) - .until(indexerBeamBreak::get); + }).until(indexerBeamBreak::get); } } diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java index 361a689..38cea90 100644 --- a/src/main/java/frc/robot/subsystems/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -33,14 +33,12 @@ public class Manipulator extends SubsystemBase { public Command runUntilCollected(double speed, boolean coral) { return run(() -> { manipulatorMotor.set(coral ? speed : speed * -1); - }) - .until(() -> coralBeamBreak.get() || algaeBeamBreak.get()); + }).until(() -> coralBeamBreak.get() || algaeBeamBreak.get()); } public Command indexCoral(double speed) { return run(() -> { manipulatorMotor.set(speed); - }) - .until(coralBeamBreak::get); + }).until(coralBeamBreak::get); } } diff --git a/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java new file mode 100644 index 0000000..827407e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java @@ -0,0 +1,62 @@ +package frc.robot.subsystems.sysid; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.constants.ElevatorConstants; +import frc.robot.subsystems.Elevator; + +public class ElevatorSysID extends Elevator { + private MutVoltage appliedVoltage; + + private MutDistance elevatorPosition; + + private MutLinearVelocity elevatorVelocity; + + private SysIdRoutine routine; + + public ElevatorSysID() { + super(); + + appliedVoltage = Volts.mutable(0); + + elevatorPosition = Meters.mutable(0); + + elevatorVelocity = MetersPerSecond.mutable(0); + + routine = new SysIdRoutine( + ElevatorConstants.kSysIDConfig, + new SysIdRoutine.Mechanism( + elevatorMotor1::setVoltage, + (log) -> { + log.motor("elevatorMotor1") + .voltage(appliedVoltage.mut_replace( + elevatorMotor1.get() * RobotController.getBatteryVoltage(), Volts + )) + .linearPosition(elevatorPosition.mut_replace( + encoder.getPosition(), Meters + )) + .linearVelocity(elevatorVelocity.mut_replace( + encoder.getVelocity(), MetersPerSecond + )); + }, + this + ) + ); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return routine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return routine.dynamic(direction); + } +} \ No newline at end of file