Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ElevatorSysID subsystem added plus other minor constant changes

Browse Source
This commit is contained in:
Bradley Bickford 2025-01-11 13:02:36 -05:00
parent d29642b89d
commit 55af6a4cc8
5 changed files with 120 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/java/frc/robot/constants/ArmConstants.java
View File

@ -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;
}

2
src/main/java/frc/robot/constants/ClimberPivotConstants.java
View File

@ -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;

42
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -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);
}
}

16
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -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,

62
src/main/java/frc/robot/subsystems/sysid/ElevatorSysID.java Normal file
View File

@ -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);
}
}