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..bc439f0 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -1,8 +1,23 @@ 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.units.measure.Velocity; +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 +25,31 @@ 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/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index baf11b4..6e78bec 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,13 @@ 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/sysid/ElevatorSysID.java b/src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java new file mode 100644 index 0000000..6d3b70d --- /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); + } +}