diff --git a/.github/workflows/javadoc.yml b/.github/workflows/javadoc.yml index 3ff8c19..abf4b65 100644 --- a/.github/workflows/javadoc.yml +++ b/.github/workflows/javadoc.yml @@ -17,13 +17,14 @@ jobs: - name: Generate Javadoc run: ./gradlew javadoc - name: Build Artifact - uses: actions/upload-pages-artifact@v1 + uses: actions/upload-pages-artifact@v4 with: path: ./build/docs/javadoc deploy-javadoc: needs: build-javadoc if: ${{ github.ref == 'refs/heads/master' }} permissions: + actions: read pages: write id-token: write environment: @@ -33,4 +34,4 @@ jobs: steps: - name: Deploy to GitHub Pages id: deployment - uses: actions/deploy-pages@v1 + uses: actions/deploy-pages@v4 \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/Limelight.java b/src/main/java/org/carlmontrobotics/lib199/Limelight.java index 0aa06a6..ccd44f6 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Limelight.java +++ b/src/main/java/org/carlmontrobotics/lib199/Limelight.java @@ -214,6 +214,11 @@ public Pose3d getTransform(Transform transform) { return new Pose3d(rawData[0], rawData[1], rawData[2], new Rotation3d(Math.toRadians(rawData[3]), Math.toRadians(rawData[4]), Math.toRadians(rawData[5]))); } + /** + * Gets Limelight Network Tables + * @param key id of the NT + * @return the NT + */ public NetworkTableEntry getNTEntry(String key) { return NetworkTableInstance.getDefault().getTable(config.ntName).getEntry(key); } diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java index 8e33230..1604845 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java @@ -17,7 +17,6 @@ import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.servohub.ServoHub.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkFlex; diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java new file mode 100644 index 0000000..0cf591b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java @@ -0,0 +1,350 @@ +package org.carlmontrobotics.lib199.SimpleMechs.Arm; +import org.carlmontrobotics.lib199.MotorConfig; + +import com.revrobotics.AbsoluteEncoder; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DriverStation; + + +public final class ArmConfig { + public final double armRelativeGearReduction, armAbsoluteGearReduction, armPIDTolerance, armMaxVolts; + public final int[] armFollowId; + public final MotorConfig[] armFollowMotorType; + public final boolean[] armFollowInversed; + + public final int armMasterMotorId; + public final MotorConfig armMasterMotorType; + public final boolean armMasterInversed; + + public final double [] armKPID, armKFeedForward; + public double armKP, armKI, armKD, armKS, armKG, armKV, armKA; + + public AbsoluteEncoder armMainAbsoluteEncoder; + + public ArmFeedforward armFeedForward; + public PIDController armPID; + public boolean armPIDContinuousInput; + + public double armAbsoluteZeroOffset; + public double bottomLimit; + public double topLimit; + + public int armMotorOfBackupRelativeEncoderId; + + + /** + * Creates a ArmConfig for a SimpleArm + * @param armRelativeGearReduction Gear reduction from shaft spinning where motors are to the arm axis + * @param armAbsoluteGearReduction Gear reduction from absolute encoder reading to the arm axis + * @param armMaxVolts Max volts that motors can output on to the arm, 14 if no limit + * @param armFollowId List all motorIds for followers + * @param armFollowMotorType List all motor configs for the followers in the same order as above + * @param armFollowInversed List all inverse configs for the followers in the same order as above + * @param armMasterMotorId Master id + * @param armMasterMotorType Motor Config of Master + * @param armMasterInversed Inverse Config of Master + * @param armKPID (kP, kI, kD) of arm + * @param armPIDTolerance Tolerance (in degrees) of arm + * @param armPIDContinuousInput True if arm can do infinite 360s, keep false otherwise! + * @param armKFeedForward (kS, kG, kV, kA) of arm + * @param armMainAbsoluteEncoder null if no absolute + * @param armMotorIDOfBackupRelativeEncoder -1 if none + * @param armAbsoluteZeroOffset zeroOffset (in degrees) of abs enc, 0 should always be horizontal! + * @param bottomLimit lowest value the arm can achieve (soft limit) in degrees with 0 being horizontal + * @param topLimit highest value the arm can achieve (soft limit) in degrees with 0 being horizontal + * @throws IllegalArgumentException if each follow motor does not have an id, motor type, and inverse config + * @throws IllegalArgumentException if the master motor does not have an id, motor type, and inverse config + * @throws IllegalArgumentException if the any of the provided motor types are not one of the 3 expected {@link MotorConfig} + * @throws IllegalArgumentException if the id of the backupEncoder is not part of the provided set of follow motors + * @throws IllegalArgumentException if spinnerKPID is not three values with each being non negative + * @throws IllegalArgumentException if spinnerKFeedForward is not three values with each being non negative + * @throws IllegalArgumentException if gearReduction is 0 + * @throws IllegalArgumentException if maxVolts is not positive + * @throws IllegalArgumentException if the top limit of the arm is lower or equal to the bottom limit of the arm + */ + public ArmConfig( + double armRelativeGearReduction, double armAbsoluteGearReduction, + double armMaxVolts, + int[] armFollowId, MotorConfig[] armFollowMotorType, boolean[] armFollowInversed, + int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, + double[] armKPID, double armPIDTolerance, boolean armPIDContinuousInput, + double[] armKFeedForward, + AbsoluteEncoder armMainAbsoluteEncoder, int armMotorIDOfBackupRelativeEncoder, + double armAbsoluteZeroOffset, + double bottomLimit, + double topLimit + ) { + + this.armRelativeGearReduction = armRelativeGearReduction; + this.armAbsoluteGearReduction = armAbsoluteGearReduction; + this.armFollowId = armFollowId; + this.armFollowMotorType = armFollowMotorType; + this.armFollowInversed = armFollowInversed; + this.armMasterMotorId = armMasterMotorId; + this.armMasterMotorType = armMasterMotorType; + this.armMasterInversed = armMasterInversed; + + this.armMainAbsoluteEncoder = armMainAbsoluteEncoder; + this.armPIDTolerance = armPIDTolerance; + this.armPIDContinuousInput = armPIDContinuousInput; + this.armMaxVolts = armMaxVolts; + this.armKPID = armKPID; + this.armKFeedForward = armKFeedForward; + this.armAbsoluteZeroOffset = armAbsoluteZeroOffset; + this.bottomLimit = bottomLimit; + this.topLimit = topLimit; + this.armMotorOfBackupRelativeEncoderId = armMotorIDOfBackupRelativeEncoder; + + checkRequirements(armMotorIDOfBackupRelativeEncoder); + + } + + + private static double defaultArmPIDTolerance = 2; //degrees + + /** + * Create a single motor arm config with PID, no FeedForward + * @param armMasterMotorId master id + * @param armMasterMotorType {@link MotorConfig} of the master motor + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param armKPID double array with 3 values, kP, kI, kD + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if MotorType is not one of the 3 expected {@link MotorConfig}s + * @throws IllegalArgumentException if armKPID does not have 3 nonnegative values + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit + */ + public static ArmConfig motorWithPID(int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, double[] armKPID, double bottomLimit, double topLimit) { + return new ArmConfig(1, 1, 14, null, null, null, armMasterMotorId, armMasterMotorType, armMasterInversed, armKPID, defaultArmPIDTolerance, false, null, null, -1, 0, bottomLimit, topLimit); + } + + /** + * Create a single motor arm config no PID, no FeedForward + * @param armMasterMotorId master id + * @param armMasterMotorType {@link MotorConfig} of the master motor + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if MotorType is not one of the 3 expected {@link MotorConfig}s + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit + */ + public static ArmConfig motorNoPID(int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, double bottomLimit, double topLimit) { + return motorWithPID(armMasterMotorId, armMasterMotorType, armMasterInversed, null, bottomLimit, topLimit); + } + + /** + * Creates a single NEO arm config with PID + * @param armMasterMotorId master id + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param armKPID double array with 3 values, kP, kI, kD + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if armKPID does not have 3 nonnegative values + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit + */ + public static ArmConfig neoWithPID(int armMasterMotorId, boolean armMasterInversed, double[] armKPID, double bottomLimit, double topLimit) { + return motorWithPID(armMasterMotorId, MotorConfig.NEO, armMasterInversed, armKPID, bottomLimit, topLimit); + } + + /** + * Create a single NEO arm config without PID + * @param armMasterMotorId master id + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit + */ + public static ArmConfig neoNoPID(int armMasterMotorId, boolean armMasterInversed, double bottomLimit, double topLimit) { + return motorNoPID(armMasterMotorId, MotorConfig.NEO, armMasterInversed, bottomLimit, topLimit); + } + + /** + * Createa a single vortex arm config, with PID + * @param armMasterMotorId master id + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param armKPID double array with 3 values, kP, kI, kD + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if armKPID does not have 3 nonnegative values + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit + */ + public static ArmConfig vortexWithPID(int armMasterMotorId, boolean armMasterInversed, double[] armKPID, double bottomLimit, double topLimit) { + return motorWithPID(armMasterMotorId, MotorConfig.NEO_VORTEX, armMasterInversed, armKPID, bottomLimit, topLimit); + } + + /** + * Create a single vortex arm config, no PID + * @param armMasterMotorId master id + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit + */ + public static ArmConfig vortexNoPID(int armMasterMotorId, boolean armMasterInversed, double bottomLimit, double topLimit) { + return motorNoPID(armMasterMotorId, MotorConfig.NEO_VORTEX, armMasterInversed, bottomLimit, topLimit); + } + + /** + * Check that arguments are good and set some of the parameters of the config. + * @param armMotorOfBackupRelativeEncoderId give motor Id + */ + private void checkRequirements(int armMotorOfBackupRelativeEncoderId) { + matchMotors(); + checkMotorConfigs(); //Prevent future issues with new motors + checkEncoders(armMotorOfBackupRelativeEncoderId); + checkPID(); + checkFeedForward(); + checkGearReduction(); + checkVolts(); + checkLimits(); + } + + /** + * Make sure that each corresponding motor id has a MotorConfig and inverse config + */ + private void matchMotors() { + if (armFollowId != null) { + if (armFollowId.length == armFollowInversed.length + && armFollowId.length == armFollowMotorType.length) { + return; + } + throw new IllegalArgumentException("Motors must have matching IDs, MotorTypes and Inverse Configs"); + } + if (armMasterMotorId != -1) { + if (armMasterMotorType != null) { + return; + } + throw new IllegalArgumentException("Master motor needs a MotorConfig"); + } + throw new IllegalArgumentException("Master motor needs to exist"); + } + + /** + * Check that the motor Configs are not any new configs that would not work or null. + */ + private void checkMotorConfigs() { + if (armFollowId != null) { + for (MotorConfig config: armFollowMotorType) { + if (config != MotorConfig.NEO && config != MotorConfig.NEO_VORTEX && config != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + } + } + if (armMasterMotorType != MotorConfig.NEO && armMasterMotorType != MotorConfig.NEO_VORTEX && armMasterMotorType != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + } + + /** + * Create {@link #armMainRelativeEncoder}, {@link #armMainAbsoluteEncoder}, and {@link #armBackupRelativeEncoder} if such are provided. + * Will update the exists boolean for each, and throw illegal arguments if does not see the backup encoder id in the follow ids + * @param armMotorOfBackupRelativeEncoderId + */ + private void checkEncoders(int armMotorOfBackupRelativeEncoderId) { + if (armMotorOfBackupRelativeEncoderId != -1) { + if (armFollowId != null) { + for (int i = 0; i < armFollowId.length; i++) { + if (armFollowId[i] == armMotorOfBackupRelativeEncoderId) { + return; + } + } + throw new IllegalArgumentException("Backup motor id of " + armMotorOfBackupRelativeEncoderId + " is not part of provided motors :("); + } + else { + throw new IllegalArgumentException("Cannot have a backup relative encoder without follow motors"); + } + } + else { + DriverStation.reportWarning("Arm does not have backup encoder, highly recommended", true); + } + + + } + + /** + * Checks PID to have 3 values, updates {@link #armPIDExists} and makes sure the values are legitimate + */ + private void checkPID() { + if (armKPID != null) { + if (armKPID.length == 3) { + if (armKPID[0] < 0 || armKPID[1] < 0 || armKPID[2] < 0) { + throw new IllegalArgumentException("PID values have to be non negative"); + } + armKP = armKPID[0]; + armKI = armKPID[1]; + armKD = armKPID[2]; + armPID = new PIDController(armKP, armKI, armKD); + armPID.setTolerance(armPIDTolerance); + if (armPIDContinuousInput) { + armPID.enableContinuousInput(-180, 180); + } + return; + } + throw new IllegalArgumentException("Need to have 3 values for PID"); + } + armKP = 0; + armKI = 0; + armKD = 0; + DriverStation.reportWarning("ArmPID is off", true); + } + + /** + * Checks FeedForward to have 4 values, updates {@link #armFeedForwardExists} and makes sure the values are legitimate + */ + private void checkFeedForward() { + if (armKFeedForward != null) { + if (armKFeedForward.length == 4) { + if (armKFeedForward[0] < 0|| armKFeedForward[2] < 0 || armKFeedForward[3] < 0) { + throw new IllegalArgumentException("FeedForward Values of kS, kV, and kA need to have non negative values"); + } + armKS = armKFeedForward[0]; + armKG = armKFeedForward[1]; + armKV = armKFeedForward[2]; + armKA = armKFeedForward[3]; + armFeedForward = new ArmFeedforward(armKS, armKG, armKV, armKA); + return; + } + throw new IllegalArgumentException("Need to have 4 values for Arm FeedForward: Friction, Gravity, Voltage, and Acceleration"); + } + armKS = 0; + armKG = 0; + armKV = 0; + armKA = 0; + DriverStation.reportWarning("ArmFeedForward is off", true); + } + + /** + * Makes sure gear reduction is not 0 + */ + private void checkGearReduction() { + if (armRelativeGearReduction == 0) { + throw new IllegalArgumentException("Gear reduction for relative encoders cannot be 0"); + } + if (armMainAbsoluteEncoder != null && armAbsoluteGearReduction == 0) { + throw new IllegalArgumentException("Absolute gear reduction cannot be 0 if absolute encoder exists"); + } + } + + /** + * Check for volt limit to be greater than 0 + */ + private void checkVolts() { + if (armMaxVolts <= 0) { + throw new IllegalArgumentException("armMaxVolts must be a positive number."); + } + return; + } + + /** + * Makes sure that the user has not mixed up the top and bottom limits. + */ + private void checkLimits() { + if (bottomLimit >= topLimit) { + throw new IllegalArgumentException("Top limit must be much greater than bottom limit. (Cannot be equal)"); + } + } + + +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java new file mode 100644 index 0000000..07b3b2f --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java @@ -0,0 +1,599 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +/* + * Important info about SimpleArm! + * Encoders are set to read in degrees from the perspective of the rotating object, with 0 being horizontal in the more often direction the use would use + */ + +package org.carlmontrobotics.lib199.SimpleMechs.Arm; + + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.SimpleMechs.Elastic; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Seconds; + +public class SimpleArm extends SubsystemBase { + /** Creates a new SimpleArm. */ + private final ArmConfig armConfig; + private SparkBase armMasterMotor; + private RelativeEncoder armFeedbackEncoder; + private AbsoluteEncoder armResetEncoder; + private RelativeEncoder armBackupEncoder; + private SparkBase[] armFollowMotors; + private double encoderDif = 0; + private final double difDanger = 1; + + private double armGoal; // in degrees + private double latestManualInput; // in percentage of power (-1, 1) + + private ArmControlState currentState = ArmControlState.AUTO; + private IdleMode armIdleMode = IdleMode.kBrake; + + private final double defaultRelativeEncoderResetValue = -90; + + private double currentStamp; + private double pastStamp; + private double currentPeriod = 0.02; + + private boolean encoderFaultReported = false; + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutAngle m_angle = Radians.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutAngularVelocity m_velocity = RadiansPerSecond.mutable(0); + + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config( + Volts.of(1).per(Seconds),//ramp rate, volts/sec + Volts.of(1), //starting voltage, volts + Seconds.of(5)//maximum sysID test time + ); + + + private final SysIdRoutine sysIdRoutine; + + /** + * Control State for the arm + * {@link #AUTO} For automatically controling the arm to go to a certain goal + * {@link #MANUAL} For manually controlling the arm by providing percentage of voltage + * {@link #BRAKE} For holding the arm in a rigidish state, not moving + * {@link #COAST} For setting the arm to not resist movement, allowing accessibility when the robot is disabled. + */ + public enum ArmControlState { + AUTO, + MANUAL, + BRAKE, + COAST + } + + /** + * Creates an object of {@link SimpleArm}, can be extended off this class to create a more complicated arm without requiring to fully remake a subsystem. + * @param armConfig + */ + public SimpleArm(ArmConfig armConfig) { + this.armConfig = armConfig; + armResetEncoder = armConfig.armMainAbsoluteEncoder; + configMotors(); + resetFeedbackEncoder(); + sysIdRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + voltage -> { + manualArm(voltage); + }, + log -> { + log.motor("Spinner-Master") + .voltage( + m_appliedVoltage + .mut_replace(armMasterMotor.getBusVoltage() * armMasterMotor.getAppliedOutput(), Volts)) + .angularPosition(m_angle.mut_replace(Units.degreesToRadians(armFeedbackEncoder.getPosition()), Radians)) + .angularVelocity(m_velocity.mut_replace(Units.degreesToRadians(armFeedbackEncoder.getVelocity()), RadiansPerSecond)); + }, + this) + ); + + if (isSysIdTesting()) { + sysIdSetup(); + } + SmartDashboard.putData(this); + } + + /** + * Sets up commands in SmartDashboard/Elastic to be ran to record data for mechanism modeling + */ + private void sysIdSetup() { + Elastic.Notification notification = new Elastic.Notification(Elastic.NotificationLevel.INFO, "SysId Testing On", "Please add Arm Quasistatic and Dynamics and begin testing.").withDisplaySeconds(5); + Elastic.sendNotification(notification); + Elastic.selectTab("SysId"); + SmartDashboard.putData("Arm Quasistatic Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + SmartDashboard.putData("Arm Quasistatic Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Arm Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Arm Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } + + /** Override this method and return true if you want to do sysid testing, return false if you don't + * @return false to do sysid testing + */ + protected boolean isSysIdTesting() { + return false; + } + + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction); + } + + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in. + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction); + } + + /** + * @return position in degrees from horizontal + */ + public double getArmPosition() { + return armFeedbackEncoder.getPosition(); + } + + + /** + * Sets {@link #armGoal}, as long as the value is within the provided limits in the {@link ArmConfig}. + * @param goal in degrees. + */ + public void setGoal(double goal){ + if (goal <= armConfig.topLimit && goal >= armConfig.bottomLimit) {armGoal = goal;} + } + + /** + * Gets the current value of {@link #armGoal}. + * @return {@link #armGoal} in degrees. + */ + public double getGoal() { + return armGoal; + } + + /** + * Checks if {@link SimpleArm} is at {@link #armGoal} within the given tolerance in the {@link ArmConfig}. + * @return true if it is at the {@link #armGoal}. + */ + public boolean atGoal() { + return Math.abs(armGoal - armFeedbackEncoder.getPosition()) <= armConfig.armPIDTolerance; + } + + /** + * Sets {@link #latestManualInput}. + * @param input -1.0 - 1.0 range for inputing voltage percentage into the arm. + */ + public void setManualInput(double input) { + latestManualInput = input; + } + + /** + * Fetches the user input being applied on the arm if the {@link #currentState} is MANUAL. + * @return {@link #latestManualInput} + */ + public double getLatestManualInput() { + return latestManualInput; + } + + /** + * @return active {@link ArmControlState}. + */ + protected ArmControlState getCurrentState() { + return currentState; + } + + /** + * Toggles {@link #currentState} between AUTO and MANUAL + * Does not do anything if arm is idling + */ + public void toggleControlMode() { + if (currentState == ArmControlState.AUTO) { + currentState = ArmControlState.MANUAL; + } + else if (currentState == ArmControlState.MANUAL) { + currentState = ArmControlState.AUTO; + } + } + + /** + * Sets {@link #currentState} to be a certain {@link ArmControlState} + * @param controlState desired control state + */ + public void setArmControlState(ArmControlState controlState) { + switch (controlState) { + case AUTO -> setAutoOn(); + case MANUAL -> setManualOn(); + case BRAKE -> setBrakeOn(); + case COAST -> setCoastOn(); + default -> DriverStation.reportWarning("Such control mode has not been implemented yet", true); + } + } + + /** + * Sets {@link #currentState} to be AUTO making the arm approach the {@link #armGoal} + */ + public void setAutoOn() { + if (armConfig.armPID != null || armConfig.armFeedForward != null) { + currentState = ArmControlState.AUTO; + } + else { + DriverStation.reportWarning("Any sort of autonomous control mode is disabled due to no PID or FeedForward", true); + } + } + + /** + * Sets {@link #currentState} to be MANUAL allowing user to move the arm manually + */ + public void setManualOn() { + currentState = ArmControlState.MANUAL; + } + + /** + * Sets {@link #currentState} to be BRAKE and brakes the motors + */ + public void setBrakeOn() { + currentState = ArmControlState.BRAKE; + if (armIdleMode == IdleMode.kCoast) { + armIdleMode = IdleMode.kBrake; + brakeMotors(); + + } + } + + /** + * Sets {@link #currentState} to be COAST and coasts the motors + */ + public void setCoastOn() { + currentState = ArmControlState.COAST; + if (armIdleMode == IdleMode.kBrake) { + armIdleMode = IdleMode.kCoast; + coastMotors(); + } + } + + /** + * Resets feedback encoder using the {@link #armResetEncoder}, if such does not exist will use a {@link #defaultRelativeEncoderResetValue} + */ + public void resetFeedbackEncoder() { + if (armConfig.armMainAbsoluteEncoder != null) { + armFeedbackEncoder.setPosition((armResetEncoder.getPosition() + armConfig.armAbsoluteZeroOffset)/armConfig.armAbsoluteGearReduction*armConfig.armRelativeGearReduction); + if (armBackupEncoder == null) {return;} + armBackupEncoder.setPosition((armResetEncoder.getPosition() + armConfig.armAbsoluteZeroOffset)/armConfig.armAbsoluteGearReduction*armConfig.armRelativeGearReduction); + } + else { + armFeedbackEncoder.setPosition(defaultRelativeEncoderResetValue); + if (armBackupEncoder == null) {return;} + armBackupEncoder.setPosition(defaultRelativeEncoderResetValue); + } + } + + /** + * Called periodically + * Checks difference between active encoder and backup encoder to see if there seems to be a mishap, such as encoder not measuring properly or just turning off. + */ + private void checkFeedBackEncoder() { + if (armBackupEncoder == null) {return;} + encoderDif = armFeedbackEncoder.getPosition() - armBackupEncoder.getPosition(); + if (Math.abs(encoderDif) >= difDanger && !encoderFaultReported) { + encoderFaultReported = true; + DriverStation.reportWarning("Arm encoder seems to be off!", true); + } + } + + /** + * Called in periodic if {@link #currentState} is auto. + * Will calculate the desired voltage to achieve the {@link #armGoal} + */ + protected void autoArm() { + double pidOutput; + double feedforwardOutput; + double curPosition = armFeedbackEncoder.getPosition(); + if (armConfig.armPID != null) { + pidOutput = armConfig.armPID.calculate(curPosition, armGoal); + } + else { + pidOutput = 0; + } + + if (armConfig.armFeedForward != null) { + feedforwardOutput = armConfig.armFeedForward.calculate(Units.degreesToRadians(curPosition), 0); + } + else { + feedforwardOutput = 0; + } + if (curPosition < armConfig.topLimit) { + if (curPosition > armConfig.bottomLimit) { + armMasterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, -armConfig.armMaxVolts, armConfig.armMaxVolts)); + } + else { + armMasterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, 0, armConfig.armMaxVolts)); + } + } + else { + armMasterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, -armConfig.armMaxVolts, 0)); + } + } + + /** + * Called in periodic if {@link #currentState} is manual. + * Will apply a percentage of voltage to the arm based of the users input: {@link #latestManualInput}, while soft locking it to avoid the arm leaving the intended bounds + */ + protected void manualArm() { + double curPosition = armFeedbackEncoder.getPosition(); + if (curPosition < armConfig.topLimit) { + if (curPosition > armConfig.bottomLimit) { + armMasterMotor.set(MathUtil.clamp(latestManualInput, -armConfig.armMaxVolts/12, armConfig.armMaxVolts/12)); + } + else { + armMasterMotor.set(MathUtil.clamp(latestManualInput, 0, armConfig.armMaxVolts/12)); + } + } + else { + armMasterMotor.set(MathUtil.clamp(latestManualInput, -armConfig.armMaxVolts/12, 0)); + } + } + + /** + * Mainly used by SysId to characterize the spinner model + * @param volts Voltage unit input + */ + protected void manualArm(Voltage volts) { + double curPosition = armFeedbackEncoder.getPosition(); + double voltage = volts.in(Volts); + if (curPosition < armConfig.topLimit) { + if (curPosition > armConfig.bottomLimit) { + armMasterMotor.setVoltage(MathUtil.clamp(voltage, -armConfig.armMaxVolts, armConfig.armMaxVolts)); + } + else { + armMasterMotor.setVoltage(MathUtil.clamp(voltage, 0, armConfig.armMaxVolts)); + } + } + else { + armMasterMotor.setVoltage(MathUtil.clamp(voltage, -armConfig.armMaxVolts, 0)); + } + } + + private void configMotors() { + configFollowMotors(); + configMasterMotor(); + } + + /** + * Configures follow motors to follow the master, and also to brake, and have the correct inverse for encoder readings, and to follow the master with the correct inverse. + */ + protected void configFollowMotors() { + if (armConfig.armFollowId != null) { + armFollowMotors = new SparkBase[armConfig.armFollowId.length]; + + SparkFlexConfig flexFollowConfig = new SparkFlexConfig(); + flexFollowConfig.idleMode(IdleMode.kBrake); + SparkMaxConfig maxFollowConfig = new SparkMaxConfig(); + maxFollowConfig.idleMode(IdleMode.kBrake); + + for (int i = 0; i < armConfig.armFollowId.length; i++) { + if (armConfig.armFollowId[i] == armConfig.armMotorOfBackupRelativeEncoderId) { + if (armConfig.armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { + SparkFlexConfig flexConfig = new SparkFlexConfig(); + SparkFlex dummyFlex = MotorControllerFactory.createSparkFlex(armConfig.armFollowId[i]); + flexConfig.encoder.positionConversionFactor(armConfig.armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armConfig.armRelativeGearReduction/60 * 360); //degrees per second + dummyFlex.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armBackupEncoder = dummyFlex.getEncoder(); + } + else { + SparkMaxConfig maxConfig = new SparkMaxConfig(); + SparkMax dummyMax = MotorControllerFactory.createSparkMax(armConfig.armFollowId[i], armConfig.armFollowMotorType[i]); + maxConfig.encoder.positionConversionFactor(armConfig.armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armConfig.armRelativeGearReduction/60 *360); //degrees per second + dummyMax.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armBackupEncoder = dummyMax.getEncoder(); + } + } + if (armConfig.armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { + flexFollowConfig.follow(armConfig.armMasterMotorId, !(armConfig.armMasterInversed && armConfig.armFollowInversed[i])); + SparkFlex followFlex = MotorControllerFactory.createSparkFlex(armConfig.armFollowId[i]); + followFlex.configure(flexFollowConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armFollowMotors[i] = followFlex; + } + else { + maxFollowConfig.follow(armConfig.armMasterMotorId, !(armConfig.armMasterInversed && armConfig.armFollowInversed[i])); + SparkMax followMax = MotorControllerFactory.createSparkMax(armConfig.armFollowId[i], armConfig.armFollowMotorType[i]); + followMax.configure(maxFollowConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armFollowMotors[i] = followMax; + } + } + } + } + + /** + * Configures master motor with brake and correct inverse config + */ + protected void configMasterMotor() { + if (armConfig.armMasterMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig flexConfig = new SparkFlexConfig(); + this.armMasterMotor = MotorControllerFactory.createSparkFlex(armConfig.armMasterMotorId); + flexConfig.inverted(armConfig.armMasterInversed) + .idleMode(IdleMode.kBrake) + .encoder.positionConversionFactor(armConfig.armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armConfig.armRelativeGearReduction/60 * 360); //degrees per second + armMasterMotor.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armFeedbackEncoder = armMasterMotor.getEncoder(); + } + else { + SparkMaxConfig maxConfig = new SparkMaxConfig(); + armMasterMotor = MotorControllerFactory.createSparkMax(armConfig.armMasterMotorId, armConfig.armMasterMotorType); + maxConfig.inverted(armConfig.armMasterInversed) + .idleMode(IdleMode.kBrake) + .encoder.positionConversionFactor(armConfig.armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armConfig.armRelativeGearReduction/60* 360); //degrees per second + armMasterMotor.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armFeedbackEncoder = armMasterMotor.getEncoder(); + } + } + + /** + * Set all motors idleModes to brake + */ + private void brakeMotors() { + SparkFlexConfig flexBrakeConfig = new SparkFlexConfig(); + SparkMaxConfig maxBrakeConfig = new SparkMaxConfig(); + + flexBrakeConfig.idleMode(IdleMode.kBrake); + maxBrakeConfig.idleMode(IdleMode.kBrake); + + if (armConfig.armFollowId != null) { + for (int i = 0; i < armFollowMotors.length; i++) { + if (armConfig.armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { + armFollowMotors[i].configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + armFollowMotors[i].configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + } + if (armConfig.armMasterMotorType == MotorConfig.NEO_VORTEX) { + armMasterMotor.configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + armMasterMotor.configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + /** + * Set all motors idleModes to coast + */ + private void coastMotors() { + SparkFlexConfig flexCoastConfig = new SparkFlexConfig(); + SparkMaxConfig maxCoastConfig = new SparkMaxConfig(); + + flexCoastConfig.idleMode(IdleMode.kCoast); + maxCoastConfig.idleMode(IdleMode.kCoast); + + if (armConfig.armFollowId != null) { + for (int i = 0; i < armFollowMotors.length; i++) { + if (armConfig.armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { + armFollowMotors[i].configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + armFollowMotors[i].configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + } + if (armConfig.armMasterMotorType == MotorConfig.NEO_VORTEX) { + armMasterMotor.configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + armMasterMotor.configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + public void stopArm() { + armMasterMotor.set(0); + } + + /** + * Use this to do any fun stuff you want to do, for armSubsystemPeriodic + */ + protected void userPeriodic() { + + } + + protected void postSmartDashboardValues() { + SmartDashboard.putNumber("armVelocity (Degrees/s)", armFeedbackEncoder.getVelocity()); + SmartDashboard.putNumber("armPosition (Degrees)", armFeedbackEncoder.getPosition()); //From horizontal being 0 and up is positive + switch (currentState) { + case AUTO -> SmartDashboard.putString("armControlState", "Autonomous"); + case MANUAL -> SmartDashboard.putString("armControlState", "Manual"); + case BRAKE -> SmartDashboard.putString("armControlState", "Idling: Brake"); + case COAST -> SmartDashboard.putString("armControlState", "Idling: Coast"); + } + SmartDashboard.putNumber("armGoal (Degrees from horizontal)", armGoal); + SmartDashboard.putNumber("Current period of Arm (ms)", currentPeriod*1000); + + } + + /** + * Checks Subsystem periodic for running under expected time limits, reports inconsitencies to user, also is usable by feedforward + */ + private void checkPeriodic() { + currentStamp = Timer.getFPGATimestamp(); + currentPeriod = currentStamp - pastStamp; + pastStamp = currentStamp; + if (currentPeriod > 0.02) { + DriverStation.reportWarning("Arm Periodic is slow: " + currentPeriod *1000 + " ms. Check for any lag reasons", true); + } + } + + /** + * @return current calculated period in seconds + */ + protected double getCurrentPeriod() { + return currentPeriod; + } + + /*Overide these methods to customize what the mechanism does in different states */ + protected void handleAuto() { autoArm();} + protected void handleManual() { manualArm();} + protected void handleBrake() { stopArm();} + protected void handleCoast() { stopArm();} + + + /** + * DO NOT OVERRIDE THIS PERIODIC UNLESS YOU KNOW WHAT YOU ARE DOING! + * Use {@link #userPeriodic()} instead. + */ + @Override + public void periodic() { + switch (currentState) { + case AUTO -> handleAuto(); + case MANUAL -> handleManual(); + case BRAKE -> handleBrake(); + case COAST -> handleCoast(); + } + checkFeedBackEncoder(); + userPeriodic(); + postSmartDashboardValues(); + checkPeriodic(); + } + +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleSetArmGoal.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleSetArmGoal.java new file mode 100644 index 0000000..46d11e8 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleSetArmGoal.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.lib199.SimpleMechs.Arm; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SimpleSetArmGoal extends Command { + /** Creates a new MoveArm. */ + SimpleArm arm; + double goal; + public SimpleSetArmGoal(SimpleArm arm, double goal) { + addRequirements(this.arm = arm); + this.goal = goal; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + arm.setAutoOn(); + arm.setGoal(goal); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return arm.atGoal(); + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java new file mode 100644 index 0000000..24ce196 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java @@ -0,0 +1,1460 @@ +package org.carlmontrobotics.lib199.SimpleMechs.Drivetrain; +import java.util.Arrays; +import java.util.Map; +import java.util.function.Supplier; + +import javax.print.attribute.standard.PrinterInfo; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.SensorFactory; +import org.carlmontrobotics.lib199.swerve.SwerveConfig; +import org.carlmontrobotics.lib199.swerve.SwerveModule; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.hardware.CANcoder; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; + +import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +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.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SelectCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; + +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volt; +import static edu.wpi.first.units.Units.Meter; +import static edu.wpi.first.units.Units.Meters; + +public class SimpleDrivetrain extends SubsystemBase { + + private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI); + private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); + // ^used by PathPlanner for chaining paths + private SwerveDriveKinematics kinematics = null; + private SwerveDriveOdometry odometry = null; + private SwerveDrivePoseEstimator poseEstimator = null; + + private SwerveModule modules[]; + private boolean fieldOriented = true; + private double fieldOffset = 0; + + private SparkFlex[] flexDriveMotors = new SparkFlex[] { null, null, null, null }; + private SparkFlex[] flexTurnMotors = new SparkFlex[] { null, null, null, null }; + private SparkMax[] maxDriveMotors = new SparkMax[] { null, null, null, null }; + private SparkMax[] maxTurnMotors = new SparkMax[] { null, null, null, null }; + private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; + private final SparkClosedLoopController[] turnPidControllers = new SparkClosedLoopController[] {null, null, null, null}; + public final float initPitch; + public final float initRoll; + + // debug purposes + private SwerveConfig swerveConfig; + private SwerveModule moduleFL; + private SwerveModule moduleFR; + private SwerveModule moduleBL; + private SwerveModule moduleBR; + + private final Field2d field = new Field2d(); + private final Field2d odometryField = new Field2d(); + private final Field2d poseWithLimelightField = new Field2d(); + + public double ppKpDrive = 5.0; + public double ppKiDrive = 0; + public double ppKdDrive = 0; + + public double ppKpTurn = 3; + public double ppKiTurn = 0; + public double ppKdTurn = 0; + + double accelX; + double accelY; + double accelXY; + + private SwerveModuleSim[] moduleSims; + private SimDouble gyroYawSim; + private Timer simTimer = new Timer(); + public double extraSpeedMult = 0; + + private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; + double kP = 0; + double kI = 0; + double kD = 0; + public enum Mode { + coast, + brake, + toggle + } + + //Constants + private double g = 9.8; //m/s^2 + private double mu = 1; + private double autoCentripetalAccel = mu * g * 2; + + + //Extra + private double wheelBase; + private double trackWidth; + + private int driveFrontLeftPort; + private int turnFrontLeftPort; + private int canCoderPortFL; + + private int driveFrontRightPort; + private int turnFrontRightPort; + private int canCoderPortFR; + + private int driveBackLeftPort; + private int turnBackLeftPort; + private int canCoderPortBL; + + private int driveBackRightPort; + private int turnBackRightPort; + private int canCoderPortBR; + + private double secsPer12Volts; + private double wheelDiameterMeters; + private double driveGearing; + private double turnGearing; + + private boolean isGyroReversed; + private double maxSpeed; + + private RobotConfig robotConfig; + + private double COLLISION_ACCELERATION_THRESHOLD; + + private boolean setupSysId; + + private boolean useFlexDrive; + private boolean useFlexTurn; + + private Rotation2d simGyroOffset = new Rotation2d(); + + + + /** + * + * @param wheelBase in meters + * @param trackWidth in meters + */ + public SimpleDrivetrain( + SwerveConfig swerveConfig, + double wheelBase, double trackWidth, + int driveFrontLeftPort, int turnFrontLeftPort, int canCoderPortFL, + int driveFrontRightPort, int turnFrontRightPort, int canCoderPortFR, + int driveBackLeftPort, int turnBackLeftPort, int canCoderPortBL, + int driveBackRightPort, int turnBackRightPort, int canCoderPortBR, + double secsPer12Volts, double wheelDiameterMeters, double driveGearing, double turnGearing, + boolean isGyroReversed, double maxSpeed, + RobotConfig robotConfig, + double COLLISION_ACCELERATION_THRESHOLD, + boolean useFlexDrive, + boolean useFlexTurn + ) + { + this.swerveConfig = swerveConfig; + this.wheelBase = wheelBase; + this.trackWidth = trackWidth; + this.driveFrontLeftPort = driveFrontLeftPort; + this.turnFrontLeftPort = turnFrontLeftPort; + this.canCoderPortFL = canCoderPortFL; + this.driveFrontRightPort = driveFrontRightPort; + this.turnFrontRightPort = turnFrontRightPort; + this.canCoderPortFR = canCoderPortFR; + this.driveBackLeftPort = driveBackLeftPort; + this.turnBackLeftPort = turnBackLeftPort; + this.canCoderPortBL = canCoderPortBL; + this.driveBackRightPort = driveBackRightPort; + this.turnBackRightPort = turnBackRightPort; + this.canCoderPortBR = canCoderPortBR; + this.secsPer12Volts = secsPer12Volts; + this.wheelDiameterMeters = wheelDiameterMeters; + this.driveGearing = driveGearing; + this.turnGearing = turnGearing; + this.isGyroReversed = isGyroReversed; + this.maxSpeed = maxSpeed; + this.robotConfig = robotConfig; + this.COLLISION_ACCELERATION_THRESHOLD = COLLISION_ACCELERATION_THRESHOLD; + this.useFlexDrive = useFlexDrive; + this.useFlexTurn = useFlexDrive; + + AutoBuilder(); + + double initTimestamp = Timer.getFPGATimestamp(); + double currentTimestamp = initTimestamp; + while (gyro.isCalibrating() && currentTimestamp - initTimestamp < 10) { + currentTimestamp = Timer.getFPGATimestamp(); + try { + Thread.sleep(1000);// 1 second + } catch (InterruptedException e) { + e.printStackTrace(); + break; + } + System.out.println("Calibrating the gyro..."); + } + gyro.reset(); + // this.resetFieldOrientation(); + System.out.println("NavX-MXP firmware version: " + gyro.getFirmwareVersion()); + System.out.println("Magnetometer is calibrated: " + gyro.isMagnetometerCalibrated()); + + // Setup Kinematics + { + // Define the corners of the robot relative to the center of the robot using + // Translation2d objects. + // Positive x-values represent moving toward the front of the robot whereas + // positive y-values represent moving toward the left of the robot. + Translation2d locationFL = new Translation2d(wheelBase / 2, trackWidth / 2); + Translation2d locationFR = new Translation2d(wheelBase / 2, -trackWidth / 2); + Translation2d locationBL = new Translation2d(-wheelBase / 2, trackWidth / 2); + Translation2d locationBR = new Translation2d(-wheelBase / 2, -trackWidth / 2); + + kinematics = new SwerveDriveKinematics(locationFL, locationFR, locationBL, locationBR); + } + + // Initialize modules + { + // initPitch = 0; + // initRoll = 0; + Supplier pitchSupplier = () -> 0F; + Supplier rollSupplier = () -> 0F; + initPitch = gyro.getPitch(); + initRoll = gyro.getRoll(); + // Supplier pitchSupplier = () -> gyro.getPitch(); + // Supplier rollSupplier = () -> gyro.getRoll(); + + if (useFlexDrive) { + if (useFlexTurn) { + moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, + flexDriveMotors[0] = MotorControllerFactory.createSparkFlex(driveFrontLeftPort), + flexTurnMotors[0] = MotorControllerFactory.createSparkFlex(turnFrontLeftPort), + turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); + moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, + flexDriveMotors[1] = MotorControllerFactory.createSparkFlex(driveFrontRightPort), + flexTurnMotors[1] = MotorControllerFactory.createSparkFlex(turnFrontRightPort), + turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, + flexDriveMotors[2] = MotorControllerFactory.createSparkFlex(driveBackLeftPort), + flexTurnMotors[2] = MotorControllerFactory.createSparkFlex(turnBackLeftPort), + turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, + flexDriveMotors[3] = MotorControllerFactory.createSparkFlex(driveBackRightPort), + flexTurnMotors[3] = MotorControllerFactory.createSparkFlex(turnBackRightPort), + turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + SparkFlexConfig driveConfig = new SparkFlexConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkFlex driveMotor : flexDriveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + SparkFlexConfig turnConfig = new SparkFlexConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkFlex turnMotor : flexTurnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + turnPidControllers[0] = flexTurnMotors[0].getClosedLoopController(); + turnPidControllers[1] = flexTurnMotors[1].getClosedLoopController(); + turnPidControllers[2] = flexTurnMotors[2].getClosedLoopController(); + turnPidControllers[3] = flexTurnMotors[3].getClosedLoopController(); + } + else { + moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, + flexDriveMotors[0] = MotorControllerFactory.createSparkFlex(driveFrontLeftPort), + maxTurnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), + turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); + moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, + flexDriveMotors[1] = MotorControllerFactory.createSparkFlex(driveFrontRightPort), + maxTurnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), + turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, + flexDriveMotors[2] = MotorControllerFactory.createSparkFlex(driveBackLeftPort), + maxTurnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), + turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, + flexDriveMotors[3] = MotorControllerFactory.createSparkFlex(driveBackRightPort), + maxTurnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), + turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + SparkFlexConfig driveConfig = new SparkFlexConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkFlex driveMotor : flexDriveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + SparkMaxConfig turnConfig = new SparkMaxConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkMax turnMotor : maxTurnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + turnPidControllers[0] = maxTurnMotors[0].getClosedLoopController(); + turnPidControllers[1] = maxTurnMotors[1].getClosedLoopController(); + turnPidControllers[2] = maxTurnMotors[2].getClosedLoopController(); + turnPidControllers[3] = maxTurnMotors[3].getClosedLoopController(); + } + } + else { + if (useFlexTurn) { + moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, + maxDriveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorConfig.NEO), + flexTurnMotors[0] = MotorControllerFactory.createSparkFlex(turnFrontLeftPort), + turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); + moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, + maxDriveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorConfig.NEO), + flexTurnMotors[1] = MotorControllerFactory.createSparkFlex(turnFrontRightPort), + turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, + maxDriveMotors[2] = MotorControllerFactory.createSparkMax(driveBackLeftPort, MotorConfig.NEO), + flexTurnMotors[2] = MotorControllerFactory.createSparkFlex(turnBackLeftPort), + turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, + maxDriveMotors[3] = MotorControllerFactory.createSparkMax(driveBackRightPort, MotorConfig.NEO), + flexTurnMotors[3] = MotorControllerFactory.createSparkFlex(turnBackRightPort), + turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkMax driveMotor : maxDriveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + SparkFlexConfig turnConfig = new SparkFlexConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkFlex turnMotor : flexTurnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + turnPidControllers[0] = flexTurnMotors[0].getClosedLoopController(); + turnPidControllers[1] = flexTurnMotors[1].getClosedLoopController(); + turnPidControllers[2] = flexTurnMotors[2].getClosedLoopController(); + turnPidControllers[3] = flexTurnMotors[3].getClosedLoopController(); + } + else { + moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, + maxDriveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorConfig.NEO), + maxTurnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), + turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); + moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, + maxDriveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorConfig.NEO), + maxTurnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), + turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, + maxDriveMotors[2] = MotorControllerFactory.createSparkMax(driveBackLeftPort, MotorConfig.NEO), + maxTurnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), + turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, + maxDriveMotors[3] = MotorControllerFactory.createSparkMax(driveBackRightPort, MotorConfig.NEO), + maxTurnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), + turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkMax driveMotor : maxDriveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + SparkMaxConfig turnConfig = new SparkMaxConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkMax turnMotor : maxTurnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + turnPidControllers[0] = maxTurnMotors[0].getClosedLoopController(); + turnPidControllers[1] = maxTurnMotors[1].getClosedLoopController(); + turnPidControllers[2] = maxTurnMotors[2].getClosedLoopController(); + turnPidControllers[3] = maxTurnMotors[3].getClosedLoopController(); + } + } + if (RobotBase.isSimulation()) { + moduleSims = new SwerveModuleSim[] { + moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() + }; + gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); + } + + SmartDashboard.putData("Module FL",moduleFL); + SmartDashboard.putData("Module FR",moduleFR); + SmartDashboard.putData("Module BL",moduleBL); + SmartDashboard.putData("Module BR",moduleBR); + + for (CANcoder coder : turnEncoders) { + coder.getAbsolutePosition().setUpdateFrequency(500); + coder.getPosition().setUpdateFrequency(500); + coder.getVelocity().setUpdateFrequency(500); + } + + accelX = gyro.getWorldLinearAccelX(); // Acceleration along the X-axis + accelY = gyro.getWorldLinearAccelY(); // Acceleration along the Y-axis + accelXY = Math.sqrt(gyro.getWorldLinearAccelX() * gyro.getWorldLinearAccelX() + gyro.getWorldLinearAccelY() * gyro.getWorldLinearAccelY()); + + + // Must call this method for SysId to run + if (setupSysId) { + sysIdSetup(); + } + } + + odometry = new SwerveDriveOdometry(kinematics, + Rotation2d.fromDegrees(getHeading()), getModulePositions(), + new Pose2d()); + poseEstimator = new SwerveDrivePoseEstimator( + getKinematics(), + Rotation2d.fromDegrees(getHeading()), + getModulePositions(), + new Pose2d()); + + SmartDashboard.putData(this); + + } + + + public boolean isAtAngle(double desiredAngleDeg, double toleranceDeg){ + for (SwerveModule module : modules) { + if (!(Math.abs(MathUtil.inputModulus(module.getModuleAngle() - desiredAngleDeg, -90, 90)) < toleranceDeg)) + return false; + } + return true; + } + + @Override + public void simulationPeriodic() { + for (var moduleSim : moduleSims) { + moduleSim.update(); + } + SwerveModuleState[] measuredStates = + new SwerveModuleState[] { + moduleFL.getCurrentState(), moduleFR.getCurrentState(), moduleBL.getCurrentState(), moduleBR.getCurrentState() + }; + ChassisSpeeds speeds = kinematics.toChassisSpeeds(measuredStates); + + double dtSecs = simTimer.get(); + simTimer.restart(); + + Pose2d simPose = field.getRobotPose(); + simPose = simPose.exp( + new Twist2d( + speeds.vxMetersPerSecond * dtSecs, + speeds.vyMetersPerSecond * dtSecs, + speeds.omegaRadiansPerSecond * dtSecs)); + double newAngleDeg = simPose.getRotation().getDegrees(); + // Subtract the offset computed the last time setPose() was called because odometry.update() adds it back. + newAngleDeg -= simGyroOffset.getDegrees(); + newAngleDeg *= (isGyroReversed ? -1.0 : 1.0); + gyroYawSim.set(newAngleDeg); + } + + // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyRoutine.quasistatic(direction); + // case 1: + // return backOnlyRoutine.quasistatic(direction); + // case 2: + // return allWheelsRoutine.quasistatic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + + /** + * Sets swerveModules IdleMode both turn and drive + * @param brake boolean for braking, if false then coast + */ + public void setDrivingIdleMode(boolean brake) { + IdleMode mode; + if (brake) { + mode = IdleMode.kBrake; + } + else { + mode = IdleMode.kCoast; + } + if (useFlexDrive) { + for (SparkFlex driveMotor : flexDriveMotors) { + SparkFlexConfig config = new SparkFlexConfig(); + config.idleMode(mode); + driveMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + else { + for (SparkMax driveMotor : maxDriveMotors) { + SparkMaxConfig config = new SparkMaxConfig(); + config.idleMode(mode); + driveMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + if (useFlexTurn) { + for (SparkFlex turnMotor : flexTurnMotors) { + SparkFlexConfig config = new SparkFlexConfig(); + config.idleMode(mode); + turnMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + else { + for (SparkMax turnMotor : maxTurnMotors) { + SparkMaxConfig config = new SparkMaxConfig(); + config.idleMode(mode); + turnMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + } + + @Override + public void periodic() { + detectCollision(); //This does nothing + PathPlannerLogging.logCurrentPose(getPose()); + + for (SwerveModule module : modules) { + // module.turnPeriodic(); For testing + module.periodic(); + } + + // field.setRobotPose(odometry.getPoseMeters()); + + + + // odometry.update(gyro.getRotation2d(), getModulePositions()); + + // poseEstimator.update(gyro.getRotation2d(), getModulePositions()); + + //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); + + // updateMT2PoseEstimator(); + + // double currSetX = + // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); + // double currSetY = + // SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY); + // double currSetTheta = SmartDashboard + // .getNumber("Pose Estimator set rotation (deg)", lastSetTheta); + + // if (lastSetX != currSetX || lastSetY != currSetY + // || lastSetTheta != currSetTheta) { + // setPose(new Pose2d(currSetX, currSetY, + // Rotation2d.fromDegrees(currSetTheta))); + // } + + // setPose(new Pose2d(getPose().getTranslation().getX(), + // getPose().getTranslation().getY(), + // Rotation2d.fromDegrees(getHeading()))); + + + // SmartDashboard.putNumber("X position with limelight", getPoseWithLimelight().getX()); + // SmartDashboard.putNumber("Y position with limelight", getPoseWithLimelight().getY()); + SmartDashboard.putNumber("X position with gyro", getPose().getX()); + SmartDashboard.putNumber("Y position with gyro", getPose().getY()); + SmartDashboard.putBoolean("setupSysId", setupSysId); + + //For finding acceleration of drivetrain for collision detector + SmartDashboard.putNumber("Accel X", accelX); + SmartDashboard.putNumber("Accel Y", accelY); + SmartDashboard.putNumber("2D Acceleration ", accelXY); + + // // // SmartDashboard.putNumber("Pitch", gyro.getPitch()); + // // // SmartDashboard.putNumber("Roll", gyro.getRoll()); + // SmartDashboard.putNumber("Raw gyro angle", gyro.getAngle()); + // SmartDashboard.putNumber("Robot Heading", getHeading()); + // // // SmartDashboard.putNumber("AdjRoll", gyro.getPitch() - initPitch); + // // // SmartDashboard.putNumber("AdjPitch", gyro.getRoll() - initRoll); + // SmartDashboard.putBoolean("Field Oriented", fieldOriented); + // SmartDashboard.putNumber("Gyro Compass Heading", gyro.getCompassHeading()); + // SmartDashboard.putNumber("Compass Offset", compassOffset); + // SmartDashboard.putBoolean("Current Magnetic Field Disturbance", gyro.isMagneticDisturbance()); + SmartDashboard.putNumber("front left encoder", moduleFL.getModuleAngle()); + SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); + SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); + SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); + userPeriodic(); + } + + public void userPeriodic() { + //Override this + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + + for (SwerveModule module : modules) + SendableRegistry.addChild(this, module); + + builder.addBooleanProperty("Magnetic Field Disturbance", + gyro::isMagneticDisturbance, null); + builder.addBooleanProperty("Gyro Calibrating", gyro::isCalibrating, null); + builder.addBooleanProperty("Field Oriented", () -> fieldOriented, + fieldOriented -> this.fieldOriented = fieldOriented); + builder.addDoubleProperty("Pose Estimator X", () -> getPose().getX(), + null); + builder.addDoubleProperty("Pose Estimator Y", () -> getPose().getY(), + null); + builder.addDoubleProperty("Pose Estimator Theta", + () -> + getPose().getRotation().getDegrees(), null); + builder.addDoubleProperty("Robot Heading", () -> getHeading(), null); + builder.addDoubleProperty("Raw Gyro Angle", gyro::getAngle, null); + builder.addDoubleProperty("Pitch", gyro::getPitch, null); + builder.addDoubleProperty("Roll", gyro::getRoll, null); + builder.addDoubleProperty("Field Offset", () -> fieldOffset, fieldOffset -> + this.fieldOffset = fieldOffset); + builder.addDoubleProperty("FL Turn Encoder (Deg)", + () -> moduleFL.getModuleAngle(), null); + builder.addDoubleProperty("FR Turn Encoder (Deg)", + () -> moduleFR.getModuleAngle(), null); + builder.addDoubleProperty("BL Turn Encoder (Deg)", + () -> moduleBL.getModuleAngle(), null); + builder.addDoubleProperty("BR Turn Encoder (Deg)", + () -> moduleBR.getModuleAngle(), null); + } + + + // #region Drive Methods + + public void setExtraSpeedMult(double set) { + extraSpeedMult=set; + } + + /** + * Calculates and implements the required SwerveStates for all 4 modules to get the wanted outcome + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is positive. + */ + public void drive(double forward, double strafe, double rotation) { + drive(getSwerveStates(forward, strafe, rotation)); + } + + /** + * Implements the provided SwerveStates for all 4 modules to get the wanted outcome + * @param moduleStates SwerveModuleState[] + */ + public void drive(SwerveModuleState[] moduleStates) { + //Max speed override + double max = maxSpeed; + SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, max); + for (int i = 0; i < 4; i++) { + // SmartDashboard.putNumber("moduleIn" + Integer.toString(i), moduleStates[i].angle.getDegrees()); + moduleStates[i].optimize(Rotation2d.fromDegrees(modules[i].getModuleAngle())); + // SmartDashboard.putNumber("moduleOT" + Integer.toString(i), moduleStates[i].angle.getDegrees()); + modules[i].move(moduleStates[i].speedMetersPerSecond, moduleStates[i].angle.getDegrees()); + } + } + + /** + * Configures PathPlanner AutoBuilder + */ + public void AutoBuilder() { + RobotConfig config = robotConfig; + AutoBuilder.configure( + //Supplier poseSupplier, + this::getPose, // Robot pose supplier + //Consumer resetPose, + this::setPoseWithLimelight, // Method to reset odometry (will be called if your auto has a starting pose) + //Supplier robotRelativeSpeedsSupplier, + this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + //BiConsumer output, + (speeds, feedforwards) -> drive(kinematics.toSwerveModuleStates(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + //PathFollowingController controller, + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(4 + , ppKiDrive, ppKdDrive), // Translation PID constants + new PIDConstants(1, ppKiTurn, ppKdTurn) + ), + //RobotConfig robotConfig, + config, // The robot configuration + //BooleanSupplier shouldFlipPath, + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + //Subsystem... driveRequirements + this // Reference to this subsystem to set requirements + ); + } + +//---------------------------------------------------------- + + public void autoCancelDtCommand() { + if(!(getDefaultCommand() instanceof SimpleTeleopDrive) || DriverStation.isAutonomous()) return; + + // Use hasDriverInput to get around acceleration limiting on slowdown + if (((SimpleTeleopDrive) getDefaultCommand()).hasDriverInput()) { + Command currentDtCommand = getCurrentCommand(); + if (currentDtCommand != getDefaultCommand() && !(currentDtCommand instanceof SimpleRotateToFieldRelativeAngle) + && currentDtCommand != null) { + currentDtCommand.cancel(); + } + } + } + + public void stop() { + for (SwerveModule module : modules) + module.move(0, 0); + } + + public boolean isStopped() { + return Math.abs(getSpeeds().vxMetersPerSecond) < 0.1 && + Math.abs(getSpeeds().vyMetersPerSecond) < 0.1 && + Math.abs(getSpeeds().omegaRadiansPerSecond) < 0.1; + } + + /** + * Constructs and returns a ChassisSpeeds objects using forward, strafe, and + * rotation values. + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive. + * @return A ChassisSpeeds object. + */ + private ChassisSpeeds getChassisSpeeds(double forward, double strafe, double rotation) { + ChassisSpeeds speeds; + if (fieldOriented) { + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(forward, strafe, rotation, + Rotation2d.fromDegrees(getHeading())); + } else { + speeds = new ChassisSpeeds(forward, strafe, rotation); + } + return speeds; + } + + /** + * Constructs and returns four SwerveModuleState objects, one for each side, + * using forward, strafe, and rotation values. + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive. + * @return A SwerveModuleState array, one for each side of the drivetrain (FL, + * FR, etc.). + */ + private SwerveModuleState[] getSwerveStates(double forward, double strafe, double rotation) { + return kinematics.toSwerveModuleStates(getChassisSpeeds(forward, -strafe, rotation)); + } + + public SwerveModuleState[] getSwerveStates(ChassisSpeeds speeds) { + return kinematics.toSwerveModuleStates(speeds); + } + + // #endregion + + // #region Getters and Setters + + /** + * @return the heading in degrees, normalized to the range -180 to 180 + */ + public double getHeading() { + double x = gyro.getAngle(); + if (fieldOriented) + x -= fieldOffset; + return Math.IEEEremainder(x * (isGyroReversed ? -1.0 : 1.0), 360); + } + + public SwerveModulePosition[] getModulePositions() { + return Arrays.stream(modules).map(SwerveModule::getCurrentPosition).toArray(SwerveModulePosition[]::new); + } + + public SwerveDrivePoseEstimator getPoseEstimator() { + return poseEstimator; + } + + /** + * Gets pose from {@link #poseEstimator} + * @return Pose2D + */ + public Pose2d getPose() { + // return odometry.getPoseMeters(); + return poseEstimator.getEstimatedPosition(); + } + + public void setPose(Pose2d initialPose) { + Rotation2d gyroRotation = gyro.getRotation2d(); + poseEstimator.resetPosition(gyroRotation, getModulePositions(), initialPose); + simGyroOffset = initialPose.getRotation().minus(gyroRotation); + odometry.resetPosition(gyroRotation, getModulePositions(), initialPose); + } + + //This method will set the pose using limelight if it sees a tag and if not it is supposed to run like setPose() + public void setPoseWithLimelight(Pose2d backupPose){ + DriverStation.reportError("Need to override setPoseWithLimelight and make the method setPose", true); + } + + /** + * Detects if the robot has experienced a collision based on acceleration thresholds. + * @return true if a 2D acceleration greater than the {@link #COLLISION_ACCELERATION_THRESHOLD} false otherwise. + */ + public boolean detectCollision(){ //We can implement this method into updatePoseWithLimelight so that if there is a collision it stops using odometry + accelX = gyro.getWorldLinearAccelX(); // Acceleration along the X-axis + accelY = gyro.getWorldLinearAccelY(); // Acceleration along the Y-axis + accelXY = Math.sqrt(accelX * accelX + accelY * accelY); // 2D Acceleration + return accelXY > COLLISION_ACCELERATION_THRESHOLD; // return true if collision detected + } + + /** + * @deprecated Use {@link #resetFieldOrientation()} instead + */ + @Deprecated + public void resetHeading() { + gyro.reset(); + + } + + public double getPitch() { + return gyro.getPitch(); + } + + public double getRoll() { + return gyro.getRoll(); + } + + /** + * true stands for fieldOriented, false stands for robotOriented + * @return boolean + */ + public boolean getFieldOriented() { + return fieldOriented; + } + + /** + * True sets fieldOriented, false sets robotOriented + * @param fieldOriented boolean + */ + public void setFieldOriented(boolean fieldOriented) { + this.fieldOriented = fieldOriented; + } + + /** + * Sets the current direction the robot is facing is to be 0 + */ + public void resetFieldOrientation() { + fieldOffset = gyro.getAngle(); + } + + /** + * Sets the current direction the robot is facing is to be 180 + */ + public void resetFieldOrientationBackwards() { + fieldOffset = 180 + gyro.getAngle(); + } + + /** + * Sets the current direction the robot is facing is plus @param angle to be 0 + * @param angle in degrees + */ + public void resetFieldOrientationWithAngle(double angle) { + fieldOffset = angle + gyro.getAngle(); + } + + public void resetPoseEstimator() { + poseEstimator.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); + gyro.reset(); + } + + public SwerveDriveKinematics getKinematics() { + return kinematics; + } + + public ChassisSpeeds getSpeeds() { + return kinematics.toChassisSpeeds(Arrays.stream(modules).map(SwerveModule::getCurrentState) + .toArray(SwerveModuleState[]::new)); + } + + public void setMode(Mode mode) { + for (SwerveModule module : modules){ + switch (mode) { + case coast: + module.coast(); + break; + case brake: + module.brake(); + break; + case toggle: + module.toggleMode(); + break; + } + } + } + /** + * @deprecated Use {@link #setMode(Mode)} instead + * Changes between IdleModes + */ + @Deprecated + public void toggleMode() { + for (SwerveModule module : modules) + module.toggleMode(); + } + /** + * @deprecated Use {@link #setMode(Mode)} instead + */ + @Deprecated + public void brake() { + for (SwerveModule module : modules) + module.brake(); + } + /** + * @deprecated Use {@link #setMode(Mode)} instead + */ + @Deprecated + public void coast() { + for (SwerveModule module : modules) + module.coast(); + } + + + + // #region SysId Code + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage[] m_appliedVoltage = new MutVoltage[8]; + + // Mutable holder for unit-safe linear distance values, persisted to avoid + // reallocation. + private final MutDistance[] m_distance = new MutDistance[4]; + // Mutable holder for unit-safe linear velocity values, persisted to avoid + // reallocation. + private final MutLinearVelocity[] m_velocity = new MutLinearVelocity[4]; + // edu.wpi.first.math.util.Units.Rotations beans; + private final MutAngle[] m_revs = new MutAngle[4]; + private final MutAngularVelocity[] m_revs_vel = new MutAngularVelocity[4]; + + private enum SysIdTest { + FRONT_DRIVE, + BACK_DRIVE, + ALL_DRIVE, + // FLBR_TURN, + // FRBL_TURN, + // ALL_TURN + FL_ROT, + FR_ROT, + BL_ROT, + BR_ROT + } + + private SendableChooser sysIdChooser = new SendableChooser<>(); + + // ROUTINES FOR SYSID + // private SysIdRoutine.Config defaultSysIdConfig = new + // SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), + // Seconds.of(5)); + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds), + Volts.of(2.891), Seconds.of(10)); + + // DRIVE + private void motorLogShort_drive(SysIdRoutineLog log, int id) { + if (useFlexDrive) { + String name = new String[] { "fl", "fr", "bl", "br" }[id]; + log.motor(name) + .voltage(m_appliedVoltage[id].mut_replace( + flexDriveMotors[id].getBusVoltage() * flexDriveMotors[id].getAppliedOutput(), Volts)) + .linearPosition( + m_distance[id].mut_replace(flexDriveMotors[id].getEncoder().getPosition(), Meters)) + .linearVelocity(m_velocity[id].mut_replace(flexDriveMotors[id].getEncoder().getVelocity(), + MetersPerSecond)); + } + else { + String name = new String[] { "fl", "fr", "bl", "br" }[id]; + log.motor(name) + .voltage(m_appliedVoltage[id].mut_replace( + maxDriveMotors[id].getBusVoltage() * maxDriveMotors[id].getAppliedOutput(), Volts)) + .linearPosition( + m_distance[id].mut_replace(maxDriveMotors[id].getEncoder().getPosition(), Meters)) + .linearVelocity(m_velocity[id].mut_replace(maxDriveMotors[id].getEncoder().getVelocity(), + MetersPerSecond)); + } + } + + // Create a new SysId routine for characterizing the drive. + private SysIdRoutine frontOnlyDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + // Tell SysId how to give the driving voltage to the motors. + (Voltage volts) -> { + if (useFlexDrive) { + flexDriveMotors[0].setVoltage(volts.in(Volts)); + flexDriveMotors[1].setVoltage(volts.in(Volts)); + } + else { + maxDriveMotors[0].setVoltage(volts.in(Volts)); + maxDriveMotors[1].setVoltage(volts.in(Volts)); + } + modules[2].coast(); + modules[3].coast(); + }, + log -> {// FRONT + motorLogShort_drive(log, 0);// fl named automatically + motorLogShort_drive(log, 1);// fr + }, + this)); + + private SysIdRoutine backOnlyDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + (Voltage volts) -> { + modules[0].coast(); + modules[1].coast(); + modules[2].brake(); + modules[3].brake(); + if (useFlexDrive) { + flexDriveMotors[2].setVoltage(volts.in(Volts)); + flexDriveMotors[3].setVoltage(volts.in(Volts)); + } + else { + maxDriveMotors[2].setVoltage(volts.in(Volts)); + maxDriveMotors[3].setVoltage(volts.in(Volts)); + } + }, + log -> {// BACK + motorLogShort_drive(log, 2);// bl + motorLogShort_drive(log, 3);// br + }, + this)); + + private SysIdRoutine allWheelsDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + (Voltage volts) -> { + if (useFlexDrive) { + for (SparkFlex dm : flexDriveMotors) { + dm.setVoltage(volts.in(Volts)); + } + } + else { + for (SparkMax dm : maxDriveMotors) { + dm.setVoltage(volts.in(Volts)); + } + } + }, + log -> { + motorLogShort_drive(log, 0);// fl named automatically + motorLogShort_drive(log, 1);// fr + motorLogShort_drive(log, 2);// bl + motorLogShort_drive(log, 3);// br + }, + this)); + + private SysIdRoutine sysidroutshort_turn(int id, String logname) { + double busVoltage; + double appliedOutput; + if (useFlexTurn) { + busVoltage = flexTurnMotors[id].getBusVoltage(); + appliedOutput = flexTurnMotors[id].getAppliedOutput(); + } + else { + busVoltage = maxTurnMotors[id].getBusVoltage(); + appliedOutput = maxTurnMotors[id].getAppliedOutput(); + } + return new SysIdRoutine( + defaultSysIdConfig, + // new SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), + // Seconds.of(3)), + new SysIdRoutine.Mechanism( + (Voltage volts) -> {if (useFlexTurn) {flexTurnMotors[id].setVoltage(volts.in(Volts));} else {maxTurnMotors[id].setVoltage(volts.in(Volts));}}, + log -> log.motor(logname + "_turn") + .voltage(m_appliedVoltage[id + 4].mut_replace( + // ^because drivemotors take up the first 4 slots of the unit holders + busVoltage * appliedOutput, Volts)) + .angularPosition( + m_revs[id].mut_replace(turnEncoders[id].getPosition().getValue())) + .angularVelocity(m_revs_vel[id].mut_replace( + turnEncoders[id].getVelocity().getValueAsDouble(), RotationsPerSecond)), + this)); + } + + // as always, fl/fr/bl/br + private SysIdRoutine[] rotateRoutine = new SysIdRoutine[] { + sysidroutshort_turn(0, "fl"), // woaw, readable code??? + sysidroutshort_turn(1, "fr"), + sysidroutshort_turn(2, "bl"), + sysidroutshort_turn(3, "br") + }; + + //TODO: migrate to elastic + //private ShuffleboardTab sysIdTab = Shuffleboard.getTab("Drivetrain SysID"); + + // void sysidtabshorthand(String name, SysIdRoutine.Direction dir, int width, + // int height){ + // sysIdTab.add(name, dir).withSize(width, height); + // } + void sysidtabshorthand_qsi(String name, SysIdRoutine.Direction dir) { + //TODO: migrate to elastic + //sysIdTab.add(name, sysIdQuasistatic(dir)).withSize(2, 1); + } + + // void sysidtabshorthand_dyn(String name, SysIdRoutine.Direction dir) { + // sysIdTab.add(name, sysIdDynamic(dir)).withSize(2, 1); + // } + + private void sysIdSetup() { + // SysId Setup + { + Supplier stopNwait = () -> new SequentialCommandGroup( + new InstantCommand(this::stop), new WaitCommand(2)); + + // sysidtabshorthand_qsi("Quasistatic Forward", SysIdRoutine.Direction.kForward); + // sysidtabshorthand_qsi("Quasistatic Backward", SysIdRoutine.Direction.kReverse); + // sysidtabshorthand_dyn("Dynamic Forward", SysIdRoutine.Direction.kForward); + // sysidtabshorthand_dyn("Dynamic Backward", SysIdRoutine.Direction.kReverse); + + //TODO: migrate to elastic + //sysIdTab + //.add(sysIdChooser) + //.withSize(2, 1); + + sysIdChooser.addOption("Front Only Drive", SysIdTest.FRONT_DRIVE); + sysIdChooser.addOption("Back Only Drive", SysIdTest.BACK_DRIVE); + sysIdChooser.addOption("All Drive", SysIdTest.ALL_DRIVE); + // sysIdChooser.addOption("fl-br Turn", SysIdTest.FLBR_TURN); + // sysIdChooser.addOption("fr-bl Turn", SysIdTest.FRBL_TURN); + // sysIdChooser.addOption("All Turn", SysIdTest.ALL_TURN); + sysIdChooser.addOption("FL Rotate", SysIdTest.FL_ROT); + sysIdChooser.addOption("FR Rotate", SysIdTest.FR_ROT); + sysIdChooser.addOption("BL Rotate", SysIdTest.BL_ROT); + sysIdChooser.addOption("BR Rotate", SysIdTest.BR_ROT); + + //TODO: migrate to elastic + //sysIdTab.add("ALL THE SYSID TESTS", allTheSYSID())// is this legal?? + //.withSize(2, 1); + + // sysIdTab.add("Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + // sysIdTab.add("Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + // SmartDashboard.putData("Quackson Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse));//.withSize(2, 1); + // SmartDashboard.putData("Quackson Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward));//.withSize(2, 1); + + // SmartDashboard.putData("Dyanmic forward", sysIdDynamic(SysIdRoutine.Direction.kForward));//.withSize(2, 1); + // SmartDashboard.putData("Dyanmic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse));//.withSize(2, 1); + //sysIdTab.add(this); + + for (int i = 0; i < 8; i++) {// first four are drive, next 4 are turn motors + m_appliedVoltage[i] = Volt.mutable(0); + } + for (int i = 0; i < 4; i++) { + m_distance[i] = Meter.mutable(0); + m_velocity[i] = MetersPerSecond.mutable(0); + + m_revs[i] = Rotation.mutable(0); + m_revs_vel[i] = RotationsPerSecond.mutable(0); + } + + // SmartDashboard.putNumber("Desired Angle", 0); + + // SmartDashboard.putNumber("kS", 0); + } + } + + // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyRoutine.quasistatic(direction); + // case 1: + // return backOnlyRoutine.quasistatic(direction); + // case 2: + // return allWheelsRoutine.quasistatic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + + private SysIdTest selector() { + //SysIdTest test = sysIdChooser.getSelected(); + SysIdTest test = SysIdTest.FRONT_DRIVE; + System.out.println("Test Selected: " + test); + return test; + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return new SelectCommand<>( + Map.ofEntries( + // DRIVE + Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running front only quasistatic forward") + : new PrintCommand("Running front only quasistatic backward"), + frontOnlyDriveRoutine.quasistatic(direction))), + Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running back only quasistatic forward") + : new PrintCommand("Running back only quasistatic backward"), + backOnlyDriveRoutine.quasistatic(direction))), + Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running all drive quasistatic forward") + : new PrintCommand("Running all drive quasistatic backward"), + allWheelsDriveRoutine.quasistatic(direction))), + // ROTATE + Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FL rotate quasistatic forward") + : new PrintCommand("Running FL rotate quasistatic backward"), + rotateRoutine[0].quasistatic(direction))), + Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FR rotate quasistatic forward") + : new PrintCommand("Running FR rotate quasistatic backward"), + rotateRoutine[1].quasistatic(direction))), + Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BL rotate quasistatic forward") + : new PrintCommand("Running BL rotate quasistatic backward"), + rotateRoutine[2].quasistatic(direction))), + Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BR rotate quasistatic forward") + : new PrintCommand("Running BR rotate quasistatic backward"), + rotateRoutine[3].quasistatic(direction))) + + // //TURN + // Map.entry(SysIdTest.FLBR_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running fL-bR turn quasistatic forward") : + // new PrintCommand("Running fL-bR turn quasistatic backward"), + // flbrTurn.quasistatic(direction) + // )), + // Map.entry(SysIdTest.FRBL_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running fR-bL turn quasistatic forward") : + // new PrintCommand("Running fR-bL turn quasistatic backward"), + // frblTurn.quasistatic(direction) + // )), + // Map.entry(SysIdTest.ALL_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running all turn quasistatic forward") : + // new PrintCommand("Running all turn quasistatic backward"), + // allWheelsTurn.quasistatic(direction) + // )) + ), + this::selector); + } + + // public Command sysIdDynamic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyDrive.dynamic(direction); + // case 1: + // return backOnlyDrive.dynamic(direction); + // case 2: + // return allWheelsDrive.dynamic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + private Command allTheSYSID(SysIdRoutine.Direction direction) { + return new SequentialCommandGroup( + frontOnlyDriveRoutine.dynamic(direction), + backOnlyDriveRoutine.dynamic(direction), + allWheelsDriveRoutine.dynamic(direction), + rotateRoutine[0].dynamic(direction), + rotateRoutine[1].dynamic(direction), + rotateRoutine[2].dynamic(direction), + rotateRoutine[3].dynamic(direction), + + frontOnlyDriveRoutine.quasistatic(direction), + backOnlyDriveRoutine.quasistatic(direction), + allWheelsDriveRoutine.quasistatic(direction), + rotateRoutine[0].quasistatic(direction), + rotateRoutine[1].quasistatic(direction), + rotateRoutine[2].quasistatic(direction), + rotateRoutine[3].quasistatic(direction)); + } + + /** + * Makes sysId to run for both directions + * @return Command to run sysId + */ + public Command allTheSYSID() { + return new SequentialCommandGroup( + allTheSYSID(SysIdRoutine.Direction.kForward), + allTheSYSID(SysIdRoutine.Direction.kReverse)); + } + /** + * Makes sysId to find feedforward and pid values for drivetrain + * @param direction SysIdRoutine.Direction.kForward or kReverse + * @return Command to run sysID + */ + // public Command sysIdDynamic(SysIdRoutine.Direction direction) { + // return new SelectCommand<>( + // Map.ofEntries( + // // DRIVE + // Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running front only dynamic forward") + // : new PrintCommand("Running front only dynamic backward"), + // frontOnlyDriveRoutine.dynamic(direction))), + // Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running back only dynamic forward") + // : new PrintCommand("Running back only dynamic backward"), + // backOnlyDriveRoutine.dynamic(direction))), + // Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running all wheels dynamic forward") + // : new PrintCommand("Running all wheels dynamic backward"), + // allWheelsDriveRoutine.dynamic(direction))), + // // ROTATE + // Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running FL rotate dynamic forward") + // : new PrintCommand("Running FL rotate dynamic backward"), + // rotateRoutine[0].dynamic(direction))), + // Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running FR rotate dynamic forward") + // : new PrintCommand("Running FR rotate dynamic backward"), + // rotateRoutine[1].dynamic(direction))), + // Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running BL rotate dynamic forward") + // : new PrintCommand("Running BL rotate dynamic backward"), + // rotateRoutine[2].dynamic(direction))), + // Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running BR rotate dynamic forward") + // : new PrintCommand("Running BR rotate dynamic backward"), + // rotateRoutine[3].dynamic(direction)))), + // this::selector); + // } + + // #endregion + + /** + * Sets all SwerveModules to point in a certain angle + * @param angle in degrees + */ + public void keepRotateMotorsAtDegrees(int angle) { + for (SwerveModule module : modules) { + module.turnPeriodic(); + module.move(0, angle); + } + } + + /** + * Gets how fast the robot is spinning from gyro + * @return degrees per second + */ + public double getGyroRate() { + return gyro.getRate(); + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java new file mode 100644 index 0000000..8382052 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java @@ -0,0 +1,52 @@ +package org.carlmontrobotics.lib199.SimpleMechs.Drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class SimpleRotateToFieldRelativeAngle extends Command { + + public final SimpleTeleopDrive teleopDrive; + public final SimpleDrivetrain drivetrain; + + double[] thetaPIDController = {0.1,0,0}; + double positionTolerance = 5; //degrees + double velocityTolerance = 5; //degrees/sec + + public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], + thetaPIDController[2]); + + public SimpleRotateToFieldRelativeAngle(Rotation2d angle, SimpleDrivetrain drivetrain) { + this.drivetrain = drivetrain; + this.teleopDrive = (SimpleTeleopDrive) drivetrain.getDefaultCommand(); + + rotationPID.enableContinuousInput(-180, 180); + rotationPID.setSetpoint(MathUtil.inputModulus(angle.getDegrees(), -180, 180)); + rotationPID.setTolerance(positionTolerance, velocityTolerance); + SendableRegistry.addChild(this, rotationPID); + // SmartDashboard.pu + + addRequirements(drivetrain); + } + + @Override + public void execute() { + if (teleopDrive == null) + drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); + else { + double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); + drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], + rotationPID.calculate(drivetrain.getHeading())); + } + } + + @Override + public boolean isFinished() { + //SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); + //SmartDashboard.putNumber("Error", rotationPID.getPositionError()); + return rotationPID.atSetpoint(); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java new file mode 100644 index 0000000..97ea3ce --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java @@ -0,0 +1,161 @@ +package org.carlmontrobotics.lib199.SimpleMechs.Drivetrain; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SimpleTeleopDrive extends Command { + + private static double robotPeriod = 0.2; + private final SimpleDrivetrain drivetrain; + private DoubleSupplier fwd; + private DoubleSupplier str; + private DoubleSupplier rcw; + private BooleanSupplier slow; + private double currentForwardVel = 0; + private double currentStrafeVel = 0; + private double prevTimestamp; + GenericHID manipulatorController; + BooleanSupplier babyModeSupplier; + + + //Extra + private final double maxForward; + private final double maxStrafe; + private final double maxRCW; + private final double kSlowDriveSpeed = 0.4; + private final double kNormalDriveSpeed = 1; + private final double kSlowDriveRotation = 0.25; + private final double kNormalDriveRotation = 0.5; + private final double kBabyDriveSpeed = 0.3; + private final double kBabyDriveRotation = 0.2; + private final double JOY_THRESH = 0.13; + + + /** + * Creates a new TeleopDrive. + */ + public SimpleTeleopDrive(SimpleDrivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, + BooleanSupplier slow, GenericHID manipulatorController, BooleanSupplier babyModeSupplier, double maxSpeed, double swerveRadius ) { + addRequirements(this.drivetrain = drivetrain); + this.fwd = fwd; + this.str = str; + this.rcw = rcw; + this.slow = slow; + this.manipulatorController = manipulatorController; + this.babyModeSupplier = babyModeSupplier; + maxForward = maxSpeed; + maxStrafe = maxSpeed; + maxRCW = maxSpeed/swerveRadius; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // SmartDashboard.putNumber("slow turn const", kSlowDriveRotation); + // SmartDashboard.putNumber("slow speed const", kSlowDriveSpeed); + // SmartDashboard.putNumber("normal turn const", kNormalDriveRotation); + // SmartDashboard.putNumber("normal speed const", kNormalDriveSpeed); + prevTimestamp = Timer.getFPGATimestamp(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double currentTime = Timer.getFPGATimestamp(); + robotPeriod = currentTime - prevTimestamp; + if (!hasDriverInput()) { + drivetrain.drive(0,0,0); + } + else { + double[] speeds = getRequestedSpeeds(); + // SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp); + prevTimestamp = currentTime; + // kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation); + // kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed); + // kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation); + // kNormalDriveSpeed = SmartDashboard.getNumber("normal speed const", kNormalDriveSpeed); + + // SmartDashboard.putNumber("fwd", speeds[0]); + // SmartDashboard.putNumber("strafe", speeds[1]); + // SmartDashboard.putNumber("turn", speeds[2]); + drivetrain.drive(speeds[0], speeds[1], speeds[2]); + } + } + + public double[] getRequestedSpeeds() { + // Sets all values less than or equal to a very small value (determined by the + // idle joystick state) to zero. + // Used to make sure that the robot does not try to change its angle unless it + // is moving, + double forward = fwd.getAsDouble(); + double strafe = str.getAsDouble(); + double rotateClockwise = rcw.getAsDouble(); + // SmartDashboard.putNumber("fwdIN", forward); + // SmartDashboard.putNumber("strafeIN", strafe); + // SmartDashboard.putNumber("turnIN", rotateClockwise); + // System.out.println("fwd str rcw: "+forward+", "+strafe+", "+rotateClockwise); + boolean slow2 = slow.getAsBoolean(); + forward *= maxForward; + strafe *= maxStrafe; + rotateClockwise *= maxRCW; + + // System.out.println("teleopDrive ExtraSpeedMult%: "+drivetrain.extraSpeedMult); + double driveMultiplier = (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed); + double rotationMultiplier = drivetrain.extraSpeedMult + (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation); + // double driveMultiplier = (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed); + // double rotationMultiplier = (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation); + + if(babyModeSupplier.getAsBoolean()){ + driveMultiplier = kBabyDriveSpeed; + rotationMultiplier = kBabyDriveRotation; + } + // double driveMultiplier = kNormalDriveSpeed; + // double rotationMultiplier = kNormalDriveRotation; + + forward *= driveMultiplier; + strafe *= driveMultiplier; + rotateClockwise *= rotationMultiplier; + + // Limit acceleration of the robot + // double accelerationX = (forward - currentForwardVel) / robotPeriod; + // double accelerationY = (strafe - currentStrafeVel) / robotPeriod; + // double translationalAcceleration = Math.hypot(accelerationX, accelerationY); + // SmartDashboard.putNumber("Translational Acceleration", translationalAcceleration); + // if (translationalAcceleration > autoMaxAccelMps2 && false) {//DOES NOT RUN!! + // Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2, + // Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX))); + // Translation2d limitedVelocityVector = limitedAccelerationVector.times(robotPeriod); + // currentForwardVel += limitedVelocityVector.getX(); + // currentStrafeVel += limitedVelocityVector.getY(); + // } else { + currentForwardVel = forward; + currentStrafeVel = strafe; + // } + // SmartDashboard.putNumber("current velocity", Math.hypot(currentForwardVel, currentStrafeVel)); + + return new double[] { currentForwardVel, currentStrafeVel, -rotateClockwise }; + } + + public boolean hasDriverInput() { + return MathUtil.applyDeadband(fwd.getAsDouble(), JOY_THRESH)!=0 + || MathUtil.applyDeadband(str.getAsDouble(), JOY_THRESH)!=0 + || MathUtil.applyDeadband(rcw.getAsDouble(), JOY_THRESH)!=0; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elastic.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elastic.java new file mode 100644 index 0000000..cf4b521 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elastic.java @@ -0,0 +1,390 @@ +// Copyright (c) 2023-2025 Gold87 and other Elastic contributors +// This software can be modified and/or shared under the terms +// defined by the Elastic license: +// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE + +package org.carlmontrobotics.lib199.SimpleMechs; + +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringTopic; + +public final class Elastic { + private static final StringTopic notificationTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications"); + private static final StringPublisher notificationPublisher = + notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); + private static final StringTopic selectedTabTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab"); + private static final StringPublisher selectedTabPublisher = + selectedTabTopic.publish(PubSubOption.keepDuplicates(true)); + private static final ObjectMapper objectMapper = new ObjectMapper(); + + /** + * Represents the possible levels of notifications for the Elastic dashboard. These levels are + * used to indicate the severity or type of notification. + */ + public enum NotificationLevel { + /** Informational Message */ + INFO, + /** Warning message */ + WARNING, + /** Error message */ + ERROR + } + + /** + * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string + * before being published. + * + * @param notification the {@link Notification} object containing notification details + */ + public static void sendNotification(Notification notification) { + try { + notificationPublisher.set(objectMapper.writeValueAsString(notification)); + } catch (JsonProcessingException e) { + e.printStackTrace(); + } + } + + /** + * Selects the tab of the dashboard with the given name. If no tab matches the name, this will + * have no effect on the widgets or tabs in view. + * + *

If the given name is a number, Elastic will select the tab whose index equals the number + * provided. + * + * @param tabName the name of the tab to select + */ + public static void selectTab(String tabName) { + selectedTabPublisher.set(tabName); + } + + /** + * Selects the tab of the dashboard at the given index. If this index is greater than or equal to + * the number of tabs, this will have no effect. + * + * @param tabIndex the index of the tab to select. + */ + public static void selectTab(int tabIndex) { + selectTab(Integer.toString(tabIndex)); + } + + /** + * Represents an notification object to be sent to the Elastic dashboard. This object holds + * properties such as level, title, description, display time, and dimensions to control how the + * notification is displayed on the dashboard. + */ + public static class Notification { + @JsonProperty("level") + private NotificationLevel level; + + @JsonProperty("title") + private String title; + + @JsonProperty("description") + private String description; + + @JsonProperty("displayTime") + private int displayTimeMillis; + + @JsonProperty("width") + private double width; + + @JsonProperty("height") + private double height; + + /** + * Creates a new Notification with all default parameters. This constructor is intended to be + * used with the chainable decorator methods + * + *

Title and description fields are empty. + */ + public Notification() { + this(NotificationLevel.INFO, "", ""); + } + + /** + * Creates a new Notification with all properties specified. + * + * @param level the level of the notification (e.g., INFO, WARNING, ERROR) + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the time in milliseconds for which the notification is displayed + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, + String title, + String description, + int displayTimeMillis, + double width, + double height) { + this.level = level; + this.title = title; + this.displayTimeMillis = displayTimeMillis; + this.description = description; + this.height = height; + this.width = width; + } + + /** + * Creates a new Notification with default display time and dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + */ + public Notification(NotificationLevel level, String title, String description) { + this(level, title, description, 3000, 350, -1); + } + + /** + * Creates a new Notification with a specified display time and default dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the display time in milliseconds + */ + public Notification( + NotificationLevel level, String title, String description, int displayTimeMillis) { + this(level, title, description, displayTimeMillis, 350, -1); + } + + /** + * Creates a new Notification with specified dimensions and default display time. If the height + * is below zero, it is automatically inferred based on screen size. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, String title, String description, double width, double height) { + this(level, title, description, 3000, width, height); + } + + /** + * Updates the level of this notification + * + * @param level the level to set the notification to + */ + public void setLevel(NotificationLevel level) { + this.level = level; + } + + /** + * @return the level of this notification + */ + public NotificationLevel getLevel() { + return level; + } + + /** + * Updates the title of this notification + * + * @param title the title to set the notification to + */ + public void setTitle(String title) { + this.title = title; + } + + /** + * Gets the title of this notification + * + * @return the title of this notification + */ + public String getTitle() { + return title; + } + + /** + * Updates the description of this notification + * + * @param description the description to set the notification to + */ + public void setDescription(String description) { + this.description = description; + } + + public String getDescription() { + return description; + } + + /** + * Updates the display time of the notification + * + * @param seconds the number of seconds to display the notification for + */ + public void setDisplayTimeSeconds(double seconds) { + setDisplayTimeMillis((int) Math.round(seconds * 1000)); + } + + /** + * Updates the display time of the notification in milliseconds + * + * @param displayTimeMillis the number of milliseconds to display the notification for + */ + public void setDisplayTimeMillis(int displayTimeMillis) { + this.displayTimeMillis = displayTimeMillis; + } + + /** + * Gets the display time of the notification in milliseconds + * + * @return the number of milliseconds the notification is displayed for + */ + public int getDisplayTimeMillis() { + return displayTimeMillis; + } + + /** + * Updates the width of the notification + * + * @param width the width to set the notification to + */ + public void setWidth(double width) { + this.width = width; + } + + /** + * Gets the width of the notification + * + * @return the width of the notification + */ + public double getWidth() { + return width; + } + + /** + * Updates the height of the notification + * + *

If the height is set to -1, the height will be determined automatically by the dashboard + * + * @param height the height to set the notification to + */ + public void setHeight(double height) { + this.height = height; + } + + /** + * Gets the height of the notification + * + * @return the height of the notification + */ + public double getHeight() { + return height; + } + + /** + * Modifies the notification's level and returns itself to allow for method chaining + * + * @param level the level to set the notification to + * @return the current notification + */ + public Notification withLevel(NotificationLevel level) { + this.level = level; + return this; + } + + /** + * Modifies the notification's title and returns itself to allow for method chaining + * + * @param title the title to set the notification to + * @return the current notification + */ + public Notification withTitle(String title) { + setTitle(title); + return this; + } + + /** + * Modifies the notification's description and returns itself to allow for method chaining + * + * @param description the description to set the notification to + * @return the current notification + */ + public Notification withDescription(String description) { + setDescription(description); + return this; + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param seconds the number of seconds to display the notification for + * @return the current notification + */ + public Notification withDisplaySeconds(double seconds) { + return withDisplayMilliseconds((int) Math.round(seconds * 1000)); + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param displayTimeMillis the number of milliseconds to display the notification for + * @return the current notification + */ + public Notification withDisplayMilliseconds(int displayTimeMillis) { + setDisplayTimeMillis(displayTimeMillis); + return this; + } + + /** + * Modifies the notification's width and returns itself to allow for method chaining + * + * @param width the width to set the notification to + * @return the current notification + */ + public Notification withWidth(double width) { + setWidth(width); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + * @param height the height to set the notification to + * @return the current notification + */ + public Notification withHeight(double height) { + setHeight(height); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + *

This will set the height to -1 to have it automatically determined by the dashboard + * + * @return the current notification + */ + public Notification withAutomaticHeight() { + setHeight(-1); + return this; + } + + /** + * Modifies the notification to disable the auto dismiss behavior + * + *

This sets the display time to 0 milliseconds + * + *

The auto dismiss behavior can be re-enabled by setting the display time to a number + * greater than 0 + * + * @return the current notification + */ + public Notification withNoAutoDismiss() { + setDisplayTimeMillis(0); + return this; + } + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java new file mode 100644 index 0000000..914c63b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java @@ -0,0 +1,191 @@ +package org.carlmontrobotics.lib199.SimpleMechs.Elevator; +import java.util.function.BooleanSupplier; + +import org.carlmontrobotics.lib199.MotorConfig; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DriverStation; + +public final class ElevatorConfig { + public final int elevatorMasterId; + public final int elevatorFollowId; //make your own config if you have more than 2 motors + public final MotorConfig elevatorMasterMotorType; + public final MotorConfig elevatorFollowMotorType; + public final boolean elevatorMasterInverted; + public final boolean elevatorFollowInverted; + public final BooleanSupplier bottomReset; + public final BooleanSupplier topReset; + public final double topLimit; //meters + public final double bottomLimit; //meters + public final double gearReduction; //rotations to meters + public double[] elevatorKPID; + public double[] elevatorKFeedForward; + public double elevatorPIDTolerance; + public double maxVolts; //14 for no limit + public double maxManualInput; //percentage of power + + public double elevatorKP; + public double elevatorKI; + public double elevatorKD; + public double elevatorKS; + public double elevatorKG; + public double elevatorKV; + public double elevatorKA; + + public boolean elevatorPIDExists; + public boolean elevatorFeedForwardExists; + + public PIDController elevatorPID; + public ElevatorFeedforward elevatorFeedforward; + + + /** + * Creates a elevator config for a {@link SimpleElevator} + * @param elevatorMasterId the port id of the master motor, int + * @param elevatorMasterMotorType the {@link MotorConfig} of the master motor, that is NEO_550, NEO, and NEO_VORTEX + * @param elevatorMasterInverted setting for positive direction of the master motor would make the elevator go up, boolean + * @param elevatorFollowId the port id for the follow motor, int + * @param elevatorFollowMotorType the {@link MotorConfig} of the follow motor, that is NEO_550, NEO, and NEO_VORTEX, generally should be the same as the master + * @param elevatorFollowInverted setting for positive direction of teh follow motor would make the elevator go up, boolean + * @param bottomReset a {@link BooleanSupplier}, that would be true if the elevator is fully down (Limit switch), null if none + * @param topReset a {@link BooleanSupplier}, that would be true if the elevator is fully up(Limit switch), null if none + * @param bottomLimit the lowest value possible the elevator can achieve based of encoder readings in meters, generally 0 + * @param topLimit the highest value possible the elevator can achieve based of encoder readings in meters + * @param elevatorKPID double array of [kP, kI, kD] have to be >= 0 + * @param elevatorKFeedForward double array of [kS, kG, kV, kA] all but kG have to be >= 0 + * @param gearReduction # of rotations of the motor to raise the elevator 1 meter (Elevator must have linear increase in height) + * @param elevatorPIDTolerance double, how many METERS of tolerance is the user fine with + * @param maxVolts maximum volts to use on the motors, 14 if no limit + * @param maxManualInput maximum percentage of how much voltage a user can use from 0-1.0 + * @throws IllegalArgumentException + * @throws IllegalArgumentException + * @throws IllegalArgumentException + */ + public ElevatorConfig(int elevatorMasterId, MotorConfig elevatorMasterMotorType, boolean elevatorMasterInverted, + int elevatorFollowId, MotorConfig elevatorFollowMotorType, boolean elevatorFollowInverted, + BooleanSupplier bottomReset, BooleanSupplier topReset, double bottomLimit, double topLimit, + double[] elevatorKPID, double[] elevatorKFeedForward, + double gearReduction, double elevatorPIDTolerance, double maxVolts, double maxManualInput) { + + this.elevatorMasterId = elevatorMasterId; + this.elevatorMasterMotorType = elevatorMasterMotorType; + this.elevatorMasterInverted = elevatorMasterInverted; + this.elevatorFollowId = elevatorFollowId; + this.elevatorFollowMotorType = elevatorFollowMotorType; + this.elevatorFollowInverted = elevatorFollowInverted; + this.bottomReset = bottomReset; + this.topReset = topReset; + this.bottomLimit = bottomLimit; + this.topLimit = topLimit; + this.gearReduction = gearReduction; + this.elevatorPIDTolerance = elevatorPIDTolerance; + this.maxVolts = maxVolts; + this.maxManualInput = maxManualInput; + this.elevatorKPID = elevatorKPID; + this.elevatorKFeedForward = elevatorKFeedForward; + checkRequirements(); + } + + public static ElevatorConfig NEOElevatorConfig(int elevatorMasterId, boolean elevatorMasterInverted, int elevatorFollowId, boolean elevatorFollowInverted, BooleanSupplier bottomReset, BooleanSupplier topReset, double topLimit, double[] elevatorKPID, double[] elevatorKFeedForward, double gearReduction, double maxManualInput) { + return new ElevatorConfig(elevatorMasterId, MotorConfig.NEO, elevatorMasterInverted, elevatorFollowId, MotorConfig.NEO, elevatorFollowInverted, bottomReset, topReset, 0, topLimit, elevatorKPID, elevatorKFeedForward, gearReduction, 0.02, 14, maxManualInput); + } + + public static ElevatorConfig VortexElevatorConfig(int elevatorMasterId, boolean elevatorMasterInverted, int elevatorFollowId, boolean elevatorFollowInverted, BooleanSupplier bottomReset, BooleanSupplier topReset, double topLimit, double[] elevatorKPID, double[] elevatorKFeedForward, double gearReduction, double maxManualInput) { + return new ElevatorConfig(elevatorMasterId, MotorConfig.NEO_VORTEX, elevatorMasterInverted, elevatorFollowId, MotorConfig.NEO_VORTEX, elevatorFollowInverted, bottomReset, topReset, 0, topLimit, elevatorKPID, elevatorKFeedForward, gearReduction, 0.02, 14, maxManualInput); + } + + /** + * Checks the elevatorConfig to not have any issues, and creates certain extra values automatically like {@link #elevatorPID} and {@link #elevatorFeedforward} + */ + private void checkRequirements() { + checkMotorConfigs(); //Prevent future issues with new motors + checkPID(); + checkFeedForward(); + checkGearReduction(); + checkLimits(); + } + + /** + * Check that the motor Configs are not any new configs that would not work or null. + */ + private void checkMotorConfigs() { + if (elevatorFollowMotorType != MotorConfig.NEO && elevatorFollowMotorType != MotorConfig.NEO_VORTEX && elevatorFollowMotorType != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + if (elevatorMasterMotorType != MotorConfig.NEO && elevatorMasterMotorType != MotorConfig.NEO_VORTEX && elevatorMasterMotorType != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + } + + /** + * Checks PID to have 3 values, updates {@link #elevatorPIDExists} and makes sure the values are legitimate + */ + private void checkPID() { + if (elevatorKPID != null) { + if (elevatorKPID.length == 3) { + if (elevatorKPID[0] < 0 || elevatorKPID[1] < 0 || elevatorKPID[2] < 0) { + throw new IllegalArgumentException("PID values have to be non negative"); + } + elevatorKP = elevatorKPID[0]; + elevatorKI = elevatorKPID[1]; + elevatorKD = elevatorKPID[2]; + elevatorPID = new PIDController(elevatorKP, elevatorKI, elevatorKD); + elevatorPID.setTolerance(elevatorPIDTolerance); + elevatorPIDExists = true; + return; + } + throw new IllegalArgumentException("Need to have 3 values for PID"); + } + elevatorKP = 0; + elevatorKI = 0; + elevatorKD = 0; + elevatorPIDExists = false; + DriverStation.reportWarning("Elevator PID is off", true); + } + + /** + * Checks FeedForward to have 4 values, updates {@link #elevatorFeedForwardExists} and makes sure the values are legitimate + */ + private void checkFeedForward() { + if (elevatorKFeedForward != null) { + if (elevatorKFeedForward.length == 4) { + if (elevatorKFeedForward[0] < 0|| elevatorKFeedForward[2] < 0 || elevatorKFeedForward[3] < 0) { + throw new IllegalArgumentException("FeedForward Values of kS, kV, and kA need to have non negative values"); + } + elevatorKS = elevatorKFeedForward[0]; + elevatorKG = elevatorKFeedForward[1]; + elevatorKV = elevatorKFeedForward[2]; + elevatorKA = elevatorKFeedForward[3]; + elevatorFeedforward = new ElevatorFeedforward(elevatorKS, elevatorKG, elevatorKV, elevatorKA); + elevatorFeedForwardExists = true; + return; + } + throw new IllegalArgumentException("Need to have 4 values for Elevator FeedForward: Friction, Gravity, Voltage, and Acceleration"); + } + elevatorKS = 0; + elevatorKG = 0; + elevatorKV = 0; + elevatorKA = 0; + elevatorFeedForwardExists = false; + DriverStation.reportWarning("ElevatorFeedForward is off", true); + } + + /** + * Makes sure gear reduction is not 0 + */ + private void checkGearReduction() { + if (gearReduction == 0) { + throw new IllegalArgumentException("Gear reduction cannot be 0"); + } + } + + /** + * Makes sure that the user has not mixed up the top and bottom limits. + */ + private void checkLimits() { + if (bottomLimit >= topLimit) { + throw new IllegalArgumentException("Top limit must be much greater than bottom limit. (Cannot be equal)"); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java new file mode 100644 index 0000000..c268fd7 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java @@ -0,0 +1,576 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.lib199.SimpleMechs.Elevator; + + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.SimpleMechs.Elastic; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.MathUtil; +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.units.measure.Voltage; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Seconds; + +public class SimpleElevator extends SubsystemBase { + /** Creates a new Elevator. */ + private final ElevatorConfig elevatorConfig; + private SparkBase masterMotor; + private SparkBase followMotor; + private RelativeEncoder feedbackEncoder; + private RelativeEncoder backupEncoder; + + private double elevatorGoal; + private double latestManualInput; // in percentage of power (-1, 1) + + private ElevatorControlState currentState = ElevatorControlState.AUTO; + private IdleMode elevatorIdleMode = IdleMode.kBrake; + + private double currentDif = 0; + private final double difDanger = 0.03; + + private double currentStamp; + private double pastStamp; + private double currentPeriod = 0.02; + + private boolean encoderFaultReported = false; + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutDistance m_distance = Meters.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0); + + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config( + Volts.of(1).per(Seconds),//ramp rate, volts/sec + Volts.of(1), //starting voltage, volts + Seconds.of(5)//maximum sysID test time + ); + + + private final SysIdRoutine sysIdRoutine; + + /** + * Different control states for the elevator + * {@link #AUTO} runs the elevator autonomously to go to a certain height + * {@link #MANUAL} allows the elevator to be manually run by the user with soft locks, and speed limits + * {@link #BRAKE} idles the elevator in brake, which in cases will cause the elevator to slowly drift downwards due to gravity + * {@link #COAST} idles the elevator in coast allowing for easy movement of the elevator during disable mode, will likely cause elevator to drop very fast though + */ + public enum ElevatorControlState { + AUTO, + MANUAL, + BRAKE, + COAST + } + + /** + * Creates an object of a simple elevator, this class can be extended for more complicated use. + * @param elevatorConfig {@link ElevatorConfig} for all needs of the elevator + */ + public SimpleElevator(ElevatorConfig elevatorConfig) { + this.elevatorConfig = elevatorConfig; + configureMotors(); + resetFeedbackEncoder(); + sysIdRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + voltage -> { + manualElevator(voltage); + }, + log -> { + log.motor("Elevator-Master") + .voltage( + m_appliedVoltage + .mut_replace(masterMotor.getBusVoltage() * masterMotor.getAppliedOutput(), Volts)) + .linearPosition(m_distance.mut_replace(feedbackEncoder.getPosition(), Meters)) + .linearVelocity(m_velocity.mut_replace(feedbackEncoder.getVelocity(), MetersPerSecond)); + }, + this) + ); + + if (isSysIdTesting()) { + sysIdSetup(); + } + SmartDashboard.putData(this); + } + + /** + * Sets up commands in SmartDashboard/Elastic to be ran to record data for mechanism modeling + */ + private void sysIdSetup() { + Elastic.Notification notification = new Elastic.Notification(Elastic.NotificationLevel.INFO, "SysId Testing On", "Please add Elevator Quasistatic and Dynamics and begin testing.").withDisplaySeconds(5); + Elastic.sendNotification(notification); + Elastic.selectTab("SysId"); + SmartDashboard.putData("Elevator Quasistatic Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + SmartDashboard.putData("Elevator Quasistatic Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Elevator Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Elevator Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } + + + /** Override this method and return true if you want to do sysid testing, return false if you don't + * @return false to do sysid testing + */ + protected boolean isSysIdTesting() { + return false; + } + + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction); + } + + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction); + } + + /** + * Sets {@link #elevatorGoal} + * @param goal in meters + */ + public void setGoal(double goal){ + if (goal <= elevatorConfig.topLimit && goal >= elevatorConfig.bottomLimit) {elevatorGoal = goal;} + } + + /** + * @return current {@link #elevatorGoal} in meters + */ + public double getGoal() { + return elevatorGoal; + } + + /** + * Checks if at {@link #elevatorGoal} + * @return true if at {@link #elevatorGoal} + */ + public boolean atGoal() { + return Math.abs(elevatorGoal - feedbackEncoder.getPosition()) <= elevatorConfig.elevatorPIDTolerance; + } + + /** + * Updates {@link #latestManualInput} to manually control elevator + * @param input voltage percentage from -1.0 - 1.0 + */ + public void setManualInput(double input) { + latestManualInput = input; + } + + /** + * @return {@link #latestManualInput} a value between -1.0 - 1.0, inclusive + */ + public double getLatestManualInput() { + return latestManualInput; + } + + /** + * @return {@link ElevatorConfig} of the object + */ + public ElevatorConfig getConfig() { + return elevatorConfig; + } + + /** + * @return the current {@link ElevatorControlState} + */ + public ElevatorControlState getCurrentControlState() { + return currentState; + } + + /** + * Sets {@link #currentState} to the inputed state + * @param controlState An {@link ElevatorControlState} + */ + public void setElevatorControlState(ElevatorControlState controlState) { + switch (controlState) { + case AUTO -> setAutoOn(); + case MANUAL -> setManualOn(); + case BRAKE -> setBrakeOn(); + case COAST -> setCoastOn(); + default -> DriverStation.reportWarning("Such control mode has not been implemented yet", true); + } + } + + /** + * Sets {@link #currentState} to be in AUTO mode + */ + public void setAutoOn() { + if (elevatorConfig.elevatorPIDExists || elevatorConfig.elevatorFeedForwardExists) { + currentState = ElevatorControlState.AUTO; + } + else { + DriverStation.reportWarning("Any sort of autonomous control mode is disabled due to no PID or FeedForward", true); + } + } + + /** + * Sets {@link #currentState} to be in MANUAL mode + */ + public void setManualOn() { + currentState = ElevatorControlState.MANUAL; + } + + /** + * Sets {@link #currentState} to be in BRAKE mode, also changes the idle mode of the motors to brake. + */ + public void setBrakeOn() { + currentState = ElevatorControlState.BRAKE; + if (elevatorIdleMode == IdleMode.kCoast) { + elevatorIdleMode = IdleMode.kBrake; + brakeMotors(); + + } + } + + /** + * Sets {@link #currentState} to be in COAST mode, also changes the idle mode of the motors to coast. + */ + public void setCoastOn() { + currentState = ElevatorControlState.COAST; + if (elevatorIdleMode == IdleMode.kBrake) { + elevatorIdleMode = IdleMode.kCoast; + coastMotors(); + } + } + + /** + * Set all motors idleModes to brake + */ + private void brakeMotors() { + SparkFlexConfig flexBrakeConfig = new SparkFlexConfig(); + SparkMaxConfig maxBrakeConfig = new SparkMaxConfig(); + + flexBrakeConfig.idleMode(IdleMode.kBrake); + maxBrakeConfig.idleMode(IdleMode.kBrake); + if (elevatorConfig.elevatorFollowMotorType == MotorConfig.NEO_VORTEX) { + followMotor.configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + followMotor.configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + if (elevatorConfig.elevatorMasterMotorType == MotorConfig.NEO_VORTEX) { + masterMotor.configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + masterMotor.configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + /** + * Set all motors idleModes to coast + */ + private void coastMotors() { + SparkFlexConfig flexCoastConfig = new SparkFlexConfig(); + SparkMaxConfig maxCoastConfig = new SparkMaxConfig(); + + flexCoastConfig.idleMode(IdleMode.kCoast); + maxCoastConfig.idleMode(IdleMode.kCoast); + if (elevatorConfig.elevatorFollowMotorType == MotorConfig.NEO_VORTEX) { + followMotor.configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + followMotor.configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + if (elevatorConfig.elevatorMasterMotorType == MotorConfig.NEO_VORTEX) { + masterMotor.configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + masterMotor.configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + /** + * Called periodically, checks for any issues with the main encoder by comparing it with the backup encoder + */ + protected void checkFeedBackEncoder() { + currentDif = feedbackEncoder.getPosition() - backupEncoder.getPosition(); + if (Math.abs(currentDif) > difDanger && !encoderFaultReported) { + DriverStation.reportWarning("Elevator encoder seems to be off!", true); + encoderFaultReported = true; + } + } + + /** + * @return current elevator height in meters + */ + public double getElevatorHeight() { + return feedbackEncoder.getPosition(); + } + + /** + * @return current elevator velocity in meters per second + */ + public double getElevatorVelocity() { + return feedbackEncoder.getVelocity(); + } + + + /** + * Called in constructor. + * Configures the master and follow motors in brake mode and makes follow motor to follow the master. + * Also configures encoders to read in meters, and meters per second for position and velocity respectively + */ + protected void configureMotors() { + if (elevatorConfig.elevatorMasterMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig masterFlexConfig = new SparkFlexConfig(); + masterFlexConfig.idleMode(IdleMode.kBrake) + .inverted(elevatorConfig.elevatorMasterInverted) + .encoder.positionConversionFactor(elevatorConfig.gearReduction) + .velocityConversionFactor(elevatorConfig.gearReduction/60); + masterMotor = MotorControllerFactory.createSparkFlex(elevatorConfig.elevatorMasterId); + masterMotor.configure(masterFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + SparkMaxConfig masterMaxConfig = new SparkMaxConfig(); + masterMaxConfig.idleMode(IdleMode.kBrake) + .inverted(elevatorConfig.elevatorMasterInverted) + .encoder.positionConversionFactor(elevatorConfig.gearReduction) + .velocityConversionFactor(elevatorConfig.gearReduction/60); + masterMotor = MotorControllerFactory.createSparkMax(elevatorConfig.elevatorMasterId, elevatorConfig.elevatorMasterMotorType); + masterMotor.configure(masterMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + feedbackEncoder = masterMotor.getEncoder(); + + if (elevatorConfig.elevatorFollowMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig followFlexConfig = new SparkFlexConfig(); + followFlexConfig.idleMode(IdleMode.kBrake) + .inverted(elevatorConfig.elevatorFollowInverted) + .follow(elevatorConfig.elevatorMasterId, (elevatorConfig.elevatorMasterInverted != elevatorConfig.elevatorFollowInverted)) + .encoder.positionConversionFactor(elevatorConfig.gearReduction) + .velocityConversionFactor(elevatorConfig.gearReduction/60); + followMotor = MotorControllerFactory.createSparkFlex(elevatorConfig.elevatorFollowId); + followMotor.configure(followFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + SparkMaxConfig followMaxConfig = new SparkMaxConfig(); + followMaxConfig.idleMode(IdleMode.kBrake) + .follow(elevatorConfig.elevatorMasterId, (elevatorConfig.elevatorMasterInverted != elevatorConfig.elevatorFollowInverted)) + .inverted(elevatorConfig.elevatorFollowInverted) + .encoder.positionConversionFactor(elevatorConfig.gearReduction) + .velocityConversionFactor(elevatorConfig.gearReduction/60); + followMotor = MotorControllerFactory.createSparkMax(elevatorConfig.elevatorFollowId, elevatorConfig.elevatorFollowMotorType); + followMotor.configure(followMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + backupEncoder = followMotor.getEncoder(); + } + + /** + * Called periodically if in AUTO mode, will approach the current active {@link #elevatorGoal} using PID and FeedForward if such are provided + */ + protected void autoElevator() { + double pidOutput; + double feedforwardOutput; + double curPosition = feedbackEncoder.getPosition(); + if (elevatorConfig.elevatorPIDExists) { + pidOutput = elevatorConfig.elevatorPID.calculate(curPosition, elevatorGoal); + } + else { + pidOutput = 0; + } + + if (elevatorConfig.elevatorFeedForwardExists) { + feedforwardOutput = elevatorConfig.elevatorFeedforward.calculate(0); //Voltage to hold at desired spot + } + else { + feedforwardOutput = 0; + } + if (curPosition < elevatorConfig.topLimit) { + if (curPosition > elevatorConfig.bottomLimit) { + masterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, -elevatorConfig.maxVolts, elevatorConfig.maxVolts)); + } + else { + masterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, 0, elevatorConfig.maxVolts)); + } + } + else { + masterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, -elevatorConfig.maxVolts, 0)); + } + } + + /** + * Called periodically if in MANUAL mode, will use {@link #latestManualInput} to move the elevator using voltage percentage with soft locking to avoid elevator damage + */ + protected void manualElevator() { + double curPosition = feedbackEncoder.getPosition(); + if (curPosition < elevatorConfig.topLimit) { + if (curPosition > elevatorConfig.bottomLimit) { + masterMotor.set(MathUtil.clamp(latestManualInput, -elevatorConfig.maxManualInput, elevatorConfig.maxManualInput)); + } + else { + masterMotor.set(MathUtil.clamp(latestManualInput, 0, elevatorConfig.maxManualInput)); + } + } + else { + masterMotor.set(MathUtil.clamp(latestManualInput, -elevatorConfig.maxManualInput, 0)); + } + } + + /** + * Mainly used by SysId to characterize an elevator model + * @param volts Voltage unit input + */ + protected void manualElevator(Voltage volts) { + double curPosition = feedbackEncoder.getPosition(); + double voltage = volts.in(Volts); + if (curPosition < elevatorConfig.topLimit) { + if (curPosition > elevatorConfig.bottomLimit) { + masterMotor.setVoltage(MathUtil.clamp(voltage, -12, 12)); + } + else { + masterMotor.setVoltage(MathUtil.clamp(voltage, 0, 12)); + } + } + else { + masterMotor.set(MathUtil.clamp(voltage,-12, 0)); + } + } + + /** + * Sets voltage to 0 + */ + public void stopElevator() { + masterMotor.set(0); + } + + /** + * Called in constructor + * Resets the feedback encoder to the lowest point the elevator can achieve + */ + public void resetFeedbackEncoder() { + resetFeedbackEncoder(elevatorConfig.bottomLimit); + } + + /** + * Allow for manual reseting + * @param position resets to this position in meters + */ + public void resetFeedbackEncoder(double position) { + if (position >= elevatorConfig.bottomLimit && position <= elevatorConfig.topLimit) { + feedbackEncoder.setPosition(position); + backupEncoder.setPosition(position); + } + else { + DriverStation.reportWarning("Cannot reset position to be outside the limits", true); + } + } + + /** + * Checks boolean suppliers for any bottom or top limit switches to update the feedbackEncoder in case it drifted a bit + */ + protected void updateFeedbackEncoder() { + if (elevatorConfig.bottomReset != null) { + if (elevatorConfig.bottomReset.getAsBoolean()) { + feedbackEncoder.setPosition(elevatorConfig.bottomLimit); + } + } + if (elevatorConfig.topReset != null) { + if (elevatorConfig.topReset.getAsBoolean()) { + feedbackEncoder.setPosition(elevatorConfig.topLimit); + } + } + } + + /** + * Use this to do any fun stuff you want to do, for elevatorSubsystemPeriodic + */ + protected void userPeriodic() { + + } + + /** + * @return current calculated period in seconds + */ + protected double getCurrentPeriod() { + return currentPeriod; + } + + /** + * Posts important information about elevator into smartdashboard + */ + protected void postSmartDashboardValues() { + SmartDashboard.putBoolean("At Goal", atGoal()); + SmartDashboard.putNumber("Goal(m)", elevatorGoal); + SmartDashboard.putNumber("Elevator Height(m)", getElevatorHeight()); + SmartDashboard.putNumber("Elevator Velocity (m/s)", getElevatorVelocity()); + SmartDashboard.putNumber("Latest manual input", latestManualInput); + SmartDashboard.putNumber("Current period of Elevator (ms)", currentPeriod*1000); + switch (currentState) { + case AUTO -> SmartDashboard.putString("elevatorControlState", "Autonomous"); + case MANUAL -> SmartDashboard.putString("elevatorControlState", "Manual"); + case BRAKE -> SmartDashboard.putString("elevatorControlState", "Idling: Brake"); + case COAST -> SmartDashboard.putString("elevatorControlState", "Idling: Coast"); + } + } + + /** + * Checks Subsystem periodic for running under expected time limits, reports inconsitencies to user, also is usable by feedforward + */ + private void checkPeriodic() { + if (pastStamp == 0) { + pastStamp = Timer.getFPGATimestamp(); + return; + } + currentStamp = Timer.getFPGATimestamp(); + currentPeriod = currentStamp - pastStamp; + pastStamp = currentStamp; + if (currentPeriod > 0.02) { + DriverStation.reportWarning("Elevator Periodic is slow: " + currentPeriod *1000 + " ms. Check for any lag reasons", true); + } + } + + /*Overide these methods to customize what the mechanism does in different states */ + protected void handleAuto() { autoElevator();} + protected void handleManual() { manualElevator();} + protected void handleBrake() { stopElevator();} + protected void handleCoast() { stopElevator();} + + /** + * DO NOT OVERRIDE THIS PERIODIC UNLESS YOU KNOW WHAT YOU ARE DOING! + * Use {@link #userPeriodic()} instead. + */ + @Override + public void periodic() { + switch (currentState) { + case AUTO -> handleAuto(); + case MANUAL -> handleManual(); + case BRAKE -> handleBrake(); + case COAST -> handleCoast(); + } + checkFeedBackEncoder(); + updateFeedbackEncoder(); + userPeriodic(); + postSmartDashboardValues(); + checkPeriodic(); + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleManualElevator.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleManualElevator.java new file mode 100644 index 0000000..29e9f61 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleManualElevator.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.lib199.SimpleMechs.Elevator; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SimpleManualElevator extends Command { + private SimpleElevator elevator; + private DoubleSupplier manualInput; + private boolean remainAtGoal; + /** Creates a new SimpleManualElevator. */ + public SimpleManualElevator(SimpleElevator elevator, DoubleSupplier manualInput, boolean remainAtGoal) { + addRequirements(this.elevator = elevator); + this.manualInput = manualInput; + this.remainAtGoal = remainAtGoal; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + elevator.setManualOn(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + elevator.setManualInput(manualInput.getAsDouble()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (remainAtGoal) { + elevator.setGoal(elevator.getElevatorHeight()); + elevator.setAutoOn(); + } + else { + elevator.setBrakeOn(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleRaiseElevator.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleRaiseElevator.java new file mode 100644 index 0000000..d6ad0ee --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleRaiseElevator.java @@ -0,0 +1,73 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.lib199.SimpleMechs.Elevator; + +import org.carlmontrobotics.lib199.SimpleMechs.Elevator.SimpleElevator.ElevatorControlState; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SimpleRaiseElevator extends Command { + private SimpleElevator elevator; + private double elevatorGoal; + private boolean useMechanicalLimit; + private boolean commandComplete = false; + private double safeMechanicalUsageSpeed = 0.1; + + /** Creates a new SimpleRaiseElevator. */ + public SimpleRaiseElevator(SimpleElevator elevator, double height, boolean useMechanicalLimit) { + addRequirements(this.elevator = elevator); + this.elevatorGoal = height; + this.useMechanicalLimit = useMechanicalLimit; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + elevator.setGoal(elevatorGoal); + elevator.setAutoOn(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (elevator.atGoal()) { + if (elevator.getConfig().bottomLimit == elevatorGoal && useMechanicalLimit) { + if (elevator.getElevatorHeight() <= elevatorGoal) {commandComplete = true;} + if (elevator.getCurrentControlState() != ElevatorControlState.MANUAL) { + elevator.setManualOn(); + } + elevator.setManualInput(-safeMechanicalUsageSpeed); + } + else if (elevator.getConfig().topLimit == elevatorGoal && useMechanicalLimit) { + if (elevator.getElevatorHeight() >= elevatorGoal) {commandComplete = true;} + if (elevator.getCurrentControlState() != ElevatorControlState.MANUAL) { + elevator.setManualOn(); + } + elevator.setManualInput(safeMechanicalUsageSpeed); + } + else { + commandComplete = true; + } + } + else { + if (elevator.getCurrentControlState() != ElevatorControlState.AUTO) { + elevator.setAutoOn(); + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + elevator.setAutoOn(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return commandComplete; + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinSpinner.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinSpinner.java new file mode 100644 index 0000000..e16d810 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinSpinner.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.lib199.SimpleMechs.Spinner; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SimpleSpinSpinner extends Command { + /** Creates a new SimpleSpinSpinner. */ + SimpleSpinner spinner; + double velocity; + public SimpleSpinSpinner(SimpleSpinner spinner, double velocity) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.spinner = spinner); + this.velocity = velocity; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + spinner.setAutoOn(); + spinner.setGoal(velocity); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinner.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinner.java new file mode 100644 index 0000000..f851faa --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinner.java @@ -0,0 +1,526 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.lib199.SimpleMechs.Spinner; + + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.SimpleMechs.Elastic; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularAcceleration; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Seconds; + +public class SimpleSpinner extends SubsystemBase { + private final SpinnerConfig spinnerConfig; + private SparkBase masterMotor; + private SparkBase followMotor; + private RelativeEncoder feedbackEncoder; + private RelativeEncoder backupEncoder; + private double spinnerGoal; + private double latestManualInput; // in percentage of power (-1, 1) + + private SpinnerControlState currentState = SpinnerControlState.AUTO; + private IdleMode spinnerIdleMode = IdleMode.kBrake; + + private double currentDif = 0; + private final double difDanger = 0.2; + + private double currentStamp = 0; + private double pastStamp = 0; + private double currentPeriod = 0.02; + + private double currentAcceleration = 0.0; + private double previousVelocity = 0; + + private boolean encoderFaultReported = false; + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutAngle m_angle = Radians.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutAngularVelocity m_velocity = RadiansPerSecond.mutable(0); + // Mutable holder for unit-safe linear accleration values, persisted to avoid reallocation + private final MutAngularAcceleration m_accleration = RadiansPerSecondPerSecond.mutable(0); + + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config( + Volts.of(1).per(Seconds),//ramp rate, volts/sec + Volts.of(1), //starting voltage, volts + Seconds.of(12)//maximum sysID test time + ); + + + private final SysIdRoutine sysIdRoutine; + + + /** + * Different control states for the spinner + * {@link #AUTO} runs the spinner autonomously to go to a certain speed + * {@link #MANUAL} allows the spinner to be manually run by the user with speed limits + * {@link #BRAKE} idles the spinner in brake which hold the spinner a certain position + * {@link #COAST} idles the spinner in coast allowing for easy movement + */ + public enum SpinnerControlState { + AUTO, + MANUAL, + BRAKE, + COAST + } + + public SimpleSpinner(SpinnerConfig spinnerConfig) { + this.spinnerConfig = spinnerConfig; + configureMotors(); + resetFeedbackEncoder(); + sysIdRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + voltage -> { + manualSpinner(voltage); + }, + log -> { + log.motor("Spinner-Master") + .voltage( + m_appliedVoltage + .mut_replace(masterMotor.getBusVoltage() * masterMotor.getAppliedOutput(), Volts)) + .angularPosition(m_angle.mut_replace(feedbackEncoder.getPosition()*Math.PI*2, Radians)) + .angularVelocity(m_velocity.mut_replace(getSpinnerVelocity()*Math.PI*2, RadiansPerSecond)) + .angularAcceleration(m_accleration.mut_replace(getSpinnerAccleration()*Math.PI*2, RadiansPerSecondPerSecond)); + }, + this) + ); + + if (isSysIdTesting()) { + sysIdSetup(); + } + SmartDashboard.putData(this); + } + + /** + * Sets up commands in SmartDashboard/Elastic to be ran to record data for mechanism modeling + */ + private void sysIdSetup() { + Elastic.Notification notification = new Elastic.Notification(Elastic.NotificationLevel.INFO, "SysId Testing On", "Please add Spinner Quasistatic and Dynamics and begin testing.").withDisplaySeconds(5); + Elastic.sendNotification(notification); + Elastic.selectTab("SysId"); + SmartDashboard.putData("Spinner Quasistatic Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + SmartDashboard.putData("Spinner Quasistatic Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Spinner Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Spinner Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } + + + /** Override this method and return true if you want to do sysid testing, return false if you don't + * @return false to do sysid testing + */ + protected boolean isSysIdTesting() { + return false; + } + + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction); + } + + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction); + } + + /** + * Sets {@link #spinnerGoal} + * @param goal in rotations per second + */ + public void setGoal(double goal){ + spinnerGoal = goal; + } + + /** + * @return current {@link #spinnerGoal} in rotations per second + */ + public double getGoal() { + return spinnerGoal; + } + + /** + * Checks if at {@link #spinnerGoal} + * @return true if at {@link #spinnerGoal} + */ + public boolean atGoal() { + return Math.abs(spinnerGoal - feedbackEncoder.getVelocity()) <= spinnerConfig.spinnerPIDTolerance; + } + + /** + * Updates {@link #latestManualInput} to manually control spinner + * @param input voltage percentage from -1.0 - 1.0 + */ + public void setManualInput(double input) { + latestManualInput = input; + } + + /** + * @return {@link #latestManualInput} a value from -1.0 - 1.0 + */ + public double getLatestManualInput() { + return latestManualInput; + } + + /** + * @return {@link SpinnerConfig} of the object + */ + public SpinnerConfig getConfig() { + return spinnerConfig; + } + + /** + * Sets {@link #currentState} to the inputed state + * @param controlState An {@link SpinnerControlState} + */ + public void setSpinnerControlState(SpinnerControlState controlState) { + switch (controlState) { + case AUTO -> setAutoOn(); + case MANUAL -> setManualOn(); + case BRAKE -> setBrakeOn(); + case COAST -> setCoastOn(); + default -> DriverStation.reportWarning("Such control mode has not been implemented yet", true); + } + } + + + /** + * @return current spinner velocity in rotations per second + */ + public double getSpinnerVelocity() { + return feedbackEncoder.getVelocity(); + } + + /** + * @return current spinner acceleration in rotations per second per second + */ + public double getSpinnerAccleration() { + return currentAcceleration; + } + + /** + * Sets {@link #currentState} to the inputed state + * @param controlState An {@link SpinnerControlState} + */ + public void setElevatorControlState(SpinnerControlState controlState) { + switch (controlState) { + case AUTO -> setAutoOn(); + case MANUAL -> setManualOn(); + case BRAKE -> setBrakeOn(); + case COAST -> setCoastOn(); + default -> DriverStation.reportWarning("Such control mode has not been implemented yet", true); + } + } + + /** + * Sets {@link #currentState} to be in AUTO mode + */ + public void setAutoOn() { + if (spinnerConfig.spinnerPIDExists || spinnerConfig.spinnerFeedForwardExists) { + currentState = SpinnerControlState.AUTO; + } + else { + DriverStation.reportWarning("Any sort of autonomous control mode is disabled due to no PID or FeedForward", true); + } + } + + /** + * Sets {@link #currentState} to be in MANUAL mode + */ + public void setManualOn() { + currentState = SpinnerControlState.MANUAL; + } + + /** + * Sets {@link #currentState} to be in BRAKE mode, also changes the idle mode of the motors to brake. + */ + public void setBrakeOn() { + currentState = SpinnerControlState.BRAKE; + if (spinnerIdleMode == IdleMode.kCoast) { + spinnerIdleMode = IdleMode.kBrake; + brakeMotors(); + + } + } + + /** + * Sets {@link #currentState} to be in COAST mode, also changes the idle mode of the motors to coast. + */ + public void setCoastOn() { + currentState = SpinnerControlState.COAST; + if (spinnerIdleMode == IdleMode.kBrake) { + spinnerIdleMode = IdleMode.kCoast; + coastMotors(); + } + } + + /** + * Set all motors idleModes to brake + */ + private void brakeMotors() { + SparkFlexConfig flexBrakeConfig = new SparkFlexConfig(); + SparkMaxConfig maxBrakeConfig = new SparkMaxConfig(); + + flexBrakeConfig.idleMode(IdleMode.kBrake); + maxBrakeConfig.idleMode(IdleMode.kBrake); + if (spinnerConfig.spinnerFollowExists) { + if (spinnerConfig.spinnerFollowMotorType == MotorConfig.NEO_VORTEX) { + followMotor.configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + followMotor.configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + if (spinnerConfig.spinnerMasterMotorType == MotorConfig.NEO_VORTEX) { + masterMotor.configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + masterMotor.configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + /** + * Set all motors idleModes to coast + */ + private void coastMotors() { + SparkFlexConfig flexCoastConfig = new SparkFlexConfig(); + SparkMaxConfig maxCoastConfig = new SparkMaxConfig(); + + flexCoastConfig.idleMode(IdleMode.kCoast); + maxCoastConfig.idleMode(IdleMode.kCoast); + if (spinnerConfig.spinnerFollowExists) { + if (spinnerConfig.spinnerFollowMotorType == MotorConfig.NEO_VORTEX) { + followMotor.configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + followMotor.configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + if (spinnerConfig.spinnerMasterMotorType == MotorConfig.NEO_VORTEX) { + masterMotor.configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + masterMotor.configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + + protected void configureMotors() { + if (spinnerConfig.spinnerMasterMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig masterFlexConfig = new SparkFlexConfig(); + masterFlexConfig.idleMode(IdleMode.kBrake) + .inverted(spinnerConfig.spinnerMasterInverted) + .encoder.positionConversionFactor(spinnerConfig.gearReduction) + .velocityConversionFactor(spinnerConfig.gearReduction/60); + masterMotor = MotorControllerFactory.createSparkFlex(spinnerConfig.spinnerMasterId); + masterMotor.configure(masterFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + SparkMaxConfig masterMaxConfig = new SparkMaxConfig(); + masterMaxConfig.idleMode(IdleMode.kBrake) + .inverted(spinnerConfig.spinnerMasterInverted) + .encoder.positionConversionFactor(spinnerConfig.gearReduction) + .velocityConversionFactor(spinnerConfig.gearReduction/60); + masterMotor = MotorControllerFactory.createSparkMax(spinnerConfig.spinnerMasterId, spinnerConfig.spinnerMasterMotorType); + masterMotor.configure(masterMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + feedbackEncoder = masterMotor.getEncoder(); + if (spinnerConfig.spinnerFollowExists) { + if (spinnerConfig.spinnerFollowMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig followFlexConfig = new SparkFlexConfig(); + followFlexConfig.idleMode(IdleMode.kBrake) + .inverted(spinnerConfig.spinnerFollowInverted) + .follow(spinnerConfig.spinnerMasterId, (spinnerConfig.spinnerMasterInverted != spinnerConfig.spinnerFollowInverted)) + .encoder.positionConversionFactor(spinnerConfig.gearReduction) + .velocityConversionFactor(spinnerConfig.gearReduction/60); + followMotor = MotorControllerFactory.createSparkFlex(spinnerConfig.spinnerFollowId); + followMotor.configure(followFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + SparkMaxConfig followMaxConfig = new SparkMaxConfig(); + followMaxConfig.idleMode(IdleMode.kBrake) + .follow(spinnerConfig.spinnerMasterId, (spinnerConfig.spinnerMasterInverted != spinnerConfig.spinnerFollowInverted)) + .inverted(spinnerConfig.spinnerFollowInverted) + .encoder.positionConversionFactor(spinnerConfig.gearReduction) + .velocityConversionFactor(spinnerConfig.gearReduction/60); + followMotor = MotorControllerFactory.createSparkMax(spinnerConfig.spinnerFollowId, spinnerConfig.spinnerFollowMotorType); + followMotor.configure(followMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + backupEncoder = followMotor.getEncoder(); + } + } + + private void resetFeedbackEncoder() { + feedbackEncoder.setPosition(0); + if (spinnerConfig.spinnerFollowExists) { + backupEncoder.setPosition(0); + } + } + + /** + * Called periodically if in AUTO mode, will approach the current active {@link #spinnerGoal} using PID and FeedForward if such are provided + */ + protected void autoSpinner() { + double pidOutput; + double feedforwardOutput; + double curVelocity = feedbackEncoder.getVelocity(); + if (spinnerConfig.spinnerPIDExists) { + pidOutput = spinnerConfig.spinnerPID.calculate(curVelocity, spinnerGoal); + } + else { + pidOutput = 0; + } + + if (spinnerConfig.spinnerFeedForwardExists) { + feedforwardOutput = spinnerConfig.spinnerFeedforward.calculateWithVelocities(curVelocity, spinnerGoal); + } + else { + feedforwardOutput = 0; + } + masterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, -spinnerConfig.maxVolts, 0)); + } + + /** + * Called periodically if in MANUAL mode, will use {@link #latestManualInput} to spin the spinner using voltage percentage + */ + protected void manualSpinner() { + masterMotor.set(MathUtil.clamp(latestManualInput, -spinnerConfig.maxManualInput, spinnerConfig.maxManualInput)); + } + + /** + * Mainly used by SysId to characterize the spinner model + * @param volts Voltage unit input + */ + protected void manualSpinner(Voltage volts) { + double voltage = volts.in(Volts); + masterMotor.setVoltage(MathUtil.clamp(voltage, -spinnerConfig.maxManualInput, spinnerConfig.maxManualInput)); + + } + + /** + * Called periodically, checks for any issues with the main encoder by comparing it with the backup encoder if such exists + */ + protected void checkFeedBackEncoder() { + if (spinnerConfig.spinnerFollowExists) { + currentDif = feedbackEncoder.getPosition() - backupEncoder.getPosition(); + if (Math.abs(currentDif) > difDanger && !encoderFaultReported) { + DriverStation.reportWarning("Spinner encoder seems to be off!", true); + encoderFaultReported = true; + } + } + } + + + /** + * Sets voltage to 0 + */ + public void stopSpinner() { + masterMotor.set(0); + } + + /** + * Use this to do any fun stuff you want to do, for spinnerSubsystemPeriodic + */ + protected void userPeriodic() { + + } + + /** + * Posts important information about Spinner Subsystem into SmartDashboard + */ + public void postSmartDashboardValues() { + SmartDashboard.putNumber("Spinner Velocity (rotations/s)", feedbackEncoder.getVelocity()); + SmartDashboard.putNumber("Spinner Goal Velocity (rotations/s)", spinnerGoal); + SmartDashboard.putBoolean("Spinner at Goal Speed", atGoal()); + SmartDashboard.putNumber("Spinner Latest Manual Input", latestManualInput); + SmartDashboard.putNumber("Current period of Spinner (ms)", currentPeriod*1000); + switch (currentState) { + case AUTO -> SmartDashboard.putString("spinnerControlState", "Autonomous"); + case MANUAL -> SmartDashboard.putString("spinnerControlState", "Manual"); + case BRAKE -> SmartDashboard.putString("spinnerControlState", "Idling: Brake"); + case COAST -> SmartDashboard.putString("spinnerControlState", "Idling: Coast"); + } + } + + /** + * Checks Subsystem periodic for running under expected time limits, reports inconsitencies to user, also is usable by feedforward + */ + private void checkPeriodic() { + if (pastStamp == 0) { + pastStamp = Timer.getFPGATimestamp(); + return; + } + currentStamp = Timer.getFPGATimestamp(); + currentPeriod = currentStamp - pastStamp; + pastStamp = currentStamp; + if (currentPeriod > 0.02) { + DriverStation.reportWarning("Spinner Periodic is slow: " + currentPeriod *1000 + " ms. Check for any lag reasons", true); + } + } + + private void calculateAccleration() { + double currentVelocity = feedbackEncoder.getVelocity(); + currentAcceleration = (currentVelocity - previousVelocity) / currentPeriod; + previousVelocity = currentVelocity; + } + + /*Overide these methods to customize what the mechanism does in different states */ + protected void handleAuto() { autoSpinner();} + protected void handleManual() { manualSpinner();} + protected void handleBrake() { stopSpinner();} + protected void handleCoast() { stopSpinner();} + + /** + * DO NOT OVERRIDE THIS PERIODIC UNLESS YOU KNOW WHAT YOU ARE DOING! + * Use {@link #userPeriodic()} instead. + */ + @Override + public void periodic() { + switch (currentState) { + case AUTO -> handleAuto(); + case MANUAL -> handleManual(); + case BRAKE -> handleBrake(); + case COAST -> handleCoast(); + } + checkFeedBackEncoder(); + userPeriodic(); + postSmartDashboardValues(); + checkPeriodic(); + calculateAccleration(); + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java new file mode 100644 index 0000000..3ca35db --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java @@ -0,0 +1,224 @@ +package org.carlmontrobotics.lib199.SimpleMechs.Spinner; + +import org.carlmontrobotics.lib199.MotorConfig; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.DriverStation; + +public final class SpinnerConfig { + public final int spinnerMasterId; + public final int spinnerFollowId; + public final MotorConfig spinnerMasterMotorType; + public final MotorConfig spinnerFollowMotorType; + public final boolean spinnerMasterInverted; + public final boolean spinnerFollowInverted; + public final double gearReduction; + public final double maxVolts; + public final double maxManualInput; + public final double[] spinnerKPID; + public final double[] spinnerKFeedForward; + public final double spinnerPIDTolerance; + public double spinnerKP; + public double spinnerKI; + public double spinnerKD; + public double spinnerKS; + public double spinnerKV; + public double spinnerKA; + public PIDController spinnerPID; + public SimpleMotorFeedforward spinnerFeedforward; + public boolean spinnerFollowExists; + public boolean spinnerPIDExists; + public boolean spinnerFeedForwardExists; + + /** + * + * @param spinnerMasterId master id + * @param spinnerFollowId follow id, -1 if no follow motor + * @param spinnerMasterMotorType master {@link MotorConfig} + * @param spinnerFollowMotorType follow {@link MotorConfig} null if no follow motor + * @param spinnerMasterInverted master inversed such that whichever is the desired movement of the spinner will be positive + * @param spinnerFollowInverted follow inversed such that whichever is the desired movement of the spinner will be positive, false if no motor + * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor + * @param maxVolts maximum volts that the spinner can use autonomously + * @param maxManualInput maximum voltage percentage that the spinner can use while being manually controlled + * @param spinnerKPID 3 values for kP, kI, and kD + * @param spinnerKFeedForward 3 values for kS, kV, and kA + * @param spinnerPIDTolerance in rotations per second how much should pid care for speed + * @throws IllegalArgumentException if SpinnerFollowMotorType is null when there is provided a spinnerFollowId + * @throws IllegalArgumentException if SpinnerFollowMotorType or SpinnerMasterMotorType is not an expected {@link MotorConfig} + * @throws IllegalArgumentException if spinnerKPID is not three values with each being non negative + * @throws IllegalArgumentException if spinnerKFeedForward is not three values with each being non negative + * @throws IllegalArgumentException if gearReduction is 0 + * @throws IllegalArgumentException if maxVolts is not positive + * @throws IllegalArgumentException if maxManualInput is not positive + */ + public SpinnerConfig(int spinnerMasterId, int spinnerFollowId, MotorConfig spinnerMasterMotorType, MotorConfig spinnerFollowMotorType, boolean spinnerMasterInverted, boolean spinnerFollowInverted, + double gearReduction, double maxVolts, double maxManualInput, double[] spinnerKPID, double[] spinnerKFeedForward, double spinnerPIDTolerance) { + this.spinnerMasterId = spinnerMasterId; + this.spinnerMasterMotorType = spinnerMasterMotorType; + this.spinnerMasterInverted = spinnerMasterInverted; + this.spinnerFollowId = spinnerFollowId; + this.spinnerFollowMotorType = spinnerFollowMotorType; + this.spinnerFollowInverted = spinnerFollowInverted; + this.gearReduction = gearReduction; + this.maxVolts = maxVolts; + this.maxManualInput = maxManualInput; + this.spinnerKPID = spinnerKPID; + this.spinnerKFeedForward = spinnerKFeedForward; + this.spinnerPIDTolerance = spinnerPIDTolerance; + checkRequirements(); + } + + /** + * Creates a simple single NEO spinner with PID + * @param spinnerMasterId id of the motor + * @param spinnerMasterInverted inversed setting such that whichever is the desired movement of the spinner will be positive + * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor + * @param maxVolts maximum volts that the spinner can use autonomously + * @param spinnerKPID 3 values for kP, kI, and kD + * @param spinnerPIDTolerance in rotations per second how much should pid care for speed + */ + public static SpinnerConfig NEOSpinnerConfig(int spinnerMasterId, boolean spinnerMasterInverted, double gearReduction, double maxVolts, double[] spinnerKPID, double spinnerPIDTolerance) { + return new SpinnerConfig(spinnerMasterId, -1, MotorConfig.NEO, null, spinnerMasterInverted, false, gearReduction, maxVolts, maxVolts/12.0, spinnerKPID, null, spinnerPIDTolerance); + } + + /** + * Creates a simple single NEO spinner without PID + * @param spinnerMasterId id of the motor + * @param spinnerMasterInverted inversed setting such that whichever is the desired movement of the spinner will be positive + * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor + * @param maxManualInput maximum voltage percentage that the spinner can use while being manually controlled + * @return + */ + public static SpinnerConfig NEOSpinnerConfig(int spinnerMasterId, boolean spinnerMasterInverted, double gearReduction, double maxManualInput) { + return new SpinnerConfig(spinnerMasterId, -1, MotorConfig.NEO, null, spinnerMasterInverted, false, gearReduction, 14, maxManualInput, null, null, 0); + } + + /** + * Creates a simple single NEO spinner with PID + * @param spinnerMasterId id of the motor + * @param spinnerMasterInverted inversed setting such that whichever is the desired movement of the spinner will be positive + * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor + * @param maxVolts maximum volts that the spinner can use autonomously + * @param spinnerKPID 3 values for kP, kI, and kD + * @param spinnerPIDTolerance in rotations per second how much should pid care for speed + */ + public static SpinnerConfig VORTEXSpinnerConfig(int spinnerMasterId, boolean spinnerMasterInverted, double gearReduction, double maxVolts, double[] spinnerKPID, double spinnerPIDTolerance) { + return new SpinnerConfig(spinnerMasterId, -1, MotorConfig.NEO_VORTEX, null, spinnerMasterInverted, false, gearReduction, maxVolts, maxVolts/12.0, spinnerKPID, null, spinnerPIDTolerance); + } + + /** + * Creates a simple single NEO spinner without PID + * @param spinnerMasterId id of the motor + * @param spinnerMasterInverted inversed setting such that whichever is the desired movement of the spinner will be positive + * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor + * @param maxManualInput maximum voltage percentage that the spinner can use while being manually controlled + * @return + */ + public static SpinnerConfig VORTEXSpinnerConfig(int spinnerMasterId, boolean spinnerMasterInverted, double gearReduction, double maxManualInput) { + return new SpinnerConfig(spinnerMasterId, -1, MotorConfig.NEO_VORTEX, null, spinnerMasterInverted, false, gearReduction, 14, maxManualInput, null, null, 0); + } + + /** + * Checks the spinnerConfig to not have any issues, and creates certain extra values automatically like {@link #spinnerPID} and {@link #spinnerFeedforward} + */ + private void checkRequirements() { + checkMotorConfigs(); //Prevent future issues with new motors + checkPID(); + checkFeedForward(); + checkGearReduction(); + checkMaxVoltsAndInput(); + } + + /** + * Check that the motor Configs are not any new configs that would not work or null. + */ + private void checkMotorConfigs() { + if (spinnerFollowId != -1) { + spinnerFollowExists = true; + if (spinnerFollowMotorType != MotorConfig.NEO && spinnerFollowMotorType != MotorConfig.NEO_VORTEX && spinnerFollowMotorType != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + } + else { + spinnerFollowExists = false; + } + if (spinnerMasterMotorType != MotorConfig.NEO && spinnerMasterMotorType != MotorConfig.NEO_VORTEX && spinnerMasterMotorType != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + } + + /** + * Checks PID to have 3 values, updates {@link #spinnerPIDExists} and makes sure the values are legitimate + */ + private void checkPID() { + if (spinnerKPID != null) { + if (spinnerKPID.length == 3) { + if (spinnerKPID[0] < 0 || spinnerKPID[1] < 0 || spinnerKPID[2] < 0) { + throw new IllegalArgumentException("PID values have to be non negative"); + } + spinnerKP = spinnerKPID[0]; + spinnerKI = spinnerKPID[1]; + spinnerKD = spinnerKPID[2]; + spinnerPID = new PIDController(spinnerKP, spinnerKI, spinnerKD); + spinnerPID.setTolerance(spinnerPIDTolerance); + spinnerPID.enableContinuousInput(0, 1); + spinnerPIDExists = true; + return; + } + throw new IllegalArgumentException("Need to have 3 values for PID"); + } + spinnerKP = 0; + spinnerKI = 0; + spinnerKD = 0; + spinnerPIDExists = false; + DriverStation.reportWarning("Spinner PID is off", true); + } + + /** + * Checks FeedForward to have 3 values, updates {@link #spinnerFeedForwardExists} and makes sure the values are legitimate + */ + private void checkFeedForward() { + if (spinnerKFeedForward != null) { + if (spinnerKFeedForward.length == 3) { + if (spinnerKFeedForward[0] < 0|| spinnerKFeedForward[1] < 0 || spinnerKFeedForward[2] < 0) { + throw new IllegalArgumentException("FeedForward values need to have non negative values"); + } + spinnerKS = spinnerKFeedForward[0]; + spinnerKV = spinnerKFeedForward[1]; + spinnerKA = spinnerKFeedForward[2]; + spinnerFeedforward = new SimpleMotorFeedforward(spinnerKS, spinnerKV, spinnerKA); + spinnerFeedForwardExists = true; + return; + } + throw new IllegalArgumentException("Need to have 3 values for Spinner FeedForward: Friction, Voltage, and Acceleration"); + } + spinnerKS = 0; + spinnerKV = 0; + spinnerKA = 0; + spinnerFeedForwardExists = false; + DriverStation.reportWarning("SpinnerFeedForward is off", true); + } + + /** + * Makes sure gear reduction is not 0 + */ + private void checkGearReduction() { + if (gearReduction == 0) { + throw new IllegalArgumentException("Gear reduction cannot be 0"); + } + } + + /** + * Checks for {@link #maxVolts} and {@link #maxManualInput} to be greater than zero + */ + private void checkMaxVoltsAndInput() { + if (maxVolts <= 0) { + throw new IllegalArgumentException("maxVolts needs to be positive"); + } + if (maxManualInput <= 0) { + throw new IllegalArgumentException("maxManualInput needs to be positive"); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java index e4f8fc7..06c4c86 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -44,7 +44,7 @@ public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseabl * @param analog Whether the encoder is an analog sensor * @param absolute Whether the encoder is an absolute encoder. This flag caps the position to * one rotation via. {@link MathUtil#inputModulus(double, double, double)}, disables - * {@link #setPosition(double)}, and enables {@link #setZeroOffset(double)}. + * {@link #setPosition(double)}, and enables setZeroOffset(double). * * {@link #getVelocity()} will return a value in rpm. */ @@ -59,7 +59,7 @@ public MockedEncoder(SimDevice device, int countsPerRev, boolean analog, * @param analog Whether the encoder is an analog sensor * @param absolute Whether the encoder is an absolute encoder. This flag caps the position to * one rotation via. {@link MathUtil#inputModulus(double, double, double)}, disables - * {@link #setPosition(double)}, and enables {@link #setZeroOffset(double)}. + * {@link #setPosition(double)}, and enables setZeroOffset(double). //FIXME where is this so called setZero * @param useRps Whether getVelocity() should return rps instead of rpm. */ public MockedEncoder(SimDevice device, int countsPerRev, boolean analog, @@ -106,7 +106,7 @@ public REVLibError setPosition(double newPosition) { // } /** - * @return The current position of the encoder, not accounting for the position offset ({@link #setPosition(double)} and {@link #setZeroOffset(double)}) + * @return The current position of the encoder, not accounting for the position offset ({@link #setPosition(double)} and setZeroOffset(double) */ public double getRawPosition() { double rotationsOrVolts = voltage != null ? voltage.get() : position.get(); diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index b83c112..7935eae 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -12,10 +12,14 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.spark.SparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; @@ -45,12 +49,27 @@ public class SwerveModule implements Sendable { public enum ModuleType {FL, FR, BL, BR}; + //constants + private final double neoStallTorqueNewtonMeters = 3.36; + private final double neoFreeCurrentAmps = 1.3; + private final double neoStallCurrentAmps = 166; + private final double neoVortexStallTorqueNewtonMeters = 3.6; + private final double neoVortexFreeCurrentAmps = 3.6; + private final double neoVortexStallCurrentAmps = 211; + private final double normalForceNewtons = 83.2 /* lbf */ * 4.4482 /* N/lbf */ / 4 /* numModules */; + + private SwerveConfig config; - private SparkMaxConfig turnConfig = new SparkMaxConfig(); - private SparkMaxConfig driveConfig = new SparkMaxConfig(); + private SparkMaxConfig turnMaxConfig = new SparkMaxConfig(); + private SparkMaxConfig driveMaxConfig = new SparkMaxConfig(); + private SparkFlexConfig turnFlexConfig = new SparkFlexConfig(); + private SparkFlexConfig driveFlexConfig = new SparkFlexConfig(); private ModuleType type; - private SparkMax drive, turn; + private SparkMax maxDrive, maxTurn; + private SparkFlex flexDrive, flexTurn; + private RelativeEncoder driveRelEnc, turnRelEnc; + private boolean useFlexDrive, useFlexTurn; //false for max true for flex private CANcoder turnEncoder; private PIDController drivePIDController; private ProfiledPIDController turnPIDController; @@ -67,35 +86,125 @@ public enum ModuleType {FL, FR, BL, BR}; private double maxTurnVelocityWithoutTippingRps; public SwerveModule(SwerveConfig config, ModuleType type, SparkMax drive, SparkMax turn, CANcoder turnEncoder, int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { - //SmartDashboard.putNumber("Target Angle (deg)", 0.0); String moduleString = type.toString(); this.timer = new Timer(); timer.start(); - // SmartDashboard.putNumber("num periods",1); - // SmartDashboard.putNumber("maxOverShootDegree",1); this.config = config; this.type = type; - this.drive = drive; + this.maxDrive = drive; + this.useFlexDrive = false; + this.driveRelEnc = maxDrive.getEncoder(); + + driveMaxConfig.inverted(config.driveInversion[arrIndex]); + turnMaxConfig.inverted(config.turnInversion[arrIndex]); + + double drivePositionFactor = config.wheelDiameterMeters * Math.PI / config.driveGearing; + final double driveVelocityFactor = drivePositionFactor / 60; + driveMaxConfig.encoder + .positionConversionFactor(drivePositionFactor) + .velocityConversionFactor(driveVelocityFactor); + + maxControllableAccerlationRps2 = 0; + double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; + double motorTorqueLimitNewtonMeters = wheelTorqueLimitNewtonMeters / config.driveGearing; + double currentLimitAmps = neoFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoStallTorqueNewtonMeters * (neoStallCurrentAmps-neoFreeCurrentAmps); + // SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps); + driveMaxConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); + + this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex], + config.kForwardVels[arrIndex], + config.kForwardAccels[arrIndex]); + this.backwardSimpleMotorFF = new SimpleMotorFeedforward(config.kBackwardVolts[arrIndex], + config.kBackwardVels[arrIndex], + config.kBackwardAccels[arrIndex]); + + drivePIDController = new PIDController(config.drivekP[arrIndex], + config.drivekI[arrIndex], + config.drivekD[arrIndex]); + + /* offset for 1 relative encoder count */ + drivetoleranceMPerS = (1.0 + / (double)(maxDrive.configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) + / Units.millisecondsToSeconds(maxDrive.configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * maxDrive.configAccessor.encoder.getQuadratureAverageDepth()); + drivePIDController.setTolerance(drivetoleranceMPerS); + + + this.maxTurn = turn; + this.useFlexTurn = false; + this.turnRelEnc = maxTurn.getEncoder(); + + this.turnSimpleMotorFeedforward = new SimpleMotorFeedforward(config.turnkS[arrIndex], + config.turnkV[arrIndex], + config.turnkA[arrIndex]); + + // Voltage = kS + kV * velocity + kA * acceleration + // Assume cruising at maximum velocity --> 12 = kS + kV * max velocity + kA * 0 --> max velocity = (12 - kS) / kV + // Limit the velocity by a factor of 0.5 + maxAchievableTurnVelocityRps = 0.5 * turnSimpleMotorFeedforward.maxAchievableVelocity(12.0, 0); + maxTurnVelocityWithoutTippingRps = maxAchievableTurnVelocityRps; + // Assume accelerating while at limited speed --> 12 = kS + kV * limited speed + kA * acceleration + // acceleration = (12 - kS - kV * limiedSpeed) / kA + turnToleranceRot = Units.degreesToRotations(3 * 360/4096.0); /* degree offset for 3 CANCoder counts */ + maxAchievableTurnAccelerationRps2 = 0.5 * turnSimpleMotorFeedforward.maxAchievableAcceleration(12.0, maxAchievableTurnVelocityRps); + + turnConstraints = new TrapezoidProfile.Constraints(maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2); + lastAngle = 0.0; + turnPIDController = new ProfiledPIDController( + config.turnkP[arrIndex], + config.turnkI[arrIndex], + config.turnkD[arrIndex], + turnConstraints); + turnPIDController.enableContinuousInput(-.5, .5); + turnPIDController.setTolerance(turnToleranceRot); + + this.driveModifier = config.driveModifier; + this.reversed = config.reversed[arrIndex]; + this.turnZeroDeg = config.turnZeroDeg[arrIndex]; + + CANcoderConfiguration CANconfig = new CANcoderConfiguration(); + CANconfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = .5; + this.turnEncoder = turnEncoder; + // CANconfig.MagnetSensor.MagnetOffset=-turnZeroDeg; //done in getModuleAngle. + this.turnEncoder.getConfigurator().apply(CANconfig); + + turnPIDController.reset(getModuleAngle()); + + this.rollDegSupplier = rollDegSupplier; + this.pitchDegSupplier = pitchDegSupplier; + + SendableRegistry.addLW(this, "SwerveModule", type.toString()); + + //do stuff here + maxDrive.configure(driveMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + maxTurn.configure(turnMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public SwerveModule(SwerveConfig config, ModuleType type, SparkMax drive, SparkFlex turn, CANcoder turnEncoder, + int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { + String moduleString = type.toString(); + this.timer = new Timer(); + timer.start(); + this.config = config; + this.type = type; + this.maxDrive = drive; + this.useFlexDrive = false; + this.driveRelEnc = maxDrive.getEncoder(); - driveConfig.inverted(config.driveInversion[arrIndex]); - turnConfig.inverted(config.turnInversion[arrIndex]); + driveMaxConfig.inverted(config.driveInversion[arrIndex]); + turnFlexConfig.inverted(config.turnInversion[arrIndex]); double drivePositionFactor = config.wheelDiameterMeters * Math.PI / config.driveGearing; - final double driveVelocityFactor = drivePositionFactor / 60;//why by 60? - driveConfig.encoder + final double driveVelocityFactor = drivePositionFactor / 60; + driveMaxConfig.encoder .positionConversionFactor(drivePositionFactor) .velocityConversionFactor(driveVelocityFactor); maxControllableAccerlationRps2 = 0; - final double normalForceNewtons = 83.2 /* lbf */ * 4.4482 /* N/lbf */ / 4 /* numModules */; double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; double motorTorqueLimitNewtonMeters = wheelTorqueLimitNewtonMeters / config.driveGearing; - final double neoStallTorqueNewtonMeters = 3.36; - final double neoFreeCurrentAmps = 1.3; - final double neoStallCurrentAmps = 166; double currentLimitAmps = neoFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoStallTorqueNewtonMeters * (neoStallCurrentAmps-neoFreeCurrentAmps); // SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps); - driveConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); + driveMaxConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex], config.kForwardVels[arrIndex], @@ -110,13 +219,14 @@ public SwerveModule(SwerveConfig config, ModuleType type, SparkMax drive, SparkM /* offset for 1 relative encoder count */ drivetoleranceMPerS = (1.0 - / (double)(drive.configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) - / Units.millisecondsToSeconds(drive.configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * drive.configAccessor.encoder.getQuadratureAverageDepth()); + / (double)(maxDrive.configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) + / Units.millisecondsToSeconds(maxDrive.configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * maxDrive.configAccessor.encoder.getQuadratureAverageDepth()); drivePIDController.setTolerance(drivetoleranceMPerS); - //System.out.println("Velocity Constant: " + (positionConstant / 60)); - this.turn = turn; + this.flexTurn = turn; + this.useFlexTurn = true; + this.turnRelEnc = flexTurn.getEncoder(); this.turnSimpleMotorFeedforward = new SimpleMotorFeedforward(config.turnkS[arrIndex], config.turnkV[arrIndex], @@ -156,29 +266,202 @@ public SwerveModule(SwerveConfig config, ModuleType type, SparkMax drive, SparkM this.rollDegSupplier = rollDegSupplier; this.pitchDegSupplier = pitchDegSupplier; - // SmartDashboard.putNumber(moduleString + " Swerve kS", turnSimpleMotorFeedforward.ks); - // SmartDashboard.putNumber(moduleString + " Swerve kV", turnSimpleMotorFeedforward.kv); - // SmartDashboard.putNumber(moduleString + " Swerve kA", turnSimpleMotorFeedforward.ka); - // SmartDashboard.putNumber(moduleString + " Swerve kP", turnPIDController.getP()); - // SmartDashboard.putNumber(moduleString +" Swerve kD", turnPIDController.getD()); - // SmartDashboard.putNumber(moduleString + " Swerve Turn Tolerance", turnToleranceRot); + SendableRegistry.addLW(this, "SwerveModule", type.toString()); + + //do stuff here + maxDrive.configure(driveMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + flexTurn.configure(turnFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public SwerveModule(SwerveConfig config, ModuleType type, SparkFlex drive, SparkMax turn, CANcoder turnEncoder, + int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { + String moduleString = type.toString(); + this.timer = new Timer(); + timer.start(); + this.config = config; + this.type = type; + this.flexDrive = drive; + this.useFlexDrive = true; + this.driveRelEnc = maxDrive.getEncoder(); + + driveFlexConfig.inverted(config.driveInversion[arrIndex]); + turnMaxConfig.inverted(config.turnInversion[arrIndex]); + + double drivePositionFactor = config.wheelDiameterMeters * Math.PI / config.driveGearing; + final double driveVelocityFactor = drivePositionFactor / 60; + driveFlexConfig.encoder + .positionConversionFactor(drivePositionFactor) + .velocityConversionFactor(driveVelocityFactor); + + maxControllableAccerlationRps2 = 0; + double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; + double motorTorqueLimitNewtonMeters = wheelTorqueLimitNewtonMeters / config.driveGearing; + double currentLimitAmps = neoVortexFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoVortexStallCurrentAmps * (neoVortexStallCurrentAmps-neoVortexFreeCurrentAmps); + // SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps); + driveFlexConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); + + this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex], + config.kForwardVels[arrIndex], + config.kForwardAccels[arrIndex]); + this.backwardSimpleMotorFF = new SimpleMotorFeedforward(config.kBackwardVolts[arrIndex], + config.kBackwardVels[arrIndex], + config.kBackwardAccels[arrIndex]); + + drivePIDController = new PIDController(config.drivekP[arrIndex], + config.drivekI[arrIndex], + config.drivekD[arrIndex]); + + /* offset for 1 relative encoder count */ + drivetoleranceMPerS = (1.0 + / (double)(flexDrive.configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) + / Units.millisecondsToSeconds(flexDrive.configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * flexDrive.configAccessor.encoder.getQuadratureAverageDepth()); + drivePIDController.setTolerance(drivetoleranceMPerS); + + + this.maxTurn = turn; + this.useFlexTurn = false; + this.turnRelEnc = maxTurn.getEncoder(); + + this.turnSimpleMotorFeedforward = new SimpleMotorFeedforward(config.turnkS[arrIndex], + config.turnkV[arrIndex], + config.turnkA[arrIndex]); + + // Voltage = kS + kV * velocity + kA * acceleration + // Assume cruising at maximum velocity --> 12 = kS + kV * max velocity + kA * 0 --> max velocity = (12 - kS) / kV + // Limit the velocity by a factor of 0.5 + maxAchievableTurnVelocityRps = 0.5 * turnSimpleMotorFeedforward.maxAchievableVelocity(12.0, 0); + maxTurnVelocityWithoutTippingRps = maxAchievableTurnVelocityRps; + // Assume accelerating while at limited speed --> 12 = kS + kV * limited speed + kA * acceleration + // acceleration = (12 - kS - kV * limiedSpeed) / kA + turnToleranceRot = Units.degreesToRotations(3 * 360/4096.0); /* degree offset for 3 CANCoder counts */ + maxAchievableTurnAccelerationRps2 = 0.5 * turnSimpleMotorFeedforward.maxAchievableAcceleration(12.0, maxAchievableTurnVelocityRps); + + turnConstraints = new TrapezoidProfile.Constraints(maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2); + lastAngle = 0.0; + turnPIDController = new ProfiledPIDController( + config.turnkP[arrIndex], + config.turnkI[arrIndex], + config.turnkD[arrIndex], + turnConstraints); + turnPIDController.enableContinuousInput(-.5, .5); + turnPIDController.setTolerance(turnToleranceRot); + + this.driveModifier = config.driveModifier; + this.reversed = config.reversed[arrIndex]; + this.turnZeroDeg = config.turnZeroDeg[arrIndex]; + + CANcoderConfiguration CANconfig = new CANcoderConfiguration(); + CANconfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = .5; + this.turnEncoder = turnEncoder; + // CANconfig.MagnetSensor.MagnetOffset=-turnZeroDeg; //done in getModuleAngle. + this.turnEncoder.getConfigurator().apply(CANconfig); - // SmartDashboard.putNumber(moduleString + " Drive kP", drivePIDController.getP()); - // SmartDashboard.putNumber(moduleString + " Drive kD", drivePIDController.getD()); - // SmartDashboard.putNumber(moduleString + " Drive kS", forwardSimpleMotorFF.ks); - // SmartDashboard.putNumber(moduleString + " Drive kV", forwardSimpleMotorFF.kv); - // SmartDashboard.putNumber(moduleString + " Drive kA", forwardSimpleMotorFF.ka); - // SmartDashboard.putNumber(moduleString + " Drive Tolerance", turnToleranceRot); + turnPIDController.reset(getModuleAngle()); - // SmartDashboard.putData(this); + this.rollDegSupplier = rollDegSupplier; + this.pitchDegSupplier = pitchDegSupplier; SendableRegistry.addLW(this, "SwerveModule", type.toString()); //do stuff here - drive.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - turn.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + flexDrive.configure(driveFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + maxTurn.configure(turnMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public SwerveModule(SwerveConfig config, ModuleType type, SparkFlex drive, SparkFlex turn, CANcoder turnEncoder, + int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { + String moduleString = type.toString(); + this.timer = new Timer(); + timer.start(); + this.config = config; + this.type = type; + this.flexDrive = drive; + this.useFlexDrive = true; + this.driveRelEnc = maxDrive.getEncoder(); + + driveFlexConfig.inverted(config.driveInversion[arrIndex]); + turnFlexConfig.inverted(config.turnInversion[arrIndex]); + + double drivePositionFactor = config.wheelDiameterMeters * Math.PI / config.driveGearing; + final double driveVelocityFactor = drivePositionFactor / 60; + driveFlexConfig.encoder + .positionConversionFactor(drivePositionFactor) + .velocityConversionFactor(driveVelocityFactor); + + maxControllableAccerlationRps2 = 0; + double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; + double motorTorqueLimitNewtonMeters = wheelTorqueLimitNewtonMeters / config.driveGearing; + double currentLimitAmps = neoVortexFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoVortexStallCurrentAmps * (neoVortexStallCurrentAmps-neoVortexFreeCurrentAmps); + // SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps); + driveFlexConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); + + this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex], + config.kForwardVels[arrIndex], + config.kForwardAccels[arrIndex]); + this.backwardSimpleMotorFF = new SimpleMotorFeedforward(config.kBackwardVolts[arrIndex], + config.kBackwardVels[arrIndex], + config.kBackwardAccels[arrIndex]); + + drivePIDController = new PIDController(config.drivekP[arrIndex], + config.drivekI[arrIndex], + config.drivekD[arrIndex]); + + /* offset for 1 relative encoder count */ + drivetoleranceMPerS = (1.0 + / (double)(flexDrive.configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) + / Units.millisecondsToSeconds(flexDrive.configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * flexDrive.configAccessor.encoder.getQuadratureAverageDepth()); + drivePIDController.setTolerance(drivetoleranceMPerS); + + + this.flexTurn = turn; + this.useFlexTurn = true; + this.turnRelEnc = flexTurn.getEncoder(); + + this.turnSimpleMotorFeedforward = new SimpleMotorFeedforward(config.turnkS[arrIndex], + config.turnkV[arrIndex], + config.turnkA[arrIndex]); + + // Voltage = kS + kV * velocity + kA * acceleration + // Assume cruising at maximum velocity --> 12 = kS + kV * max velocity + kA * 0 --> max velocity = (12 - kS) / kV + // Limit the velocity by a factor of 0.5 + maxAchievableTurnVelocityRps = 0.5 * turnSimpleMotorFeedforward.maxAchievableVelocity(12.0, 0); + maxTurnVelocityWithoutTippingRps = maxAchievableTurnVelocityRps; + // Assume accelerating while at limited speed --> 12 = kS + kV * limited speed + kA * acceleration + // acceleration = (12 - kS - kV * limiedSpeed) / kA + turnToleranceRot = Units.degreesToRotations(3 * 360/4096.0); /* degree offset for 3 CANCoder counts */ + maxAchievableTurnAccelerationRps2 = 0.5 * turnSimpleMotorFeedforward.maxAchievableAcceleration(12.0, maxAchievableTurnVelocityRps); + + turnConstraints = new TrapezoidProfile.Constraints(maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2); + lastAngle = 0.0; + turnPIDController = new ProfiledPIDController( + config.turnkP[arrIndex], + config.turnkI[arrIndex], + config.turnkD[arrIndex], + turnConstraints); + turnPIDController.enableContinuousInput(-.5, .5); + turnPIDController.setTolerance(turnToleranceRot); + + this.driveModifier = config.driveModifier; + this.reversed = config.reversed[arrIndex]; + this.turnZeroDeg = config.turnZeroDeg[arrIndex]; + + CANcoderConfiguration CANconfig = new CANcoderConfiguration(); + CANconfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = .5; + this.turnEncoder = turnEncoder; + // CANconfig.MagnetSensor.MagnetOffset=-turnZeroDeg; //done in getModuleAngle. + this.turnEncoder.getConfigurator().apply(CANconfig); + turnPIDController.reset(getModuleAngle()); + + this.rollDegSupplier = rollDegSupplier; + this.pitchDegSupplier = pitchDegSupplier; + + SendableRegistry.addLW(this, "SwerveModule", type.toString()); + + //do stuff here + flexDrive.configure(driveFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + flexTurn.configure(turnFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } public ModuleType getType() { @@ -186,6 +469,7 @@ public ModuleType getType() { } private double prevTurnVelocity = 0; + public void periodic() { drivePeriodic(); updateSmartDashboard(); @@ -216,8 +500,12 @@ public void drivePeriodic() { // SmartDashboard.putNumber(moduleString + " pidVolts", pidVolts); double appliedVoltage = MathUtil.clamp(targetVoltage, -12, 12); // SmartDashboard.putNumber(moduleString + " appliedVoltage", appliedVoltage); - - drive.setVoltage(appliedVoltage); + if (useFlexDrive) { + flexDrive.setVoltage(appliedVoltage); + } + else { + maxDrive.setVoltage(appliedVoltage); + } } public void turnPeriodic() { @@ -251,9 +539,19 @@ public void turnPeriodic() { turnFFVolts = turnSimpleMotorFeedforward.calculate(state.velocity);//(state.velocity-prevTurnVelocity) / period); turnVolts = turnFFVolts + turnSpeedCorrectionVolts; if (!turnPIDController.atGoal()) { - turn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0)); + if (useFlexTurn) { + flexTurn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0)); + } + else { + maxTurn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0)); + } } else { - turn.setVoltage(turnSimpleMotorFeedforward.calculate(goal.velocity)); + if (useFlexTurn) { + flexTurn.setVoltage(turnSimpleMotorFeedforward.calculate(goal.velocity)); + } + else { + maxTurn.setVoltage(turnSimpleMotorFeedforward.calculate(goal.velocity)); + } } prevTurnVelocity = state.velocity; } @@ -348,11 +646,11 @@ public SwerveModulePosition getCurrentPosition() { } public double getCurrentDistance() { - return drive.getEncoder().getPosition(); + return driveRelEnc.getPosition(); } public double getCurrentSpeed() { - return drive.getEncoder().getVelocity(); + return driveRelEnc.getVelocity(); } /** @@ -369,7 +667,7 @@ public void updateSmartDashboard() { SmartDashboard.putNumber(moduleString + " Absolute Angle (deg)", Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble())); // Display the module angle as calculated using the absolute encoder. SmartDashboard.putNumber(moduleString + " Turn Measured Pos (deg)", getModuleAngle()); - SmartDashboard.putNumber(moduleString + " Encoder Position", drive.getEncoder().getPosition()); + SmartDashboard.putNumber(moduleString + " Encoder Position", driveRelEnc.getPosition()); // Display the speed that the robot thinks it is travelling at. SmartDashboard.putNumber(moduleString + " Current Speed", getCurrentSpeed()); SmartDashboard.putBoolean(moduleString + " Drive is at Goal", drivePIDController.atSetpoint()); @@ -429,20 +727,40 @@ public void updateSmartDashboard() { } public void toggleMode() { - if (drive.configAccessor.getIdleMode() == IdleMode.kBrake && turn.configAccessor.getIdleMode() == IdleMode.kCoast) coast(); + IdleMode driveIdleMode; + IdleMode turnIdleMode; + if (useFlexDrive) { + driveIdleMode = flexDrive.configAccessor.getIdleMode(); + } + else { + driveIdleMode = maxDrive.configAccessor.getIdleMode(); + } + if (useFlexTurn) { + turnIdleMode = flexTurn.configAccessor.getIdleMode(); + } + else { + turnIdleMode = maxTurn.configAccessor.getIdleMode(); + } + if (driveIdleMode == IdleMode.kBrake && turnIdleMode == IdleMode.kCoast) coast(); else brake(); } public void brake() { - SparkBaseConfig config = new SparkMaxConfig().idleMode(IdleMode.kBrake); - drive.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - turn .configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + SparkBaseConfig maxConfig = new SparkMaxConfig().idleMode(IdleMode.kBrake); + SparkBaseConfig flexConfig = new SparkFlexConfig().idleMode(IdleMode.kBrake); + if (useFlexDrive) {flexDrive.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + else {maxDrive.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + if (useFlexTurn) {flexTurn.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + else {maxTurn.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} } public void coast() { - SparkBaseConfig config = new SparkMaxConfig().idleMode(IdleMode.kCoast); - drive.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - turn .configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + SparkBaseConfig maxConfig = new SparkMaxConfig().idleMode(IdleMode.kCoast); + SparkBaseConfig flexConfig = new SparkFlexConfig().idleMode(IdleMode.kCoast); + if (useFlexDrive) {flexDrive.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + else {maxDrive.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + if (useFlexTurn) {flexTurn.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + else {maxTurn.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} } /** @@ -464,7 +782,7 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("Incremental Position", () -> turnEncoder.getPosition().getValueAsDouble(), null); builder.addDoubleProperty("Absolute Angle (deg)", () -> Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble()), null); builder.addDoubleProperty("Turn Measured Pos (deg)", this::getModuleAngle, null); - builder.addDoubleProperty("Encoder Position", drive.getEncoder()::getPosition, null); + builder.addDoubleProperty("Encoder Position", driveRelEnc::getPosition, null); // Display the speed that the robot thinks it is travelling at. builder.addDoubleProperty("Current Speed", this::getCurrentSpeed, null); builder.addDoubleProperty("Turn Setpoint Pos (deg)", () -> turnPIDController.getSetpoint().position, null); @@ -492,8 +810,10 @@ public void initSendable(SendableBuilder builder) { */ public SwerveModuleSim createSim(Mass massOnWheel, double turnGearing, double turnMoiKgM2) { double driveMoiKgM2 = massOnWheel.in(Kilogram) * Math.pow(config.wheelDiameterMeters/2, 2); - return new SwerveModuleSim(drive.getDeviceId(), config.driveGearing, driveMoiKgM2, - turn.getDeviceId(), turnEncoder.getDeviceID(), turnGearing, turnMoiKgM2); + int driveId = useFlexDrive ? flexDrive.getDeviceId() : maxDrive.getDeviceId(); + int turnId = useFlexTurn ? flexTurn.getDeviceId() : maxTurn.getDeviceId(); + return new SwerveModuleSim(driveId, config.driveGearing, driveMoiKgM2, + turnId, turnEncoder.getDeviceID(), turnGearing, turnMoiKgM2); } /** diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index 4987540..c16ccc0 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -23,7 +23,7 @@ public class SwerveModuleSim { private Timer timer = new Timer(); /** - * Constructs a SwerveModuleSim that simulates the physics of a swerve module. + * Constructs a SwerveModuleSim that simulates the physics of a swerve module with Neos. * * @param drivePortNum the port of the SparkMax drive motor * @param driveGearing the gearing reduction between the drive motor and the wheel @@ -35,13 +35,61 @@ public class SwerveModuleSim { */ public SwerveModuleSim(int drivePortNum, double driveGearing, double driveMoiKgM2, int turnMotorPortNum, int turnEncoderPortNum, double turnGearing, double turnMoiKgM2) { - driveMotorSim = new SimDeviceSim("CANMotor:CANSparkMax", drivePortNum); - driveEncoderSim = new SimDeviceSim("CANEncoder:CANSparkMax", drivePortNum); + driveMotorSim = new SimDeviceSim("CANMotor:SparkMax", drivePortNum); + driveEncoderSim = new SimDeviceSim("CANEncoder:SparkMax", drivePortNum); DCMotor dcmotor = DCMotor.getNEO(1); drivePhysicsSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(dcmotor, driveMoiKgM2, driveGearing), dcmotor);//FIXME WHAT DO WE WANT THE MEASUREMENT STDDEVS TO BE? (LAST ARG) this.driveGearing = driveGearing; - turnMotorSim = new SimDeviceSim("CANMotor:CANSparkMax", turnMotorPortNum); + turnMotorSim = new SimDeviceSim("CANMotor:SparkMax", turnMotorPortNum); + turnEncoderSim = new SimDeviceSim("CANDutyCycle:CANCoder", turnEncoderPortNum); + turnPhysicsSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(dcmotor, turnMoiKgM2, turnGearing), + dcmotor); + } + + /** + * Constructs a SwerveModuleSim that simulates the physics of a swerve module with Neos and Neo Vortexes. + * + * @param drivePortNum the port of the SparkMax drive motor + * @param driveGearing the gearing reduction between the drive motor and the wheel + * @param driveMoiKgM2 the effective moment of inertia around the wheel axle (typciall the mass of the robot divided the number of modules times the square of the wheel radius) + * @param turnMotorPortNum the port of the SparkMax turn motor + * @param turnEncoderPortNum the port of the CANCoder measuring the module's angle + * @param turnGearing the gearing reduction between the turn motor and the module + * @param turnMoiKgM2 the moment of inertia of the part of the module turned by the turn motor (in kg m^2) + * @param useFlexDrive boolean for whether to use Vortex or Neos for drive motors + * @param useFlexTurn boolean for whether to use Vortex or Neos for turn motors + */ + public SwerveModuleSim(int drivePortNum, double driveGearing, double driveMoiKgM2, + int turnMotorPortNum, int turnEncoderPortNum, double turnGearing, double turnMoiKgM2, boolean useFlexDrive, boolean useFlexTurn) { + DCMotor dcmotor; + double[] measurementStdDevs = { + 0.01, // position noise + 0.1 // velocity noise + }; + if (useFlexDrive) { + driveMotorSim = new SimDeviceSim("CANMotor:SparkFlex", drivePortNum); + driveEncoderSim = new SimDeviceSim("CANEncoder:SparkFlex", drivePortNum); + dcmotor = DCMotor.getNeoVortex(1); + } + else { + driveMotorSim = new SimDeviceSim("CANMotor:SparkMax", drivePortNum); + driveEncoderSim = new SimDeviceSim("CANEncoder:SparkMax", drivePortNum); + dcmotor = DCMotor.getNEO(1); + } + + drivePhysicsSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(dcmotor, driveMoiKgM2, driveGearing), dcmotor, measurementStdDevs); + this.driveGearing = driveGearing; + + if (useFlexTurn) { + turnMotorSim = new SimDeviceSim("CANMotor:SparkFlex", turnMotorPortNum); + dcmotor = DCMotor.getNeoVortex(1); + } + else { + turnMotorSim = new SimDeviceSim("CANMotor:SparkMax", turnMotorPortNum); + dcmotor = DCMotor.getNEO(1); + } turnEncoderSim = new SimDeviceSim("CANDutyCycle:CANCoder", turnEncoderPortNum); turnPhysicsSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(dcmotor, turnMoiKgM2, turnGearing),