diff --git a/.github/workflows/javadoc.yml b/.github/workflows/javadoc.yml index f086b01c..abf4b657 100644 --- a/.github/workflows/javadoc.yml +++ b/.github/workflows/javadoc.yml @@ -34,4 +34,4 @@ jobs: steps: - name: Deploy to GitHub Pages id: deployment - uses: actions/deploy-pages@v4 + uses: actions/deploy-pages@v4 \ No newline at end of file diff --git a/README.md b/README.md index d772d4ef..bf5a94e4 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # FRC Team 199 Library -This projects builds `lib199.jar` which contains code that reuse across projects/years. The javadocs can be found [here](https://docs.carlmontrobotics.org/lib199/). +This projects builds `lib199.jar` which contains code that reuse across projects/years. The javadocs can be found [here](https://deepbluerobotics.github.io/lib199/). ## Changelog A changelog is automatically generated with each release and is available on the [Releases](https://github.com/DeepBlueRobotics/lib199/releases) page. Please write documentation for all changes and, if significant enough, add appropriate information to the [programming training website](https://deep-blue-training.readthedocs.io/en/latest/). diff --git a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java index 2f013360..3b6ed67c 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java +++ b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java @@ -13,6 +13,9 @@ public class Lib199Subsystem implements Subsystem { private static final CopyOnWriteArrayList periodicSimulationMethods = new CopyOnWriteArrayList<>(); private static final Consumer RUN_RUNNABLE = Runnable::run; + @Deprecated + public static final long asyncSleepTime = 20; + static { ensureRegistered(); } @@ -40,10 +43,37 @@ public static void registerPeriodic(Runnable method) { periodicMethods.add(method); } + @Deprecated + /** + * @deprecated Use registerSimulationPeriodic + * @param method + */ + public static void simulationPeriodic(Runnable method) { + registerSimulationPeriodic(method); + } + public static void registerSimulationPeriodic(Runnable method) { periodicSimulationMethods.add(method); } + @Deprecated + /** + * @deprecated Use registerPeriodic + * @param method + */ + public static void registerAsyncPeriodic(Runnable method) { + registerPeriodic(method); + } + + @Deprecated + /** + * @deprecated Use registerSimulationPeriodic + * @param method + */ + public static void registerAsyncSimulationPeriodic(Runnable method) { + registerSimulationPeriodic(method); + } + @Override public void periodic() { periodicMethods.forEach(RUN_RUNNABLE); @@ -54,6 +84,13 @@ public void simulationPeriodic() { periodicSimulationMethods.forEach(RUN_RUNNABLE); } + @Deprecated + /** + * No longer does anything. + */ + public synchronized void asyncPeriodic() { + } + private Lib199Subsystem() {} } diff --git a/src/main/java/org/carlmontrobotics/lib199/Limelight.java b/src/main/java/org/carlmontrobotics/lib199/Limelight.java index 0aa06a65..ccd44f67 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/MotorConfig.java b/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java index 5f1dbabe..9693498f 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorConfig.java @@ -2,27 +2,20 @@ public class MotorConfig { - public static final MotorConfig NEO = new MotorConfig(70, 40, MotorControllerType.SPARK_MAX); - public static final MotorConfig NEO_550 = new MotorConfig(70, 40, MotorControllerType.SPARK_MAX); - public static final MotorConfig NEO_2 = new MotorConfig(70, 40, MotorControllerType.SPARK_MAX); //TODO: find the max temp for NEO 2.0 + public static final MotorConfig NEO = new MotorConfig(70, 40); + public static final MotorConfig NEO_550 = new MotorConfig(40, 20); // The temp limit of 100C for the Vortex is based on the fact that its temp sensors are mounted directly on the // windings (which is not the case for the Neo or Neo550, causing them to have very delayed temp readings) and the // fact that the winding enamel will melt at 140C. // See: https://www.chiefdelphi.com/t/rev-robotics-spark-flex-and-neo-vortex/442595/349?u=brettle // As a result I think 100C should be safe. I wouldn't increase it past 120. --Dean - public static final MotorConfig NEO_VORTEX = new MotorConfig(100, 40, MotorControllerType.SPARK_FLEX); - public static final MotorConfig NEO_SOLO_VORTEX = new MotorConfig(100, 40, MotorControllerType.SPARK_MAX); + public static final MotorConfig NEO_VORTEX = new MotorConfig(100, 60); public final int temperatureLimitCelsius, currentLimitAmps; - public final MotorControllerType controllerType; - public MotorConfig(int temperatureLimitCelsius, int currentLimitAmps, MotorControllerType controllerType) { + public MotorConfig(int temperatureLimitCelsius, int currentLimitAmps) { this.temperatureLimitCelsius = temperatureLimitCelsius; this.currentLimitAmps = currentLimitAmps; - this.controllerType = controllerType; } - public MotorControllerType getControllerType() { - return controllerType; - } } diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java index 66cdb2a2..16048451 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java @@ -7,29 +7,56 @@ package org.carlmontrobotics.lib199; -import org.carlmontrobotics.lib199.sim.MockSparkFlex; -import org.carlmontrobotics.lib199.sim.MockSparkMax; -// import org.carlmontrobotics.lib199.sim.MockSparkFlex; -// import org.carlmontrobotics.lib199.sim.MockSparkMax; -import org.carlmontrobotics.lib199.sim.MockTalonSRX; - import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel; +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; +import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkClosedLoopController; +// import org.carlmontrobotics.lib199.sim.MockSparkFlex; +// import org.carlmontrobotics.lib199.sim.MockSparkMax; +import org.carlmontrobotics.lib199.sim.MockTalonSRX; +import org.carlmontrobotics.lib199.sim.MockVictorSPX; +import org.carlmontrobotics.lib199.sim.MockedCANCoder; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; import edu.wpi.first.wpilibj.RobotBase; /** * Add your docs here. */ public class MotorControllerFactory { + public static WPI_VictorSPX createVictor(int port) { + WPI_VictorSPX victor; + if (RobotBase.isReal()) { + victor = new WPI_VictorSPX(port); + } else { + victor = MockVictorSPX.createMockVictorSPX(port); + } + + // Put all configurations for the victor motor controllers in here. + MotorErrors.reportError(victor.configNominalOutputForward(0, 10)); + MotorErrors.reportError(victor.configNominalOutputReverse(0, 10)); + MotorErrors.reportError(victor.configPeakOutputForward(1, 10)); + MotorErrors.reportError(victor.configPeakOutputReverse(-1, 10)); + MotorErrors.reportError(victor.configNeutralDeadband(0.001, 10)); + victor.setNeutralMode(NeutralMode.Brake); + + return victor; + } + public static WPI_TalonSRX createTalon(int id) { WPI_TalonSRX talon; if (RobotBase.isReal()) { @@ -54,6 +81,7 @@ public static WPI_TalonSRX createTalon(int id) { return talon; } + /** * Create a default sparkMax controller (NEO or 550). * @@ -61,7 +89,29 @@ public static WPI_TalonSRX createTalon(int id) { * @param motorConfig either MotorConfig.NEO or MotorConfig.NEO_550 */ public static SparkMax createSparkMax(int id, MotorConfig motorConfig) { - return createSparkMax(id, motorConfig, sparkConfig(motorConfig)); + SparkMax spark=null; + if (RobotBase.isReal()) { + spark = new SparkMax(id, SparkLowLevel.MotorType.kBrushless); + } else { + System.err.println("heyy... lib199 doesn't have sim support sorri"); + // spark = MockSparkMax.createMockSparkMax(id, SparkLowLevel.MotorType.kBrushless); + } + + // config.setPeriodicFramePeriod(SparkLowLevel.PeriodicFrame.kStatus0, 1); + if (spark!=null) + MotorErrors.reportSparkMaxTemp(spark, motorConfig.temperatureLimitCelsius); + + MotorErrors.checkSparkMaxErrors(spark); + + if (motorConfig==MotorConfig.NEO || motorConfig==MotorConfig.NEO_550) + spark.configure(baseSparkMaxConfig(), SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + else if (motorConfig==MotorConfig.NEO_VORTEX) + spark.configure(baseSparkFlexConfig(), SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + else + spark.configure(baseSparkConfig(), SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + + + return spark; } /** * Create a sparkMax controller (NEO or 550) with custom settings. @@ -69,30 +119,48 @@ public static SparkMax createSparkMax(int id, MotorConfig motorConfig) { * @param id the port of the motor controller * @param config the custom config to set */ - public static SparkMax createSparkMax(int id, MotorConfig motorConfig, SparkBaseConfig config) { - SparkMax spark; + public static SparkMax createSparkMax(int id, SparkBaseConfig config) { + SparkMax spark = null; if (RobotBase.isReal()) { spark = new SparkMax(id, SparkLowLevel.MotorType.kBrushless); } else { - spark = MockSparkMax.createMockSparkMax(id, SparkLowLevel.MotorType.kBrushless, MockSparkMax.NEOType.NEO); + System.err.println("heyy... lib199 doesn't have sim support sorri"); + // spark = MockSparkMax.createMockSparkMax(id, SparkLowLevel.MotorType.kBrushless); } - spark.configure( - config, - SparkBase.ResetMode.kResetSafeParameters, - SparkBase.PersistMode.kNoPersistParameters - ); - MotorErrors.reportSparkTemp(spark, motorConfig.temperatureLimitCelsius); - MotorErrors.checkSparkErrors(spark); + if (spark!=null) + spark.configure( + config, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters + ); return spark; } /** - * Create a default SparkFlex-Vortex controller. + * Create a default sparkFlex-Vortex controller. * * @param id the port of the motor controller */ public static SparkFlex createSparkFlex(int id) { - return createSparkFlex(id, MotorConfig.NEO_VORTEX, sparkConfig(MotorConfig.NEO_VORTEX)); + MotorConfig motorConfig = MotorConfig.NEO_VORTEX; + + SparkFlex spark=null; + if (RobotBase.isReal()) { + spark = new SparkFlex(id, SparkLowLevel.MotorType.kBrushless); + } else { + System.err.println("heyy... lib199 doesn't have sim support sorri"); + // spark = MockSparkMax.createMockSparkMax(id, SparkLowLevel.MotorType.kBrushless); + } + + // config.setPeriodicFramePeriod(SparkLowLevel.PeriodicFrame.kStatus0, 1); + if (spark!=null) + MotorErrors.reportSparkTemp(spark, motorConfig.temperatureLimitCelsius); + + MotorErrors.checkSparkErrors(spark); + + spark.configure(baseSparkFlexConfig(), SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); + + return spark; } /** * Create a sparkFlex controller (VORTEX) with custom settings. @@ -100,49 +168,95 @@ public static SparkFlex createSparkFlex(int id) { * @param id the port of the motor controller * @param config the custom config to set */ - public static SparkFlex createSparkFlex(int id, MotorConfig motorConfig, SparkBaseConfig config) { + public static SparkFlex createSparkFlex(int id, SparkFlexConfig config) { SparkFlex spark = null; if (RobotBase.isReal()) { spark = new SparkFlex(id, SparkLowLevel.MotorType.kBrushless); } else { - spark = MockSparkFlex.createMockSparkFlex(id, SparkLowLevel.MotorType.kBrushless); + System.err.println("heyy... lib199 doesn't have sim support sorri"); + // spark = MockSparkFlex.createMockSparkFlex(id, SparkLowLevel.MotorType.kBrushless); } - - MotorErrors.reportSparkTemp(spark, motorConfig.temperatureLimitCelsius); - MotorErrors.checkSparkErrors(spark); + if (spark!=null) + spark.configure( + config, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters + ); return spark; } - public static SparkBaseConfig createConfig(MotorControllerType type) { - SparkBaseConfig config = null; - switch(type){ - case SPARK_MAX: - config = new SparkMaxConfig(); - break; - case SPARK_FLEX: - config = new SparkFlexConfig(); - break; - } + //does this not do the same as baseSparkMaxConfig, as it also creates a Spark MaxConfig? + public static SparkBaseConfig baseSparkConfig() { + SparkMaxConfig config = new SparkMaxConfig(); + + config.idleMode(IdleMode.kBrake); + + config.voltageCompensation(12);//FIXME does this need to be different for different motors? + config.smartCurrentLimit(50); + + config.closedLoop + .minOutput(-1) + .maxOutput(1) + .pid(0,0,0) + .velocityFF(0); + return config; } + /** + * Overrides an old config - but does not change other settings. + */ + //does this not do the same as baseSparkMaxConfig, as it also creates a Spark MaxConfig? + public static SparkBaseConfig baseSparkConfig(SparkMaxConfig config) { + config.idleMode(IdleMode.kBrake); + + config.voltageCompensation(12);//FIXME does this need to be different for different motors? + config.smartCurrentLimit(50); + + config.closedLoop + .minOutput(-1) + .maxOutput(1) + .pid(0,0,0) + .velocityFF(0); - public static MotorControllerType getControllerType(SparkBase motor){ - if(motor instanceof SparkMax){ - return MotorControllerType.SPARK_MAX; - }else if(motor instanceof SparkFlex){ - return MotorControllerType.SPARK_FLEX; - } - return null; + return config; } - - public static SparkBaseConfig sparkConfig(MotorConfig motorConfig){ - SparkBaseConfig config = motorConfig.controllerType.createConfig(); - //configs that apply to all motors + /** + * Overrides an old config - but does not change other settings. + */ + public static SparkMaxConfig baseSparkMaxConfig(SparkMaxConfig config){ + //typical operating voltage: 12V. + return (SparkMaxConfig) baseSparkConfig(config);//FIXME apply needed config changes for each controller + } + public static SparkMaxConfig baseSparkMaxConfig(){ + return (SparkMaxConfig) baseSparkConfig(); + } + /** + * Overrides an old config - but does not change other settings. + */ + public static SparkFlexConfig baseSparkFlexConfig(SparkFlexConfig config){ + //typical operating voltage: 12V. ( same as sparkMax ) config.idleMode(IdleMode.kBrake); - config.voltageCompensation(12); - config.smartCurrentLimit(motorConfig.currentLimitAmps); + + config.voltageCompensation(12);//FIXME does this need to be different for different motors? + config.smartCurrentLimit(50); + + config.closedLoop + .minOutput(-1) + .maxOutput(1) + .pid(0,0,0) + .velocityFF(0); + + return config; + } + public static SparkFlexConfig baseSparkFlexConfig(){//why? no Se. + SparkFlexConfig config = new SparkFlexConfig(); + config.idleMode(IdleMode.kBrake); + + config.voltageCompensation(12);//FIXME does this need to be different for different motors? + config.smartCurrentLimit(50); + config.closedLoop .minOutput(-1) .maxOutput(1) @@ -151,4 +265,48 @@ public static SparkBaseConfig sparkConfig(MotorConfig motorConfig){ return config; } -} \ No newline at end of file + + /** + * @deprecated Use {@link SensorFactory#createCANCoder(int)} instead. + */ + @Deprecated + public static CANcoder createCANCoder(int port) { + CANcoder canCoder = new CANcoder(port); + if(RobotBase.isSimulation()) new MockedCANCoder(canCoder); + return canCoder; + } + + /** + * Configures a USB Camera. + * See {@link CameraServer#startAutomaticCapture} for more details. + * This MUST be called AFTER AHRS initialization or the code will be unable to connect to the gyro. + * + * @return The configured camera + * + * @deprecated Use {@link SensorFactory#configureCamera()} instead. + */ + @Deprecated + public static UsbCamera configureCamera() { + UsbCamera camera = CameraServer.startAutomaticCapture(); + camera.setConnectionStrategy(ConnectionStrategy.kKeepOpen); + CameraServer.getServer().setSource(camera); + return camera; + } + + /** + * This method is equivalent to calling {@link #configureCamera()} {@code numCameras} times. + * The last camera will be set as the primary Camera feed. + * To change it, call {@code CameraServer.getServer().setSource()}. + * + * @param numCameras The number of cameras to configure + * @return The configured cameras. + * + * @deprecated Use {@link SensorFactory#configureCameras(int)} instead. + */ + @Deprecated + public static UsbCamera[] configureCameras(int numCameras) { + UsbCamera[] cameras = new UsbCamera[numCameras]; + for(int i = 0; i < numCameras; i++) cameras[i] = configureCamera(); + return cameras; + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorControllerType.java b/src/main/java/org/carlmontrobotics/lib199/MotorControllerType.java deleted file mode 100644 index 15608bf5..00000000 --- a/src/main/java/org/carlmontrobotics/lib199/MotorControllerType.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.carlmontrobotics.lib199; - -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.config.SparkBaseConfig; - -public enum MotorControllerType { - SPARK_MAX, - SPARK_FLEX; - public static MotorControllerType getMotorControllerType(SparkBase motor){ - return MotorControllerFactory.getControllerType(motor); - } - public SparkBaseConfig createConfig(){ - return MotorControllerFactory.createConfig(this); - } -} diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java index d053d72f..076d30d8 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorErrors.java @@ -12,14 +12,11 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.Faults; import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.REVLibError; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import org.carlmontrobotics.lib199.MotorControllerFactory; - public final class MotorErrors { private static final Map temperatureSparks = new ConcurrentSkipListMap<>(); @@ -30,10 +27,6 @@ public final class MotorErrors { private static final Map stickyFlags = new ConcurrentSkipListMap<>( (spark1, spark2) -> (spark1.getDeviceId() - spark2.getDeviceId())); - private static final SparkBaseConfig OVERHEAT_MAX_CONFIG = new SparkMaxConfig().smartCurrentLimit(1); - private static final SparkBaseConfig OVERHEAT_FLEX_CONFIG = new SparkFlexConfig().smartCurrentLimit(1); - - public static final int kOverheatTripCount = 5; static { @@ -80,8 +73,8 @@ public static void checkSparkErrors(SparkBase spark) { // short faults = spark.getFaults(); Faults faults = spark.getFaults(); Faults stickyFaults = spark.getStickyFaults(); - Faults prevFaults = flags.getOrDefault(spark, null); - Faults prevStickyFaults = stickyFlags.getOrDefault(spark, null); + Faults prevFaults = flags.containsKey(spark) ? flags.get(spark) : null; + Faults prevStickyFaults = stickyFlags.containsKey(spark) ? stickyFlags.get(spark) : null; if (spark.hasActiveFault() && prevFaults!=null && prevFaults.rawBits != faults.rawBits) { System.err.println("Fault Errors! (spark id " + spark.getDeviceId() + "): [" + formatFaults(spark) + "], ooF!"); @@ -94,7 +87,13 @@ public static void checkSparkErrors(SparkBase spark) { stickyFlags.put(spark, stickyFaults); } - private static String formatFaults(Faults f) { + @Deprecated + public static void checkSparkMaxErrors(SparkMax spark) { + checkSparkErrors((SparkBase)spark); + } + + private static String formatFaults(SparkBase spark) { + Faults f = spark.getFaults(); return "" //i hope this makes you proud of yourself, REVLib + (f.can ? "CAN " : "") + (f.escEeprom ? "Flash ROM " : "") @@ -107,14 +106,23 @@ private static String formatFaults(Faults f) { ; } - private static String formatFaults(SparkBase spark) { - Faults f = spark.getFaults(); - return formatFaults(f); - } - private static String formatStickyFaults(SparkBase spark) { Faults f = spark.getStickyFaults(); - return formatFaults(f); + return "" + + (f.can ? "CAN " : "") + + (f.escEeprom ? "Flash ROM " : "") + + (f.firmware ? "Firmware " : "") + + (f.gateDriver ? "Gate Driver " : "") + + (f.motorType ? "Motor Type " : "") + + (f.other ? "Other " : "") + + (f.sensor ? "Sensor " : "") + + (f.temperature ? "Temperature " : "") + ; + } + + @Deprecated + public static void printSparkMaxErrorMessages() { + printSparkErrorMessages(); } public static void printSparkErrorMessages() { @@ -129,12 +137,27 @@ static void reportNextNSparkErrors(int n) { lastSparkErrorIndexReported = (lastSparkErrorIndexReported + n) % flags.size(); } - public static boolean isSparkOverheated(SparkBase spark){ + //what does this even supposed to do?? + public static SparkMax createDummySparkMax() { + return Mocks.mock(SparkMax.class, new REVLibErrorAnswer()); + } + + @Deprecated + public static void reportSparkMaxTemp(SparkMax spark, TemperatureLimit temperatureLimit) { + reportSparkMaxTemp(spark, temperatureLimit.limit); + } + + public static boolean isSparkMaxOverheated(SparkMax spark){ int id = spark.getDeviceId(); int motorMaxTemp = sparkTemperatureLimits.get(id); return ( spark.getMotorTemperature() >= motorMaxTemp ); } + @Deprecated + public static void reportSparkMaxTemp(SparkMax spark, int temperatureLimit) { + reportSparkTemp((SparkBase) spark, temperatureLimit); + } + public static void reportSparkTemp(SparkBase spark, int temperatureLimit) { int id = spark.getDeviceId(); temperatureSparks.put(id, spark); @@ -142,6 +165,11 @@ public static void reportSparkTemp(SparkBase spark, int temperatureLimit) { overheatedSparks.put(id, 0); } + @Deprecated + public static void doReportSparkMaxTemp() { + doReportSparkTemp(); + } + public static void doReportSparkTemp() { temperatureSparks.forEach(MotorErrors::reportSparkTemp); } @@ -187,24 +215,24 @@ private static void reportSparkTemp(int port, SparkBase spark) { System.err.println("Port " + port + " spark is operating at " + temp + " degrees Celsius! It will be disabled until the robot code is restarted."); } - switch(MotorControllerFactory.getControllerType(spark)){ - case SPARK_MAX: - spark.configure( - OVERHEAT_MAX_CONFIG, - SparkBase.ResetMode.kNoResetSafeParameters, - SparkBase.PersistMode.kNoPersistParameters); - break; - case SPARK_FLEX: - spark.configure( - OVERHEAT_FLEX_CONFIG, - SparkBase.ResetMode.kNoResetSafeParameters, - SparkBase.PersistMode.kNoPersistParameters); - break; - default: - System.err.println("Unknown spark :("); - } + spark.configure( + new SparkMaxConfig().smartCurrentLimit(1), + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); } } private MotorErrors() {} + + @Deprecated + public static enum TemperatureLimit { + NEO(70), NEO_550(40); + + public final int limit; + + private TemperatureLimit(int limit) { + this.limit = limit; + } + } + } 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 00000000..0cf591b2 --- /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 00000000..07b3b2f1 --- /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 00000000..46d11e86 --- /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 00000000..24ce1968 --- /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 00000000..83820520 --- /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 00000000..97ea3cec --- /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/vendorLibs/Elastic.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elastic.java similarity index 94% rename from src/main/java/org/carlmontrobotics/lib199/vendorLibs/Elastic.java rename to src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elastic.java index 8c8cca42..cf4b5210 100644 --- a/src/main/java/org/carlmontrobotics/lib199/vendorLibs/Elastic.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elastic.java @@ -1,11 +1,9 @@ // 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 +// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE -// From: https://frc-elastic.gitbook.io/docs/additional-features-and-references/robot-notifications-with-elasticlib and https://github.com/Gold872/elastic-dashboard/blob/main/elasticlib/Elastic.java - -package org.carlmontrobotics.lib199.vendorLibs; +package org.carlmontrobotics.lib199.SimpleMechs; import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.core.JsonProcessingException; @@ -14,12 +12,6 @@ import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.StringPublisher; import edu.wpi.first.networktables.StringTopic; -/** - * A class that provides methods for interacting with the Elastic dashboard, including sending - * notifications and selecting tabs. This taken straight from the official Elastic documentation - * (see https://frc-elastic.gitbook.io/docs/additional-features-and-references/robot-notifications-with-elasticlib for documentation - * and for original code https://github.com/Gold872/elastic-dashboard/blob/main/elasticlib/Elastic.java) - */ public final class Elastic { private static final StringTopic notificationTopic = 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 00000000..914c63bf --- /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 00000000..c268fd7c --- /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 00000000..29e9f61f --- /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 00000000..d6ad0ee4 --- /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 00000000..e16d810e --- /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 00000000..f851faa0 --- /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 00000000..3ca35db3 --- /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/SparkVelocityPIDController.java b/src/main/java/org/carlmontrobotics/lib199/SparkVelocityPIDController.java index 3a3886c6..66bb7c03 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SparkVelocityPIDController.java +++ b/src/main/java/org/carlmontrobotics/lib199/SparkVelocityPIDController.java @@ -1,9 +1,10 @@ package org.carlmontrobotics.lib199; -import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; +// import com.revrobotics.SparkBase.ControlType; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; @@ -19,14 +20,14 @@ public class SparkVelocityPIDController implements Sendable { @SuppressWarnings("unused") - private final SparkBase spark; + private final SparkMax spark; private final SparkClosedLoopController pidController; private final RelativeEncoder encoder; private final String name; private double targetSpeed, tolerance; private double currentP, currentI, currentD, kS, kV; - public SparkVelocityPIDController(String name, SparkBase spark, double defaultP, double defaultI, double defaultD, double kS, double kV, double targetSpeed, double tolerance) { + public SparkVelocityPIDController(String name, SparkMax spark, double defaultP, double defaultI, double defaultD, double kS, double kV, double targetSpeed, double tolerance) { this.spark = spark; this.pidController = spark.getClosedLoopController(); this.encoder = spark.getEncoder(); diff --git a/src/main/java/org/carlmontrobotics/lib199/logging/VarType.java b/src/main/java/org/carlmontrobotics/lib199/logging/VarType.java new file mode 100644 index 00000000..5ae5244b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/logging/VarType.java @@ -0,0 +1,10 @@ +package org.carlmontrobotics.lib199.logging; + +/** + * Represents the type of a variable in the logging code + * @deprecated Instead use WPILib's Logging API + */ +@Deprecated +public enum VarType { + BOOLEAN, INTEGER, DOUBLE, STRING; +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java index ef466638..2c94b326 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkBase.java @@ -52,10 +52,10 @@ public class MockSparkBase extends MockedMotorBase { private SparkAnalogSensorSim analogSensorImpl = null; private final String name; - public enum NEOType { //is it fine if we make it public so that MotorControllerFactory can access it? + enum NEOType { NEO(DCMotor.getNEO(1)), NEO550(DCMotor.getNeo550(1)), - VORTEX(DCMotor.getNeoVortex(1)), + Vortex(DCMotor.getNeoVortex(1)), UNKNOWN(DCMotor.getNEO(1)); public DCMotor dcMotor; @@ -73,21 +73,20 @@ private NEOType(DCMotor dcmotordata){ * to follow the inversion state of the motor and its {@code setInverted} method will be disabled. * @param name the name of the type of controller ("SparkMax" or "SparkFlex") * @param countsPerRev the number of counts per revolution of this controller's built-in encoder. - * @param neoType the type of NEO motor */ public MockSparkBase(int port, MotorType type, String name, int countsPerRev, NEOType neoType) { super(name, port); this.type = type; this.name = name; - if (neoType == NEOType.VORTEX){ //only vortex uses sparkflex - this.motor = new SparkFlex(port,type); + if (neoType != NEOType.Vortex){ //WARNING can't initialize a sparkbase without an actual spark... + this.motor = new SparkMax(port,type); this.spark = new SparkSim( this.motor, neoType.dcMotor ); - } else { //WARNING can't initialize a sparkbase without an actual spark... - this.motor = new SparkMax(port,type); + } else { //only vortex uses sparkflex + this.motor = new SparkFlex(port,type); this.spark = new SparkSim( this.motor, neoType.dcMotor @@ -230,6 +229,7 @@ public void close() { if (encoder != null) { encoder.close(); } + //simply drop all references for garbage collection (?) absoluteEncoderImpl=null; analogSensorImpl=null; alternateEncoder=null; diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java index acc97814..78a46f71 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkFlex.java @@ -9,7 +9,7 @@ public class MockSparkFlex extends MockSparkBase { public MockSparkFlex(int port, MotorType type) { - super(port, type, "CANSparkFlex", 7168, NEOType.VORTEX); + super(port, type, "CANSparkFlex", 7168, NEOType.Vortex); } public static SparkFlex createMockSparkFlex(int portPWM, MotorType type) { diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockVictorSPX.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockVictorSPX.java new file mode 100644 index 00000000..bb59d10d --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockVictorSPX.java @@ -0,0 +1,19 @@ +package org.carlmontrobotics.lib199.sim; + +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; + +import org.carlmontrobotics.lib199.ErrorCodeAnswer; +import org.carlmontrobotics.lib199.Mocks; + +import edu.wpi.first.wpilibj.motorcontrol.VictorSP; + +public class MockVictorSPX extends MockPhoenixController { + public MockVictorSPX(int portPWM) { + super(portPWM); + motorPWM = new VictorSP(portPWM); + } + + public static WPI_VictorSPX createMockVictorSPX(int portPWM) { + return Mocks.createMock(WPI_VictorSPX.class, new MockVictorSPX(portPWM), new ErrorCodeAnswer(), AutoCloseable.class); + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java index a7847a60..02dfd603 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java @@ -37,4 +37,5 @@ public void callback(String name, int handle, int direction, HALValue value) { }, true); sims.put(port, this); } + } \ 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 6776a56c..06c4c867 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -43,8 +43,8 @@ public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseabl * @param countsPerRev The value that this.getCountsPerRevolution() should return * @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 {@code setZeroOffset(double)}. + * one rotation via. {@link MathUtil#inputModulus(double, double, double)}, disables + * {@link #setPosition(double)}, and enables setZeroOffset(double). * * {@link #getVelocity()} will return a value in rpm. */ @@ -58,8 +58,8 @@ public MockedEncoder(SimDevice device, int countsPerRev, boolean analog, * @param countsPerRev The value that this.getCountsPerRevolution() should return * @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 {@code setZeroOffset(double)}. + * one rotation via. {@link MathUtil#inputModulus(double, double, double)}, disables + * {@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 {@code 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 1619a0ae..7935eae9 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -6,20 +6,21 @@ import java.util.function.Supplier; -import org.carlmontrobotics.lib199.MotorControllerType; import org.mockito.internal.reporting.SmartPrinter; +// import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; 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.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -30,8 +31,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.Measure; import edu.wpi.first.units.measure.Mass; +import edu.wpi.first.units.Measure; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -48,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 SparkBaseConfig turnConfig; - private SparkBaseConfig driveConfig; + private SparkMaxConfig turnMaxConfig = new SparkMaxConfig(); + private SparkMaxConfig driveMaxConfig = new SparkMaxConfig(); + private SparkFlexConfig turnFlexConfig = new SparkFlexConfig(); + private SparkFlexConfig driveFlexConfig = new SparkFlexConfig(); private ModuleType type; - private SparkBase 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; @@ -68,46 +84,127 @@ public enum ModuleType {FL, FR, BL, BR}; private double turnSpeedCorrectionVolts, turnFFVolts, turnVolts; private double maxTurnVelocityWithoutTippingRps; + public SwerveModule(SwerveConfig config, ModuleType type, SparkMax 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.maxDrive = drive; + this.useFlexDrive = false; + this.driveRelEnc = maxDrive.getEncoder(); - MotorControllerType driveMotorType; - MotorControllerType turnMotorType; - - public SwerveModule(SwerveConfig config, ModuleType type, SparkBase drive, SparkBase turn, CANcoder turnEncoder, + 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) { - driveMotorType = MotorControllerType.getMotorControllerType(drive); - turnMotorType = MotorControllerType.getMotorControllerType(turn); - driveConfig = driveMotorType.createConfig(); - turnConfig = turnMotorType.createConfig(); - //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(); - 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; - driveConfig.encoder + driveMaxConfig.encoder .positionConversionFactor(drivePositionFactor) - .velocityConversionFactor(driveVelocityFactor) - .quadratureAverageDepth(2); + .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], @@ -121,23 +218,15 @@ public SwerveModule(SwerveConfig config, ModuleType type, SparkBase drive, Spark config.drivekD[arrIndex]); /* offset for 1 relative encoder count */ - switch(MotorControllerType.getMotorControllerType(drive)) { - case SPARK_MAX: - drivetoleranceMPerS = (1.0 - / (double)(((SparkMax)drive).configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) - / Units.millisecondsToSeconds(((SparkMax)drive).configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * ((SparkMax)drive).configAccessor.encoder.getQuadratureAverageDepth()); - break; - case SPARK_FLEX: - drivetoleranceMPerS = (1.0 - / (double)(((SparkFlex)drive).configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) - / Units.millisecondsToSeconds(((SparkFlex)drive).configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * ((SparkFlex)drive).configAccessor.encoder.getQuadratureAverageDepth()); - break; - } + drivetoleranceMPerS = (1.0 + / (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], @@ -177,29 +266,202 @@ public SwerveModule(SwerveConfig config, ModuleType type, SparkBase drive, Spark 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); - // 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); + 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.putData(this); + turnPIDController.reset(getModuleAngle()); + + 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() { @@ -207,9 +469,10 @@ public ModuleType getType() { } private double prevTurnVelocity = 0; + public void periodic() { drivePeriodic(); - // updateSmartDashboard(); + updateSmartDashboard(); turnPeriodic(); } @@ -220,8 +483,10 @@ public void drivePeriodic() { double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF : backwardSimpleMotorFF) .calculateWithVelocities( actualSpeed, - desiredSpeed + extraAccel * TimedRobot.kDefaultPeriod //m/s + ( m/s^2 * s ) - ); + desiredSpeed + extraAccel * TimedRobot.kDefaultPeriod//m/s + ( m/s^2 * s ) + );//clippedAcceleration); + //calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get()) + // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc. // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed) double pidVolts = drivePIDController.calculate(actualSpeed, desiredSpeed); @@ -235,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() { @@ -270,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; } @@ -367,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(); } /** @@ -388,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()); @@ -448,46 +727,40 @@ public void updateSmartDashboard() { } public void toggleMode() { - switch(MotorControllerType.getMotorControllerType(drive)) { - case SPARK_MAX: - switch (MotorControllerType.getMotorControllerType(turn)) { - case SPARK_MAX: - if (((SparkMax)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkMax)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast(); - else brake(); - break; - case SPARK_FLEX: - if (((SparkMax)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkFlex)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast(); - else brake(); - break; - } - case SPARK_FLEX: - switch (MotorControllerType.getMotorControllerType(turn)) { - case SPARK_MAX: - if (((SparkFlex)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkMax)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast(); - else brake(); - break; - case SPARK_FLEX: - if (((SparkFlex)drive).configAccessor.getIdleMode() == IdleMode.kBrake && ((SparkFlex)turn).configAccessor.getIdleMode() == IdleMode.kCoast) coast(); - else brake(); - break; - } - break; + 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 = driveMotorType.createConfig().idleMode(IdleMode.kBrake); - drive.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - config = turnMotorType.createConfig().idleMode(IdleMode.kBrake); - 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 = driveMotorType.createConfig().idleMode(IdleMode.kCoast); - - drive.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - config = turnMotorType.createConfig().idleMode(IdleMode.kCoast); - 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);} } /** @@ -509,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); @@ -537,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 4987540d..c16ccc01 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), diff --git a/src/main/java/org/carlmontrobotics/lib199/vendorLibs/LimelightHelpers.java b/src/main/java/org/carlmontrobotics/lib199/vendorLibs/LimelightHelpers.java deleted file mode 100644 index 47aefa0f..00000000 --- a/src/main/java/org/carlmontrobotics/lib199/vendorLibs/LimelightHelpers.java +++ /dev/null @@ -1,1705 +0,0 @@ -/* -docs: https://docs.limelightvision.io/docs/docs-limelight/apis/limelight-lib -code: https://github.com/LimelightVision/limelightlib-wpijava/blob/main/LimelightHelpers.java -*/ - -//LimelightHelpers v1.12 (REQUIRES LLOS 2025.0 OR LATER) - -package org.carlmontrobotics.lib199.vendorLibs; - -import edu.wpi.first.networktables.DoubleArrayEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.TimestampedDoubleArray; -import org.carlmontrobotics.lib199.vendorLibs.LimelightHelpers.LimelightResults; -import org.carlmontrobotics.lib199.vendorLibs.LimelightHelpers.PoseEstimate; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; - -import java.io.IOException; -import java.net.HttpURLConnection; -import java.net.MalformedURLException; -import java.net.URL; -import java.util.Arrays; -import java.util.Map; -import java.util.concurrent.CompletableFuture; - -import com.fasterxml.jackson.annotation.JsonFormat; -import com.fasterxml.jackson.annotation.JsonFormat.Shape; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; -import java.util.concurrent.ConcurrentHashMap; - -/** - * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. - * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. - */ -public class LimelightHelpers { - - private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - - /** - * Represents a Color/Retroreflective Target Result extracted from JSON Output - */ - public static class LimelightTarget_Retro { - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - - } - - /** - * Represents an AprilTag/Fiducial Target Result extracted from JSON Output - */ - public static class LimelightTarget_Fiducial { - - @JsonProperty("fID") - public double fiducialID; - - @JsonProperty("fam") - public String fiducialFamily; - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - /** - * Represents a Barcode Target Result extracted from JSON Output - */ - public static class LimelightTarget_Barcode { - - /** - * Barcode family type (e.g. "QR", "DataMatrix", etc.) - */ - @JsonProperty("fam") - public String family; - - /** - * Gets the decoded data content of the barcode - */ - @JsonProperty("data") - public String data; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("pts") - public double[][] corners; - - public LimelightTarget_Barcode() { - } - - public String getFamily() { - return family; - } - } - - /** - * Represents a Neural Classifier Pipeline Result extracted from JSON Output - */ - public static class LimelightTarget_Classifier { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("zone") - public double zone; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Classifier() { - } - } - - /** - * Represents a Neural Detector Pipeline Result extracted from JSON Output - */ - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - public LimelightTarget_Detector() { - } - } - - /** - * Limelight Results object, parsed from a Limelight's JSON results output. - */ - public static class LimelightResults { - - public String error; - - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("cl") - public double latency_capture; - - public double latency_jsonParse; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; - - @JsonProperty("botpose_tagcount") - public double botpose_tagcount; - - @JsonProperty("botpose_span") - public double botpose_span; - - @JsonProperty("botpose_avgdist") - public double botpose_avgdist; - - @JsonProperty("botpose_avgarea") - public double botpose_avgarea; - - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } - - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); - } - - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); - } - - public Pose2d getBotPose2d() { - return toPose2D(botpose); - } - - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); - } - - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } - - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; - - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; - - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; - - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; - - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; - - public LimelightResults() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - camerapose_robotspace = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; - - } - - - } - - /** - * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. - */ - public static class RawFiducial { - public int id = 0; - public double txnc = 0; - public double tync = 0; - public double ta = 0; - public double distToCamera = 0; - public double distToRobot = 0; - public double ambiguity = 0; - - - public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; - } - - @Override - public boolean equals(Object obj) { - if (this == obj) return true; - if (obj == null || getClass() != obj.getClass()) return false; - RawFiducial other = (RawFiducial) obj; - return id == other.id && - Double.compare(txnc, other.txnc) == 0 && - Double.compare(tync, other.tync) == 0 && - Double.compare(ta, other.ta) == 0 && - Double.compare(distToCamera, other.distToCamera) == 0 && - Double.compare(distToRobot, other.distToRobot) == 0 && - Double.compare(ambiguity, other.ambiguity) == 0; - } - - } - - /** - * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. - */ - public static class RawDetection { - public int classId = 0; - public double txnc = 0; - public double tync = 0; - public double ta = 0; - public double corner0_X = 0; - public double corner0_Y = 0; - public double corner1_X = 0; - public double corner1_Y = 0; - public double corner2_X = 0; - public double corner2_Y = 0; - public double corner3_X = 0; - public double corner3_Y = 0; - - - public RawDetection(int classId, double txnc, double tync, double ta, - double corner0_X, double corner0_Y, - double corner1_X, double corner1_Y, - double corner2_X, double corner2_Y, - double corner3_X, double corner3_Y ) { - this.classId = classId; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.corner0_X = corner0_X; - this.corner0_Y = corner0_Y; - this.corner1_X = corner1_X; - this.corner1_Y = corner1_Y; - this.corner2_X = corner2_X; - this.corner2_Y = corner2_Y; - this.corner3_X = corner3_X; - this.corner3_Y = corner3_Y; - } - } - - /** - * Represents a 3D Pose Estimate. - */ - public static class PoseEstimate { - public Pose2d pose; - public double timestampSeconds; - public double latency; - public int tagCount; - public double tagSpan; - public double avgTagDist; - public double avgTagArea; - - public RawFiducial[] rawFiducials; - public boolean isMegaTag2; - - /** - * Instantiates a PoseEstimate object with default values - */ - public PoseEstimate() { - this.pose = new Pose2d(); - this.timestampSeconds = 0; - this.latency = 0; - this.tagCount = 0; - this.tagSpan = 0; - this.avgTagDist = 0; - this.avgTagArea = 0; - this.rawFiducials = new RawFiducial[]{}; - this.isMegaTag2 = false; - } - - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { - - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - this.isMegaTag2 = isMegaTag2; - } - - @Override - public boolean equals(Object obj) { - if (this == obj) return true; - if (obj == null || getClass() != obj.getClass()) return false; - PoseEstimate that = (PoseEstimate) obj; - // We don't compare the timestampSeconds as it isn't relevant for equality and makes - // unit testing harder - return Double.compare(that.latency, latency) == 0 - && tagCount == that.tagCount - && Double.compare(that.tagSpan, tagSpan) == 0 - && Double.compare(that.avgTagDist, avgTagDist) == 0 - && Double.compare(that.avgTagArea, avgTagArea) == 0 - && pose.equals(that.pose) - && Arrays.equals(rawFiducials, that.rawFiducials); - } - - } - - /** - * Encapsulates the state of an internal Limelight IMU. - */ - public static class IMUData { - public double robotYaw = 0.0; - public double Roll = 0.0; - public double Pitch = 0.0; - public double Yaw = 0.0; - public double gyroX = 0.0; - public double gyroY = 0.0; - public double gyroZ = 0.0; - public double accelX = 0.0; - public double accelY = 0.0; - public double accelZ = 0.0; - - public IMUData() {} - - public IMUData(double[] imuData) { - if (imuData != null && imuData.length >= 10) { - this.robotYaw = imuData[0]; - this.Roll = imuData[1]; - this.Pitch = imuData[2]; - this.Yaw = imuData[3]; - this.gyroX = imuData[4]; - this.gyroY = imuData[5]; - this.gyroZ = imuData[6]; - this.accelX = imuData[7]; - this.accelY = imuData[8]; - this.accelZ = imuData[9]; - } - } - } - - - private static ObjectMapper mapper; - - /** - * Print JSON Parse time to the console in milliseconds - */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if ("".equals(name) || name == null) { - return "limelight"; - } - return name; - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose3d object. - * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose3d object representing the pose, or empty Pose3d if invalid data - */ - public static Pose3d toPose3D(double[] inData){ - if(inData.length < 6) - { - //System.err.println("Bad LL 3D Pose Data!"); - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose2d object. - * Uses only x, y, and yaw components, ignoring z, roll, and pitch. - * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose2d object representing the pose, or empty Pose2d if invalid data - */ - public static Pose2d toPose2D(double[] inData){ - if(inData.length < 6) - { - //System.err.println("Bad LL 2D Pose Data!"); - return new Pose2d(); - } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); - } - - /** - * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * - * @param pose The Pose3d object to convert - * @return A 6-element array containing [x, y, z, roll, pitch, yaw] - */ - public static double[] pose3dToArray(Pose3d pose) { - double[] result = new double[6]; - result[0] = pose.getTranslation().getX(); - result[1] = pose.getTranslation().getY(); - result[2] = pose.getTranslation().getZ(); - result[3] = Units.radiansToDegrees(pose.getRotation().getX()); - result[4] = Units.radiansToDegrees(pose.getRotation().getY()); - result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); - return result; - } - - /** - * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. - * - * @param pose The Pose2d object to convert - * @return A 6-element array containing [x, y, 0, 0, 0, yaw] - */ - public static double[] pose2dToArray(Pose2d pose) { - double[] result = new double[6]; - result[0] = pose.getTranslation().getX(); - result[1] = pose.getTranslation().getY(); - result[2] = 0; - result[3] = Units.radiansToDegrees(0); - result[4] = Units.radiansToDegrees(0); - result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); - return result; - } - - private static double extractArrayEntry(double[] inData, int position){ - if(inData.length < position+1) - { - return 0; - } - return inData[position]; - } - - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { - DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); - - TimestampedDoubleArray tsValue = poseEntry.getAtomic(); - double[] poseArray = tsValue.value; - long timestamp = tsValue.timestamp; - - if (poseArray.length == 0) { - // Handle the case where no data is available - return null; // or some default PoseEstimate - } - - var pose = toPose2D(poseArray); - double latency = extractArrayEntry(poseArray, 6); - int tagCount = (int)extractArrayEntry(poseArray, 7); - double tagSpan = extractArrayEntry(poseArray, 8); - double tagDist = extractArrayEntry(poseArray, 9); - double tagArea = extractArrayEntry(poseArray, 10); - - // Convert server timestamp from microseconds to seconds and adjust for latency - double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); - - RawFiducial[] rawFiducials = new RawFiducial[tagCount]; - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial * tagCount; - - if (poseArray.length != expectedTotalVals) { - // Don't populate fiducials - } else { - for(int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int)poseArray[baseIndex]; - double txnc = poseArray[baseIndex + 1]; - double tync = poseArray[baseIndex + 2]; - double ta = poseArray[baseIndex + 3]; - double distToCamera = poseArray[baseIndex + 4]; - double distToRobot = poseArray[baseIndex + 5]; - double ambiguity = poseArray[baseIndex + 6]; - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - } - - return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); - } - - /** - * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawFiducial objects containing detection details - */ - public static RawFiducial[] getRawFiducials(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); - var rawFiducialArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 7; - if (rawFiducialArray.length % valsPerEntry != 0) { - return new RawFiducial[0]; - } - - int numFiducials = rawFiducialArray.length / valsPerEntry; - RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; - - for (int i = 0; i < numFiducials; i++) { - int baseIndex = i * valsPerEntry; - int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); - double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); - double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); - double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); - double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); - double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); - double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); - - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - - return rawFiducials; - } - - /** - * Gets the latest raw neural detector results from NetworkTables - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawDetection objects containing detection details - */ - public static RawDetection[] getRawDetections(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); - var rawDetectionArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 12; - if (rawDetectionArray.length % valsPerEntry != 0) { - return new RawDetection[0]; - } - - int numDetections = rawDetectionArray.length / valsPerEntry; - RawDetection[] rawDetections = new RawDetection[numDetections]; - - for (int i = 0; i < numDetections; i++) { - int baseIndex = i * valsPerEntry; // Starting index for this detection's data - int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); - double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); - double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); - double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); - double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); - double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); - double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); - double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); - double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); - double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); - double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); - double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); - - rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); - } - - return rawDetections; - } - - /** - * Prints detailed information about a PoseEstimate to standard output. - * Includes timestamp, latency, tag count, tag span, average tag distance, - * average tag area, and detailed information about each detected fiducial. - * - * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." - */ - public static void printPoseEstimate(PoseEstimate pose) { - if (pose == null) { - System.out.println("No PoseEstimate available."); - return; - } - - System.out.printf("Pose Estimate Information:%n"); - System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); - System.out.printf("Latency: %.3f ms%n", pose.latency); - System.out.printf("Tag Count: %d%n", pose.tagCount); - System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); - System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); - System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); - System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); - System.out.println(); - - if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { - System.out.println("No RawFiducials data available."); - return; - } - - System.out.println("Raw Fiducials Details:"); - for (int i = 0; i < pose.rawFiducials.length; i++) { - RawFiducial fiducial = pose.rawFiducials[i]; - System.out.printf(" Fiducial #%d:%n", i + 1); - System.out.printf(" ID: %d%n", fiducial.id); - System.out.printf(" TXNC: %.2f%n", fiducial.txnc); - System.out.printf(" TYNC: %.2f%n", fiducial.tync); - System.out.printf(" TA: %.2f%n", fiducial.ta); - System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); - System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); - System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); - System.out.println(); - } - } - - public static Boolean validPoseEstimate(PoseEstimate pose) { - return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; - } - - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } - - public static void Flush() { - NetworkTableInstance.getDefault().flush(); - } - - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } - - public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { - String key = tableName + "/" + entryName; - return doubleArrayEntries.computeIfAbsent(key, k -> { - NetworkTable table = getLimelightNTTable(tableName); - return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); - }); - } - - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } - - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } - - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); - } - - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } - - - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } - - public static String[] getLimelightNTStringArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); - } - - - public static URL getLimelightURLString(String tableName, String request) { - String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; - URL url; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); - } - return null; - } - ///// - ///// - - /** - * Does the Limelight have a valid target? - * @param limelightName Name of the Limelight camera ("" for default) - * @return True if a valid target is present, false otherwise - */ - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); - } - - /** - * Gets the horizontal offset from the crosshair to the target in degrees. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } - - /** - * Gets the vertical offset from the crosshair to the target in degrees. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } - - /** - * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTXNC(String limelightName) { - return getLimelightNTDouble(limelightName, "txnc"); - } - - /** - * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTYNC(String limelightName) { - return getLimelightNTDouble(limelightName, "tync"); - } - - /** - * Gets the target area as a percentage of the image (0-100%). - * @param limelightName Name of the Limelight camera ("" for default) - * @return Target area percentage (0-100) - */ - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } - - /** - * T2D is an array that contains several targeting metrcis - * @param limelightName Name of the Limelight camera - * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, - * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] - */ - public static double[] getT2DArray(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "t2d"); - } - - /** - * Gets the number of targets currently detected. - * @param limelightName Name of the Limelight camera - * @return Number of detected targets - */ - public static int getTargetCount(String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[1]; - } - return 0; - } - - /** - * Gets the classifier class index from the currently running neural classifier pipeline - * @param limelightName Name of the Limelight camera - * @return Class index from classifier pipeline - */ - public static int getClassifierClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[10]; - } - return 0; - } - - /** - * Gets the detector class index from the primary result of the currently running neural detector pipeline. - * @param limelightName Name of the Limelight camera - * @return Class index from detector pipeline - */ - public static int getDetectorClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[11]; - } - return 0; - } - - /** - * Gets the current neural classifier result class name. - * @param limelightName Name of the Limelight camera - * @return Class name string from classifier pipeline - */ - public static String getClassifierClass (String limelightName) { - return getLimelightNTString(limelightName, "tcclass"); - } - - /** - * Gets the primary neural detector result class name. - * @param limelightName Name of the Limelight camera - * @return Class name string from detector pipeline - */ - public static String getDetectorClass (String limelightName) { - return getLimelightNTString(limelightName, "tdclass"); - } - - /** - * Gets the pipeline's processing latency contribution. - * @param limelightName Name of the Limelight camera - * @return Pipeline latency in milliseconds - */ - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } - - /** - * Gets the capture latency. - * @param limelightName Name of the Limelight camera - * @return Capture latency in milliseconds - */ - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } - - /** - * Gets the active pipeline index. - * @param limelightName Name of the Limelight camera - * @return Current pipeline index (0-9) - */ - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } - - /** - * Gets the current pipeline type. - * @param limelightName Name of the Limelight camera - * @return Pipeline type string (e.g. "retro", "apriltag", etc) - */ - public static String getCurrentPipelineType(String limelightName) { - return getLimelightNTString(limelightName, "getpipetype"); - } - - /** - * Gets the full JSON results dump. - * @param limelightName Name of the Limelight camera - * @return JSON string containing all current results - */ - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } - - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } - - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } - - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } - - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } - - public static String[] getRawBarcodeData(String limelightName) { - return getLimelightNTStringArray(limelightName, "rawbarcodes"); - } - - ///// - ///// - - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } - - /** - * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Red Alliance field space - */ - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } - - /** - * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space - */ - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } - - /** - * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation relative to the target - */ - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } - - /** - * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the target - */ - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } - - /** - * Gets the target's 3D pose with respect to the camera's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the camera - */ - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } - - /** - * Gets the target's 3D pose with respect to the robot's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the robot - */ - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the camera's 3D pose with respect to the robot's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the robot - */ - public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } - - /** - * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); - } - - /** - * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * Make sure you are calling setRobotOrientation() before calling this method. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { - - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); - - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED - * alliance - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired", false); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED - * alliance - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { - - double[] result = getBotPose(limelightName); - return toPose2D(result); - - } - - /** - * Gets the current IMU data from NetworkTables. - * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. - * Returns all zeros if data is invalid or unavailable. - * - * @param limelightName Name/identifier of the Limelight - * @return IMUData object containing all current IMU data - */ - public static IMUData getIMUData(String limelightName) { - double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); - if (imuData == null || imuData.length < 10) { - return new IMUData(); // Returns object with all zeros - } - return new IMUData(imuData); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** - * Sets LED mode to be controlled by the current pipeline. - * @param limelightName Name of the Limelight camera - */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); - } - - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); - } - - /** - * Enables standard side-by-side stream mode. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - /** - * Enables Picture-in-Picture mode with secondary stream in the corner. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - /** - * Enables Picture-in-Picture mode with primary stream in the corner. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - - /** - * Sets the crop window for the camera. The crop window in the UI must be completely open. - * @param limelightName Name of the Limelight camera - * @param cropXMin Minimum X value (-1 to 1) - * @param cropXMax Maximum X value (-1 to 1) - * @param cropYMin Minimum Y value (-1 to 1) - * @param cropYMax Maximum Y value (-1 to 1) - */ - public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { - double[] entries = new double[4]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - entries[3] = cropYMax; - setLimelightNTDoubleArray(limelightName, "crop", entries); - } - - /** - * Sets 3D offset point for easy 3D targeting. - */ - public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { - double[] entries = new double[3]; - entries[0] = offsetX; - entries[1] = offsetY; - entries[2] = offsetZ; - setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); - } - - /** - * Sets robot orientation values used by MegaTag2 localization algorithm. - * - * @param limelightName Name/identifier of the Limelight - * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC - * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second - * @param pitch (Unnecessary) Robot pitch in degrees - * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second - * @param roll (Unnecessary) Robot roll in degrees - * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second - */ - public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { - SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); - } - - public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { - SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); - } - - private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate, boolean flush) { - - double[] entries = new double[6]; - entries[0] = yaw; - entries[1] = yawRate; - entries[2] = pitch; - entries[3] = pitchRate; - entries[4] = roll; - entries[5] = rollRate; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - if(flush) - { - Flush(); - } - } - - /** - * Configures the IMU mode for MegaTag2 Localization - * - * @param limelightName Name/identifier of the Limelight - * @param mode IMU mode. - */ - public static void SetIMUMode(String limelightName, int mode) { - setLimelightNTDouble(limelightName, "imumode_set", mode); - } - - /** - * Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4) - * - * @param limelightName Name/identifier of the Limelight - * @param alpha Defaults to .001. Higher values will cause the internal IMU to converge onto the assist source more rapidly. - */ - public static void SetIMUAssistAlpha(String limelightName, double alpha) { - setLimelightNTDouble(limelightName, "imuassistalpha_set", alpha); - } - - - /** - * Configures the throttle value. Set to 100-200 while disabled to reduce thermal output/temperature. - * - * @param limelightName Name/identifier of the Limelight - * @param throttle Defaults to 0. Your Limelgiht will process one frame after skipping throttle frames. - */ - public static void SetThrottle(String limelightName, int throttle) { - setLimelightNTDouble(limelightName, "throttle_set", throttle); - } - - /** - * Sets the 3D point-of-interest offset for the current fiducial pipeline. - * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking - * - * @param limelightName Name/identifier of the Limelight - * @param x X offset in meters - * @param y Y offset in meters - * @param z Z offset in meters - */ - public static void SetFidcuial3DOffset(String limelightName, double x, double y, - double z) { - - double[] entries = new double[3]; - entries[0] = x; - entries[1] = y; - entries[2] = z; - setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); - } - - /** - * Overrides the valid AprilTag IDs that will be used for localization. - * Tags not in this list will be ignored for robot pose estimation. - * - * @param limelightName Name/identifier of the Limelight - * @param validIDs Array of valid AprilTag IDs to track - */ - public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { - double[] validIDsDouble = new double[validIDs.length]; - for (int i = 0; i < validIDs.length; i++) { - validIDsDouble[i] = validIDs[i]; - } - setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); - } - - /** - * Sets the downscaling factor for AprilTag detection. - * Increasing downscale can improve performance at the cost of potentially reduced detection range. - * - * @param limelightName Name/identifier of the Limelight - * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. - */ - public static void SetFiducialDownscalingOverride(String limelightName, float downscale) - { - int d = 0; // pipeline - if (downscale == 1.0) - { - d = 1; - } - if (downscale == 1.5) - { - d = 2; - } - if (downscale == 2) - { - d = 3; - } - if (downscale == 3) - { - d = 4; - } - if (downscale == 4) - { - d = 5; - } - setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); - } - - /** - * Sets the camera pose relative to the robot. - * @param limelightName Name of the Limelight camera - * @param forward Forward offset in meters - * @param side Side offset in meters - * @param up Up offset in meters - * @param roll Roll angle in degrees - * @param pitch Pitch angle in degrees - * @param yaw Yaw angle in degrees - */ - public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { - double[] entries = new double[6]; - entries[0] = forward; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - ///// - ///// - - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - ///// - ///// - - /** - * Asynchronously take snapshot. - */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync(() -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); - } - - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && !"".equals(snapshotName)) { - connection.setRequestProperty("snapname", snapshotName); - } - - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } - - /** - * Gets the latest JSON results output and returns a LimelightResults object. - * @param limelightName Name of the Limelight camera - * @return LimelightResults object containing all current target data - */ - public static LimelightResults getLatestResults(String limelightName) { - - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - } - - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - results.error = "lljson error: " + e.getMessage(); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; - } -} \ No newline at end of file diff --git a/src/test/java/org/carlmontrobotics/lib199/DummySparkMaxAnswerTest.txt b/src/test/java/org/carlmontrobotics/lib199/DummySparkMaxAnswerTest.txt new file mode 100644 index 00000000..4084fc99 --- /dev/null +++ b/src/test/java/org/carlmontrobotics/lib199/DummySparkMaxAnswerTest.txt @@ -0,0 +1,61 @@ +package org.carlmontrobotics.lib199; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertNotNull; + +import com.revrobotics.REVLibError; +// import com.revrobotics.CANSparkMax; +// import com.revrobotics.CANSparkBase.IdleMode; +// import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkAnalogSensor.Mode; +import com.revrobotics.SparkLimitSwitch.Type; +// import com.revrobotics.SparkPIDController.AccelStrategy; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import com.revrobotics.spark.SparkLimitSwitch;// +import com.revrobotics.spark.SparkClosedLoopController; + +import org.junit.Test; + +public class DummySparkMaxAnswerTest { + + public SparkMax createMockedSparkMax() { + return Mocks.mock(SparkMax.class, new DummySparkMaxAnswer()); + } + + public static void assertTestResponses(SparkMax spark) { + // Check device id + assertEquals(0, spark.getDeviceId()); + + // Check get and set methods + assertEquals(0, spark.get(), 0.01); + spark.set(0); + + // Check that all REV specific objects are non-null + assertNotNull(spark.getAnalog((Mode) null)); + assertNotNull(spark.getEncoder()); + assertNotNull(spark.getForwardLimitSwitch((Type) null)); + assertNotNull(spark.getClosedLoopController()); + + // Check that all REV specific objects return "null" values + assertEquals(0, spark.getEncoder().getPosition(), 0.01); + assertEquals(0, spark.getEncoder().getVelocity(), 0.01); + assertEquals(IdleMode.kBrake, spark.getIdleMode()); + assertEquals(MotorType.kBrushless, spark.getMotorType()); + assertEquals(AccelStrategy.kTrapezoidal, spark.getClosedLoopController().getSmartMotionAccelStrategy(0)); + assertEquals(REVLibError.kOk, spark.getLastError()); + assertEquals(REVLibError.kOk, spark.getClosedLoopController().setP(0, 0)); + assertEquals(REVLibError.kOk, spark.getEncoder().setAverageDepth(0)); + assertEquals(REVLibError.kOk, spark.getAnalog((Mode) null).setInverted(false)); + } + + @Test + public void testResponses() { + assertTestResponses(createMockedSparkMax()); + } + +} + diff --git a/src/test/java/org/carlmontrobotics/lib199/MotorControllerFactoryTest.java b/src/test/java/org/carlmontrobotics/lib199/MotorControllerFactoryTest.java index 87ccc3ed..c887d01a 100644 --- a/src/test/java/org/carlmontrobotics/lib199/MotorControllerFactoryTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/MotorControllerFactoryTest.java @@ -20,6 +20,7 @@ public class MotorControllerFactoryTest extends ErrStreamTest { public void testCreateNoErrors() throws Exception { // Call close to free PWM ports ((AutoCloseable)MotorControllerFactory.createTalon(0)).close(); + ((AutoCloseable)MotorControllerFactory.createVictor(1)).close(); MotorControllerFactory.createSparkMax(2, MotorConfig.NEO); assertEquals(0, errStream.toByteArray().length); } diff --git a/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java b/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java index e785fb9f..754b0616 100644 --- a/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/MotorErrorsTest.java @@ -10,8 +10,9 @@ import com.ctre.phoenix.ErrorCode; import com.revrobotics.REVLibError; -import com.revrobotics.sim.SparkSimFaultManager; -import com.revrobotics.spark.SparkBase.Faults; +// import com.revrobotics.SparkMax; +//import com.revrobotics.spark.SparkBase.Faults; + import com.revrobotics.spark.SparkMax; import org.carlmontrobotics.lib199.testUtils.ErrStreamTest; @@ -20,7 +21,40 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class MotorErrorsTest extends ErrStreamTest { - public static interface TemperatureSparkMax { + public enum FaultID { + kBrownout(0), kOvercurrent(1), kOvervoltage(2), kMotorFault(3), kSensorFault(4), kStall(5), kEEPROMCRC(6), + kCANTX(7), kCANRX(8), kHasReset(9), kDRVFault(10), kOtherFault(11), kSoftLimitFwd(12), kSoftLimitRev(13), + kHardLimitFwd(14), kHardLimitRev(15); + + @SuppressWarnings("MemberName") + public final int value; + + FaultID(int value) { + this.value = value; + } + } + + public static class SensorFaultSparkMax { + public short getFaults() { + return (short)FaultID.kSensorFault.value; + } + + public boolean getFault(FaultID faultID) { + return faultID == FaultID.kSensorFault; + } + } + + public static class StickySensorFaultSparkMax { + public short getStickyFaults() { + return (short)FaultID.kSensorFault.value; + } + + public boolean getStickyFault(FaultID faultID) { + return faultID == FaultID.kSensorFault; + } + } + + public static interface TemperatureSparkMax { public void setTemperature(double temperature); public int getSmartCurrentLimit(); @@ -112,32 +146,32 @@ public void testOtherErrors() { @Test public void testFaultReporting() { - Faults sensorFault = new Faults(4); - SparkMax sensorFaultSparkMax = MotorControllerFactory.createSparkMax(1, MotorConfig.NEO, MotorControllerFactory.sparkConfig(MotorConfig.NEO)); - SparkSimFaultManager sparkSimFaultManager = new SparkSimFaultManager(sensorFaultSparkMax); - sparkSimFaultManager.setFaults(sensorFault); + SparkMax sensorFaultSparkMax = Mocks.createMock(SparkMax.class, new SensorFaultSparkMax(), false); errStream.reset(); - MotorErrors.checkSparkErrors(sensorFaultSparkMax); + MotorErrors.checkSparkMaxErrors(sensorFaultSparkMax); assertNotEquals(0, errStream.toByteArray().length); errStream.reset(); - MotorErrors.checkSparkErrors(sensorFaultSparkMax); + MotorErrors.checkSparkMaxErrors(sensorFaultSparkMax); assertEquals(0, errStream.toByteArray().length); } @Test public void testStickyFaultReporting() { - Faults sensorFault = new Faults(4); - SparkMax stickySensorFaultSparkMax = MotorControllerFactory.createSparkMax(1, MotorConfig.NEO, MotorControllerFactory.sparkConfig(MotorConfig.NEO)); - SparkSimFaultManager sparkSimFaultManager = new SparkSimFaultManager(stickySensorFaultSparkMax); - sparkSimFaultManager.setStickyFaults(sensorFault); + SparkMax stickySensorFaultSparkMax = Mocks.createMock(SparkMax.class, new StickySensorFaultSparkMax(), false); errStream.reset(); - MotorErrors.checkSparkErrors(stickySensorFaultSparkMax); + MotorErrors.checkSparkMaxErrors(stickySensorFaultSparkMax); assertNotEquals(0, errStream.toByteArray().length); errStream.reset(); - MotorErrors.checkSparkErrors(stickySensorFaultSparkMax); + MotorErrors.checkSparkMaxErrors(stickySensorFaultSparkMax); assertEquals(0, errStream.toByteArray().length); } + //FIXME: do we need this? + // @Test + // public void testDummySparkMax() { + // DummySparkMaxAnswerTest.assertTestResponses(MotorErrors.createDummySparkMax()); + // } + @Test public void testReportSparkMaxTemp() { assertTrue(MotorErrors.kOverheatTripCount > 0); @@ -150,30 +184,30 @@ public void testReportSparkMaxTemp() { private void doTestReportSparkMaxTemp(int id) { TemperatureSparkMax spark = (TemperatureSparkMax)Mocks.createMock(SparkMax.class, new TemperatureSparkMax.Instance(id), TemperatureSparkMax.class); String smartDashboardKey = "Port " + id + " Spark Max Temp"; - MotorErrors.reportSparkTemp((SparkMax)spark, 40); + MotorErrors.reportSparkMaxTemp((SparkMax)spark, 40); spark.setTemperature(20); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkTemp(); + MotorErrors.doReportSparkMaxTemp(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); spark.setTemperature(20); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkTemp(); + MotorErrors.doReportSparkMaxTemp(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); if(MotorErrors.kOverheatTripCount > 1) { spark.setTemperature(51); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkTemp(); + MotorErrors.doReportSparkMaxTemp(); assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); spark.setTemperature(20); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkTemp(); + MotorErrors.doReportSparkMaxTemp(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(50, spark.getSmartCurrentLimit()); } @@ -186,7 +220,7 @@ private void doTestReportSparkMaxTemp(int id) { spark.setTemperature(51); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkTemp(); + MotorErrors.doReportSparkMaxTemp(); assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); } @@ -195,13 +229,13 @@ private void doTestReportSparkMaxTemp(int id) { spark.setTemperature(51); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkTemp(); + MotorErrors.doReportSparkMaxTemp(); assertEquals(51, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(1, spark.getSmartCurrentLimit()); spark.setTemperature(20); spark.setSmartCurrentLimit(50); - MotorErrors.doReportSparkTemp(); + MotorErrors.doReportSparkMaxTemp(); assertEquals(20, SmartDashboard.getNumber(smartDashboardKey, 0), 0.01); assertEquals(1, spark.getSmartCurrentLimit()); diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockVictorSPXTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockVictorSPXTest.java new file mode 100644 index 00000000..b6fdd307 --- /dev/null +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockVictorSPXTest.java @@ -0,0 +1,12 @@ +package org.carlmontrobotics.lib199.sim; + +import com.ctre.phoenix.motorcontrol.can.BaseMotorController; + +public class MockVictorSPXTest extends MockPheonixControllerTest { + + @Override + protected BaseMotorController createController(int portPWM) { + return MockVictorSPX.createMockVictorSPX(portPWM); + } + +}