diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index cc03486..3ecc1fb 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -1,14 +1,24 @@ 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.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; public class ArmConstants { public static final int kArmMotorID = 0; public static final int kCANcoderID = 0; + public static final int kMotorAmpsMax = 0; + public static final double kArmMaxVelocity = 0; public static final double kPositionalP = 0; @@ -31,11 +41,39 @@ public class ArmConstants { public static final double kArmL3AlgaePosition = 0; public static final double kArmSafeStowPosition = 0; + public static final double kMagnetOffset = 0.0; + public static final double kAbsoluteSensorDiscontinuityPoint = 0.0; + + public static final double kSysIDRampRate = 1; + public static final double kSysIDStepVolts = 7; + public static final double kSysIDTimeout = 10; + + public static final SensorDirectionValue kSensorDirection = SensorDirectionValue.CounterClockwise_Positive; + + public static final IdleMode kIdleMode = IdleMode.kBrake; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIG + + public static final SysIdRoutine.Config kSysIDConfig = new Config( + Volts.of(kSysIDRampRate).per(Second), + Volts.of(kSysIDStepVolts), + Seconds.of(kSysIDTimeout) + ); + public static final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration(); + public static final SparkMaxConfig motorConfig = new SparkMaxConfig(); + static { canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; canCoderConfig.MagnetSensor.MagnetOffset = 0.0; - canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + // TODO Need to do more reading on this setting, and how to properly offset the Arm so that horizontal is 0 + //canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + + motorConfig + .smartCurrentLimit(kMotorAmpsMax) + .idleMode(kIdleMode); } + + } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index a5d1b86..99fca13 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -4,6 +4,8 @@ import java.util.function.DoubleSupplier; import com.ctre.phoenix6.hardware.CANcoder; 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.ArmFeedforward; @@ -14,7 +16,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ArmConstants; public class Arm extends SubsystemBase { - private SparkMax armMotor; + protected SparkMax armMotor; private CANcoder canCoder; @@ -29,6 +31,8 @@ public class Arm extends SubsystemBase { MotorType.kBrushless ); + armMotor.configure(ArmConstants.motorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + positionController = new PIDController( ArmConstants.kPositionalP, ArmConstants.kPositionalI, diff --git a/src/main/java/frc/robot/subsystems/sysid/ArmSysID.java b/src/main/java/frc/robot/subsystems/sysid/ArmSysID.java new file mode 100644 index 0000000..c9d2a9f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/sysid/ArmSysID.java @@ -0,0 +1,65 @@ +package frc.robot.subsystems.sysid; + +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +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.ArmConstants; +import frc.robot.subsystems.Arm; + +public class ArmSysID extends Arm { + private MutVoltage appliedVoltage; + + private MutAngle armPosition; + + private MutAngularVelocity armVelocity; + + private SysIdRoutine routine; + + public ArmSysID() { + super(); + + appliedVoltage = Volts.mutable(0); + + armPosition = Radians.mutable(0); + + armVelocity = RadiansPerSecond.mutable(0); + + routine = new SysIdRoutine( + ArmConstants.kSysIDConfig, + new SysIdRoutine.Mechanism( + armMotor::setVoltage, + (log) -> { + log.motor("armMotor") + .voltage(appliedVoltage.mut_replace( + armMotor.get() * RobotController.getBatteryVoltage(), Volts + )) + .angularPosition(armPosition.mut_replace( + getEncoderPosition(), Radians + )) + .angularVelocity(armVelocity.mut_replace( + getEncoderVelocity(), RadiansPerSecond + )); + }, + this + ) + ); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return routine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return routine.dynamic(direction); + } +}