diff --git a/.roboRIOTeamNumberSetter/roborioteamnumbersetter-window.json b/.roboRIOTeamNumberSetter/roborioteamnumbersetter-window.json new file mode 100644 index 0000000..5c09f01 --- /dev/null +++ b/.roboRIOTeamNumberSetter/roborioteamnumbersetter-window.json @@ -0,0 +1,27 @@ +{ + "MainWindow": { + "GLOBAL": { + "font": "Proggy Dotted", + "fps": "120", + "height": "0", + "maximized": "0", + "style": "0", + "userScale": "2", + "width": "0", + "xpos": "-32000", + "ypos": "-32000" + } + }, + "Window": { + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "Entries": { + "Collapsed": "0", + "Pos": "0,0", + "Size": "46,32" + } + } +} diff --git a/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json new file mode 100644 index 0000000..3c8b709 --- /dev/null +++ b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json @@ -0,0 +1,3 @@ +{ + "TeamNumber": 2767 +} diff --git a/build.gradle b/build.gradle index 93a674b..a985adc 100644 --- a/build.gradle +++ b/build.gradle @@ -3,10 +3,10 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" id "org.jetbrains.kotlin.jvm" version "1.9.0" //1.7.21 - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2025.2.1" } -version = "24.0.1" +version = "25.0.0" sourceCompatibility = JavaVersion.VERSION_17 targetCompatibility = JavaVersion.VERSION_17 diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt index 35a233e..c2bff4e 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt @@ -1,10 +1,11 @@ package org.strykeforce.thirdcoast import com.ctre.phoenix.CANifier -import com.ctre.phoenix.motorcontrol.can.TalonFX import com.ctre.phoenix.motorcontrol.can.TalonSRX import com.ctre.phoenix.sensors.PigeonIMU import com.ctre.phoenix6.hardware.CANcoder +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.hardware.TalonFXS import edu.wpi.first.math.geometry.Translation2d import edu.wpi.first.wpilibj.DigitalOutput import edu.wpi.first.wpilibj.PneumaticsModuleType @@ -14,6 +15,7 @@ import org.jline.reader.LineReader import org.jline.reader.LineReaderBuilder import org.jline.terminal.Terminal import org.jline.terminal.TerminalBuilder +import org.koin.core.qualifier.named import org.koin.dsl.module //import org.koin.dsl.module.module import org.strykeforce.swerve.* @@ -45,12 +47,14 @@ val tctModule = module { single { TalonService(get()) { id -> TalonSRX(id) } } - single { LegacyTalonFxService(get()) { id -> TalonFX(id) } } - single { TalonFxService(get()) { id -> com.ctre.phoenix6.hardware.TalonFX(id)} } single {TalonFxFDService(get()) {id -> com.ctre.phoenix6.hardware.TalonFX(id, "*")} } + single {TalonFxsService(get()) {id -> com.ctre.phoenix6.hardware.TalonFXS(id)} } + + single {TalonFXsFDService(get()) {id -> TalonFXS(id, "*")} } + single { ServoService { id -> Servo(id) } } single { SolenoidService { id -> Solenoid(PneumaticsModuleType.CTREPCM, id) } } @@ -75,8 +79,16 @@ val tctModule = module { val swerveModule = module { - single { SwerveDrive(*getSwerveModules()) } + single (named("V6")){ SwerveDrive(*getSwerveModules()) } + +} + +val fxSwerveModule = module { + single (named("FX")) {SwerveDrive(*getFXSwerveModules())} +} +val fxCANivoreSwerveModule = module { + single (named("FX-CANifier")) {SwerveDrive(*getCANivoreSwerveModules())} } private fun getSwerveModules() = Array(4) { i -> getSwerveModule(i) } @@ -95,6 +107,31 @@ private fun getSwerveModule(i: Int) : V6TalonSwerveModule { .wheelLocationMeters(location).build() } +private fun getFXSwerveModules() = Array(4) {i -> getFXSwerveModule(i)} +private fun getCANivoreSwerveModules() = Array(4) {i -> getCANivoreSwerveModule(i) } +private val fxModuleBuilder = FXSwerveModule.FXBuilder().driveGearRatio(1.0).wheelDiameterInches(1.0).driveMaximumMetersPerSecond(1.0) + +private fun getFXSwerveModule(i:Int): FXSwerveModule { + val location = when(i) { + 0 -> Translation2d(1.0, 1.0) + 1 -> Translation2d(1.0, -1.0) + 2 -> Translation2d(-1.0, 1.0) + else -> Translation2d(-1.0, -1.0) + } + return fxModuleBuilder.azimuthTalon(TalonFXS(i)).driveTalon(TalonFX(i+10)).wheelLocationMeters(location).build() +} + +private fun getCANivoreSwerveModule(i: Int): FXSwerveModule { + val location = when(i) { + 0 -> Translation2d(1.0, 1.0) + 1 -> Translation2d(1.0, -1.0) + 2 -> Translation2d(-1.0, 1.0) + else -> Translation2d(-1.0, -1.0) + } + return fxModuleBuilder.azimuthTalon(TalonFXS(i,"*")).driveTalon(TalonFX(i+10,"*")).wheelLocationMeters(location).build() +} + + /* private fun getWheels(): Array { val azimuthConfig = TalonSRXConfiguration().apply { diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/Robot.kt b/src/main/kotlin/org/strykeforce/thirdcoast/Robot.kt index 7afd58a..e67731a 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/Robot.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/Robot.kt @@ -24,7 +24,7 @@ class Robot : TimedRobot(), KoinComponent { override fun robotInit() { startKoin{ - modules(listOf(tctModule, swerveModule)) + modules(listOf(tctModule, swerveModule, fxSwerveModule, fxCANivoreSwerveModule)) logger(SLF4JLogger()) } val telemetryService: TelemetryService by inject() diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameter.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameter.kt index edd1f77..4f4c45a 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameter.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameter.kt @@ -12,7 +12,8 @@ class CancoderParameter( ): AbstractParameter(command, toml) { enum class Enum { POSITION, - MAG_OFFSET + MAG_OFFSET, + DISCONTINUITY_POINT } companion object { diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt index 67c0def..b3badf6 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/CancoderParameterCommand.kt @@ -2,6 +2,9 @@ package org.strykeforce.thirdcoast.cancoder import com.ctre.phoenix6.configs.CANcoderConfiguration import com.ctre.phoenix6.hardware.CANcoder +import edu.wpi.first.units.Units +import edu.wpi.first.units.Units.Rotations +import edu.wpi.first.units.measure.Angle import mu.KotlinLogging import net.consensys.cava.toml.TomlTable //import org.koin.standalone.inject @@ -38,6 +41,10 @@ class CancoderParameterCommand( MAG_OFFSET -> formatMenu( if(bus == "rio") cancoderService.activeConfiguration.MagnetSensor.MagnetOffset else cancoderFDService.activeConfiguration.MagnetSensor.MagnetOffset) + DISCONTINUITY_POINT -> formatMenu( + if(bus == "rio") cancoderService.activeConfiguration.MagnetSensor.AbsoluteSensorDiscontinuityPoint + else cancoderFDService.activeConfiguration.MagnetSensor.AbsoluteSensorDiscontinuityPoint + ) } } @@ -53,6 +60,12 @@ class CancoderParameterCommand( activeConfig.MagnetSensor.MagnetOffset = value cancoder.configurator.apply(activeConfig) } + DISCONTINUITY_POINT -> configDoubleParam(activeConfig.MagnetSensor.absoluteSensorDiscontinuityPointMeasure.`in`( + Rotations)) { + caNcoder, value -> + activeConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = value + caNcoder.configurator.apply(activeConfig) + } else -> TODO("${param.name} not implemented") } return super.execute() @@ -73,6 +86,22 @@ class CancoderParameterCommand( } else throw IllegalArgumentException() } + private fun configAngleParameter(default: Angle, config: (CANcoder, Angle) -> Unit) { + val paramValue = param.readDouble(reader, default.`in`(Units.Rotations)) + if(bus == "rio") { + cancoderService.active.forEach { + config(it, Rotations.of(paramValue)) + logger.info { "set ${cancoderService.active.size} CANcoder's ${param.name}: $paramValue" } + } + } else if(bus == "canivore") { + cancoderFDService.active.forEach { + config(it, Rotations.of(paramValue)) + logger.info { "set ${cancoderFDService.active.size} CANcoder's ${param.name}: $paramValue" } + } + } else throw IllegalArgumentException() + } + + private fun configIntParameter(default: Int, config: (CANcoder, Int) -> Unit) { val paramValue = param.readInt(reader, default) if(bus == "rio") { diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt deleted file mode 100644 index c4bc13c..0000000 --- a/src/main/kotlin/org/strykeforce/thirdcoast/cancoder/SelectAbsRangeValueCommand.kt +++ /dev/null @@ -1,52 +0,0 @@ -package org.strykeforce.thirdcoast.cancoder - -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue.* -import net.consensys.cava.toml.TomlTable -import org.koin.core.component.inject -import org.koin.java.KoinJavaComponent.inject -//import org.koin.standalone.inject -import org.strykeforce.thirdcoast.command.AbstractSelectCommand -import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.CancoderFDService -import org.strykeforce.thirdcoast.device.CancoderService - -private val RANGE_VALUE = listOf( - Unsigned_0To1, - Signed_PlusMinusHalf -) - -private val LABELS = listOf( - "0 to 1", - "-0.5 to 0.5" -) - -class SelectAbsRangeValueCommand( - parent: Command?, - key: String, - toml: TomlTable -): AbstractSelectCommand(parent, key, toml, RANGE_VALUE, LABELS) { - private val cancoderService: CancoderService by inject() - private val cancoderFDService: CancoderFDService by inject() - - val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") - - override val activeIndex: Int - get() { - if(bus == "rio") return cancoderService.activeConfiguration.MagnetSensor.AbsoluteSensorRange.ordinal - else if(bus == "canivore") return cancoderFDService.activeConfiguration.MagnetSensor.AbsoluteSensorRange.ordinal - else throw IllegalArgumentException() - - } - - override fun setActive(index: Int) { - val range = values[index] - if(bus == "rio") { - cancoderService.activeConfiguration.MagnetSensor.AbsoluteSensorRange = range - cancoderService.active.forEach{it.configurator.apply(cancoderService.activeConfiguration)} - } else if(bus == "canivore") { - cancoderFDService.activeConfiguration.MagnetSensor.AbsoluteSensorRange = range - cancoderFDService.active.forEach { it.configurator.apply(cancoderFDService.activeConfiguration.MagnetSensor) } - } - } -} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt index 9ea8bca..7b25847 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt @@ -18,10 +18,7 @@ import org.strykeforce.thirdcoast.servo.RunServosCommand import org.strykeforce.thirdcoast.servo.SelectServosCommand import org.strykeforce.thirdcoast.solenoid.RunSolenoidsCommand import org.strykeforce.thirdcoast.solenoid.SelectSolenoidsCommand -import org.strykeforce.thirdcoast.swerve.AdjustAzimuthCommand -import org.strykeforce.thirdcoast.swerve.SaveZeroCommand -import org.strykeforce.thirdcoast.swerve.SelectAzimuthCommand -import org.strykeforce.thirdcoast.swerve.SetAzimuthCommand +import org.strykeforce.thirdcoast.swerve.* import org.strykeforce.thirdcoast.talon.* import org.strykeforce.thirdcoast.talon.phoenix6.* @@ -66,9 +63,6 @@ interface Command { "talon.hard.source" -> SelectHardLimitSourceCommand(parent, key, toml) "talon.hard.normal" -> SelectHardLimitNormalCommand(parent, key, toml) "talon.velocity.period" -> SelectVelocityMeasurmentPeriodCommand(parent, key, toml) - "talon.commutation" -> SelectMotorCommutationCommand(parent, key, toml) - "talon.absoluteRange" -> SelectAbsoluteSensorRange(parent, key, toml) - "talon.initStrategy" -> SelectInitializationStrategy(parent, key, toml) "servo.select" -> SelectServosCommand(parent, key, toml) "servo.run" -> RunServosCommand(parent, key, toml) "solenoid.select" -> SelectSolenoidsCommand(parent, key, toml) @@ -86,6 +80,10 @@ interface Command { "swerve.azimuth.save" -> SaveZeroCommand(parent, key, toml) "swerve.azimuth.select" -> SelectAzimuthCommand(parent, key, toml) "swerve.azimuth.adjust" -> AdjustAzimuthCommand(parent, key, toml) + "p6.swerve.azimuth" -> P6SetAzimuthCommand(parent, key, toml) + "p6.swerve.azimuth.save" -> P6SaveZeroCommand(parent, key, toml) + "p6.swerve.azimuth.select" -> P6SelectAzimuthCommand(parent, key, toml) + "p6.swerve.azimuth.adjust" -> P6AdjustAzimuthCommand(parent, key, toml) "pigeon.select" -> SelectPigeonCommand(parent, key, toml) "pigeon.param" -> PigeonParameterCommand(parent, key, toml) "p6.run" -> P6RunCommand(parent, key, toml) @@ -98,6 +96,7 @@ interface Command { "p6.feedback" -> P6SelectFeedbackSensorCommand(parent, key, toml) "p6.mmType" -> P6SelectMotionMagicTypeCommand(parent, key, toml) "p6.diffType" -> P6SelectDifferentialTypeCommand(parent, key, toml) + "p6.diffSource" -> P6SelectDifferentialSensorSourceCommand(parent, key, toml) "p6.followType" -> P6SelectFollowerTypeCommand(parent, key, toml) "p6.neutralOut" -> P6SelectNeutralOutputCommand(parent, key, toml) "p6.units" -> P6SelectUnitCommand(parent, key, toml) @@ -111,10 +110,14 @@ interface Command { "p6.revSource" -> P6SelectRevHardLimitSourceCommand(parent, key, toml) "p6.factory" -> P6FactoryDefaultCommand(parent, key, toml) "p6.graph" -> P6DefaultStatusFrameCommand(parent, key, toml) + "p6.extFeedback" -> P6SelectExternalFeedbackSourceCommand(parent, key, toml) + "p6.phase" -> P6SelectSensorPhaseCommand(parent, key, toml) + "p6.motorArrange" -> P6SelectMotorArrangementCommand(parent, key, toml) + "p6.wiring" -> P6SelectBrushedWiringCommand(parent, key, toml) + "p6.advHall" -> P6SelectAdvancedHallSupportCommand(parent, key, toml) "cancoder.select" -> SelectCancoderCommand(parent, key, toml) "cancoder.status" -> CancoderStatusCommand(parent, key, toml) "cancoder.param" -> CancoderParameterCommand(parent, key, toml) - "cancoder.absRange" -> SelectAbsRangeValueCommand(parent, key, toml) "cancoder.sensorDirection" -> SelectCancoderSensorDirectionCommand(parent, key, toml) else -> DefaultCommand(parent, key, toml) } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/LegacyTalonFxService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/LegacyTalonFxService.kt deleted file mode 100644 index e5ae51d..0000000 --- a/src/main/kotlin/org/strykeforce/thirdcoast/device/LegacyTalonFxService.kt +++ /dev/null @@ -1,71 +0,0 @@ -package org.strykeforce.thirdcoast.device - -import com.ctre.phoenix.motorcontrol.ControlMode -import com.ctre.phoenix.motorcontrol.NeutralMode -import com.ctre.phoenix.motorcontrol.can.SlotConfiguration -import com.ctre.phoenix.motorcontrol.can.TalonFX -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration -import mu.KotlinLogging -import org.strykeforce.telemetry.TelemetryService - -private val logger = KotlinLogging.logger {} - -private val CONTROL_MODE_DEFAULT = ControlMode.PercentOutput -private const val ACTIVE_SLOT_DEFAULT = 0 -private val NEUTRAL_MODE_DEFAULT = NeutralMode.EEPROMSetting -private const val VOLTAGE_COMPENSATION_ENABLED_DEFAULT = true -private const val CURRENT_LIMIT_ENABLED_DEFAULT = false -private const val SENSOR_PHASE_INVERTED_DEFAULT = false -private const val OUTPUT_INVERTED_DEFAULT = false - - -class LegacyTalonFxService( - private val telemetryService: TelemetryService, factory: (id: Int) -> TalonFX -) : AbstractDeviceService(factory) { - - val timeout = 10 - var dirty = true - var neutralMode = NEUTRAL_MODE_DEFAULT - var controlMode = CONTROL_MODE_DEFAULT - var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT - var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT - var activeSlotIndex: Int = ACTIVE_SLOT_DEFAULT - - var activeConfiguration = TalonFXConfiguration() - get() { - if (!dirty) return field - active.firstOrNull()?.getAllConfigs(field, timeout) - ?: logger.debug("no active talon fx's, returning default config") - dirty = false - return field - } - - val activeSlot: SlotConfiguration - get() = when (activeSlotIndex) { - 0 -> activeConfiguration.slot0 - 1 -> activeConfiguration.slot1 - 2 -> activeConfiguration.slot2 - 3 -> activeConfiguration.slot3 - else -> throw IllegalStateException("invalid slot: $activeSlotIndex") - } - - val outputInverted: Boolean - get() = active.firstOrNull()?.inverted ?: OUTPUT_INVERTED_DEFAULT - - override fun activate(ids: Collection): Set { - dirty = true - - val new = super.activate(ids) - telemetryService.stop() - active.filter { new.contains(it.deviceID) }.forEach { - it.setNeutralMode(neutralMode) - it.selectProfileSlot(activeSlotIndex, 0) - it.enableVoltageCompensation(voltageCompensation) - it.setSensorPhase(sensorPhase) - it.inverted = OUTPUT_INVERTED_DEFAULT - telemetryService.register(it) - } - telemetryService.start() - return new - } -} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFXsFDService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFXsFDService.kt new file mode 100644 index 0000000..a680070 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFXsFDService.kt @@ -0,0 +1,174 @@ +package org.strykeforce.thirdcoast.device + +import com.ctre.phoenix6.configs.TalonFXSConfiguration +import com.ctre.phoenix6.hardware.TalonFXS +import com.ctre.phoenix6.signals.NeutralModeValue +import mu.KotlinLogging +import org.strykeforce.telemetry.TelemetryService + +private const val ACTIVE_SLOT_DEFAULT = 0 +private val logger = KotlinLogging.logger {} +class TalonFXsFDService( + private val telemetryService: TelemetryService, + factory: (id:Int) -> TalonFXS +): AbstractDeviceService(factory) { + + val timeout = 10.0 + var dirty = true + var activeSlotIndex: Int = ACTIVE_SLOT_DEFAULT + var activeUnits: Units = Units.PERCENT + var active_MM_type: MM_Type = MM_Type.STANDARD + var activeFeedForward: Double = 0.0 + var activeTorqueCurrentDeadband: Double = 0.0 + var activeTorqueCurrentMaxOut: Double = 1.0 + var activeVelocity: Double = 0.0 + var activeAcceleration: Double = 0.0 + var activeJerk: Double = 0.0 + var activeDifferentialTarget: Double = 0.0 + var activeFollowerType: FollowerType = FollowerType.STRICT + var activeOpposeMain: Boolean = false + var activeDifferentialSlot: Int = 0 + var activeFOC: Boolean = false + var setpointType: SetpointType = SetpointType.OPEN_LOOP + var differentialType: DifferentialType = DifferentialType.OPEN_LOOP + var activeNeutralOut: NeutralModeValue = NeutralModeValue.Coast + var activeOverrideNeutral: Boolean = false + var limFwdMotion: Boolean = false; + var limRevMotion: Boolean = false; + var grapherStatusFrameHz: Double = 20.0; + + var controlMode: String = "Duty Cycle Out" + get() { + when(setpointType){ + SetpointType.OPEN_LOOP -> { + when(activeUnits){ + Units.PERCENT -> return "Duty Cycle Out" + Units.VOLTAGE -> return "Voltage Out" + Units.TORQUE_CURRENT -> return "(pro) Torque Current FOC" + } + } + SetpointType.POSITION -> { + when(activeUnits){ + Units.PERCENT -> return "Position: Duty Cycle" + Units.VOLTAGE -> return "Position: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Position: Torque Current FOC" + } + } + SetpointType.VELOCITY -> { + when(activeUnits) { + Units.PERCENT -> return "Velocity: Duty Cycle" + Units.VOLTAGE -> return "Velocity: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Velocity: Torque Current FOC" + } + } + SetpointType.MOTION_MAGIC -> { + when(active_MM_type) { + MM_Type.STANDARD -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic: Torque Current FOC" + } + } MM_Type.VELOCITY -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic Velocity: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic Velocity: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic Velocity: Torque Current FOC" + } + } MM_Type.DYNAMIC -> { + when(activeUnits) { + Units.PERCENT -> return "Dynamic Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Dynamic Motion Magic: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Dynamic Motion Magic: Torque Current FOC" + } + } MM_Type.EXPONENTIAL -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic Exponential: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic Exponential: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic Exponential: Torque Current FOC" + } + } + } + } + SetpointType.DIFFERENTIAL -> { + when(differentialType) { + DifferentialType.OPEN_LOOP -> { + when(activeUnits) { + Units.PERCENT -> return "Differential: Duty Cycle" + Units.VOLTAGE -> return "Differential: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.POSITION -> { + when(activeUnits) { + Units.PERCENT -> return "Differential Position: Duty Cycle" + Units.VOLTAGE -> return "Differential Position: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.VELOCITY -> { + when(activeUnits){ + Units.PERCENT -> return "Differential Velocity: Duty Cycle" + Units.VOLTAGE -> return "Differential Velocity: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.MOTION_MAGIC -> { + when(activeUnits) { + Units.PERCENT -> return "Differential Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Differential Motion Magic: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.FOLLOWER -> { + when(activeFollowerType){ + FollowerType.STANDARD -> return "Differential Follower: Non-Strict" + FollowerType.STRICT -> return "Differential Follower: Strict" + } + } + } + } + SetpointType.FOLLOWER -> { + when(activeFollowerType){ + FollowerType.STANDARD -> return "Follower: Non-Strict" + FollowerType.STRICT -> return "Follower: Strict" + } + } + SetpointType.NEUTRAL -> { + when(activeNeutralOut){ + NeutralModeValue.Brake -> return "Brake Mode" + NeutralModeValue.Coast -> return "Coast Mode" + } + } + SetpointType.MUSIC -> { + return "Music Tone" + + } + } + } + + var activeConfiguration = TalonFXSConfiguration() + get() { + if(!dirty) return field + active.firstOrNull()?.configurator?.refresh(field) + ?:logger.debug("no active talon fxs's, returning default config") + dirty = false + return field + } + + override fun activate(ids: Collection): Set { + dirty = true + logger.info { "Number Active: ${active.size}" } + active.forEach { + logger.info { "Active TalonFXS: ${it.deviceID}" } + } + + val new = super.activate(ids) + logger.info { "Number New: ${new.size}" } + telemetryService.stop() + active.filter { new.contains(it.deviceID) }.forEach{ + logger.info { "New TalonFXS: ${it.deviceID}" } + activeSlotIndex = it.closedLoopSlot.value + telemetryService.register(it, true) + } + telemetryService.start() + active.firstOrNull() + return new + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxsService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxsService.kt new file mode 100644 index 0000000..0549473 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxsService.kt @@ -0,0 +1,174 @@ +package org.strykeforce.thirdcoast.device + +import com.ctre.phoenix6.configs.TalonFXSConfiguration +import com.ctre.phoenix6.hardware.TalonFXS +import com.ctre.phoenix6.signals.NeutralModeValue +import mu.KotlinLogging +import org.strykeforce.telemetry.TelemetryService + +private const val ACTIVE_SLOT_DEFAULT = 0 +private val logger = KotlinLogging.logger {} +class TalonFxsService( + private val telemetryService: TelemetryService, factory: (id:Int) -> TalonFXS +):AbstractDeviceService(factory) { + + val timeout = 10.0 + var dirty = true + var activeSlotIndex: Int = ACTIVE_SLOT_DEFAULT + var activeUnits: Units = Units.PERCENT + var active_MM_type: MM_Type = MM_Type.STANDARD + var activeFeedForward: Double = 0.0 + var activeTorqueCurrentDeadband: Double = 0.0 + var activeTorqueCurrentMaxOut: Double = 1.0 + var activeVelocity: Double = 0.0 + var activeAcceleration: Double = 0.0 + var activeJerk: Double = 0.0 + var activeDifferentialTarget: Double = 0.0 + var activeFollowerType: FollowerType = FollowerType.STRICT + var activeOpposeMain: Boolean = false + var activeDifferentialSlot: Int = 0 + var activeFOC: Boolean = false + var setpointType: SetpointType = SetpointType.OPEN_LOOP + var differentialType: DifferentialType = DifferentialType.OPEN_LOOP + var activeNeutralOut: NeutralModeValue = NeutralModeValue.Coast + var activeOverrideNeutral: Boolean = false + var limFwdMotion: Boolean = false; + var limRevMotion: Boolean = false; + var grapherStatusFrameHz: Double = 20.0; + + var controlMode: String = "Duty Cycle Out" + get() { + when(setpointType){ + SetpointType.OPEN_LOOP -> { + when(activeUnits){ + Units.PERCENT -> return "Duty Cycle Out" + Units.VOLTAGE -> return "Voltage Out" + Units.TORQUE_CURRENT -> return "(pro) Torque Current FOC" + } + } + SetpointType.POSITION -> { + when(activeUnits){ + Units.PERCENT -> return "Position: Duty Cycle" + Units.VOLTAGE -> return "Position: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Position: Torque Current FOC" + } + } + SetpointType.VELOCITY -> { + when(activeUnits) { + Units.PERCENT -> return "Velocity: Duty Cycle" + Units.VOLTAGE -> return "Velocity: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Velocity: Torque Current FOC" + } + } + SetpointType.MOTION_MAGIC -> { + when(active_MM_type) { + MM_Type.STANDARD -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic: Torque Current FOC" + } + } MM_Type.VELOCITY -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic Velocity: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic Velocity: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic Velocity: Torque Current FOC" + } + } MM_Type.DYNAMIC -> { + when(activeUnits) { + Units.PERCENT -> return "Dynamic Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Dynamic Motion Magic: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Dynamic Motion Magic: Torque Current FOC" + } + } MM_Type.EXPONENTIAL -> { + when(activeUnits) { + Units.PERCENT -> return "Motion Magic Exponential: Duty Cycle" + Units.VOLTAGE -> return "Motion Magic Exponential: Voltage" + Units.TORQUE_CURRENT -> return "(pro) Motion Magic Exponential: Torque Current FOC" + } + } + } + } + SetpointType.DIFFERENTIAL -> { + when(differentialType) { + DifferentialType.OPEN_LOOP -> { + when(activeUnits) { + Units.PERCENT -> return "Differential: Duty Cycle" + Units.VOLTAGE -> return "Differential: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.POSITION -> { + when(activeUnits) { + Units.PERCENT -> return "Differential Position: Duty Cycle" + Units.VOLTAGE -> return "Differential Position: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.VELOCITY -> { + when(activeUnits){ + Units.PERCENT -> return "Differential Velocity: Duty Cycle" + Units.VOLTAGE -> return "Differential Velocity: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.MOTION_MAGIC -> { + when(activeUnits) { + Units.PERCENT -> return "Differential Motion Magic: Duty Cycle" + Units.VOLTAGE -> return "Differential Motion Magic: Voltage" + else -> return "INVALID COMBO: No Differenital Torque Current" + } + } DifferentialType.FOLLOWER -> { + when(activeFollowerType){ + FollowerType.STANDARD -> return "Differential Follower: Non-Strict" + FollowerType.STRICT -> return "Differential Follower: Strict" + } + } + } + } + SetpointType.FOLLOWER -> { + when(activeFollowerType){ + FollowerType.STANDARD -> return "Follower: Non-Strict" + FollowerType.STRICT -> return "Follower: Strict" + } + } + SetpointType.NEUTRAL -> { + when(activeNeutralOut){ + NeutralModeValue.Brake -> return "Brake Mode" + NeutralModeValue.Coast -> return "Coast Mode" + } + } + SetpointType.MUSIC -> { + return "Music Tone" + + } + } + } + + var activeConfiguration = TalonFXSConfiguration() + get() { + if(!dirty) return field + active.firstOrNull()?.configurator?.refresh(field) + ?:logger.debug("no active talon fxs's, returning default config") + dirty = false + return field + } + + override fun activate(ids: Collection): Set { + dirty = true + logger.info { "Number Active: ${active.size}" } + active.forEach { + logger.info { "Active TalonFXS: ${it.deviceID}" } + } + + val new = super.activate(ids) + logger.info { "Number New: ${new.size}" } + telemetryService.stop() + active.filter { new.contains(it.deviceID) }.forEach{ + logger.info { "New TalonFXS: ${it.deviceID}" } + activeSlotIndex = it.closedLoopSlot.value + telemetryService.register(it, true) + } + telemetryService.start() + active.firstOrNull() + return new + } + +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/AdjustAzimuthCommands.kt b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/AdjustAzimuthCommands.kt index a2d578c..a02e696 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/AdjustAzimuthCommands.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/AdjustAzimuthCommands.kt @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.Timer import mu.KotlinLogging import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject +import org.koin.core.qualifier.named import org.strykeforce.swerve.SwerveDrive import org.strykeforce.swerve.V6TalonSwerveModule import org.strykeforce.thirdcoast.command.AbstractCommand @@ -50,7 +51,7 @@ class AdjustAzimuthCommand( toml: TomlTable ) : AbstractCommand(parent, key, toml) { - private val swerve: SwerveDrive by inject() + private val swerve: SwerveDrive by inject((named("V6"))) override val menu: String get() = formatMenu( diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/P6AdjustAzimuthCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/P6AdjustAzimuthCommand.kt new file mode 100644 index 0000000..9e69ea9 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/P6AdjustAzimuthCommand.kt @@ -0,0 +1,94 @@ +package org.strykeforce.thirdcoast.swerve + +import com.ctre.phoenix6.controls.MotionMagicVoltage +import edu.wpi.first.wpilibj.Timer +import mu.KotlinLogging +import net.consensys.cava.toml.TomlTable +import org.strykeforce.swerve.FXSwerveModule +import org.strykeforce.swerve.SwerveDrive +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.command.prompt +import org.strykeforce.thirdcoast.readDouble +import org.strykeforce.thirdcoast.readInt +import org.strykeforce.thirdcoast.warn +import kotlin.math.abs +import org.koin.core.component.inject +import org.koin.core.qualifier.named + + +private val logger = KotlinLogging.logger{} + +class P6SelectAzimuthCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + override val menu: String + get() = formatMenu(P6AdjustAzimuthCommand.active) + + override fun execute(): Command { + while(true) { + try { + val azimuth = reader.readInt(prompt(), default = P6AdjustAzimuthCommand.active) + if(!(0..3).contains(azimuth)) throw IllegalArgumentException() + P6AdjustAzimuthCommand.active = azimuth + return super.execute() + } catch (e: Exception) { + terminal.warn("Please eneter an integer") + } + } + } +} + +private const val GOOD_ENOUGH = 10.0/4096.0 +private const val JOG = 250.0/4096.0 +private const val DELAY = 5.0/1000.0 +class P6AdjustAzimuthCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractCommand(parent, key, toml) { + private val swerve: SwerveDrive by inject(named("FX")) + private val canifierSwerve: SwerveDrive by inject(named("FX-CANifier")) + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + + override val menu: String + get() { + if(bus=="rio") return formatMenu( + (swerve.swerveModules[active] as FXSwerveModule).azimuthTalon.getPosition().valueAsDouble) + else return formatMenu((canifierSwerve.swerveModules[active] as FXSwerveModule).azimuthTalon.getPosition().valueAsDouble) + } + + override fun execute(): Command { + val swerveModule = + if(bus== "rio") swerve.swerveModules[active] as FXSwerveModule + else canifierSwerve.swerveModules[active] as FXSwerveModule + var position = swerveModule.azimuthTalon.getPosition().valueAsDouble + while(true) { + try { + position = reader.readDouble(prompt(), position) + swerveModule.jogAround(position.toDouble()) + logger.info { "positioned wheel $active to $position" } + return super.execute() + } catch (e: java.lang.IllegalArgumentException) { + terminal.warn("Please enter a Double") + } + } + } + companion object { + var active: Int = 0 + } +} + +private fun FXSwerveModule.jogAround(position: Double) { + val positions = listOf(position - JOG, position + JOG, position) + positions.forEach { + this.azimuthTalon.setControl(MotionMagicVoltage(it)) + while (!this.onTarget(it)) Timer.delay(DELAY) + } +} + +internal fun FXSwerveModule.onTarget(target: Double, goodEnough: Double = GOOD_ENOUGH) = + abs((target) - (this.azimuthTalon.getPosition().valueAsDouble)) < goodEnough \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/P6SaveZeroCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/P6SaveZeroCommand.kt new file mode 100644 index 0000000..0173783 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/P6SaveZeroCommand.kt @@ -0,0 +1,53 @@ +package org.strykeforce.thirdcoast.swerve + +import mu.KotlinLogging +import net.consensys.cava.toml.TomlTable +import org.strykeforce.swerve.FXSwerveModule +import org.strykeforce.swerve.SwerveDrive +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.command.prompt +import org.strykeforce.thirdcoast.info +import org.strykeforce.thirdcoast.readBoolean +import org.strykeforce.thirdcoast.warn +import java.lang.IllegalArgumentException +import org.koin.core.component.inject +import org.koin.core.qualifier.named + +private val logger = KotlinLogging.logger { } +class P6SaveZeroCommand( + parent: Command?, key: String, toml: TomlTable +): AbstractCommand(parent, key, toml) { + val swerve: SwerveDrive by inject(named("FX")) + val canifierSwerve: SwerveDrive by inject(named("FX-CANifier")) + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + + override fun execute(): Command { + while (true) { + try { + if(reader.readBoolean(prompt(), false)) { + if(bus=="rio") { + swerve.swerveModules.forEach { + val module = it as FXSwerveModule + logger.debug { "azimuth ${module.azimuthTalon.deviceID}: store zero, before=${module.azimuthTalon.position.valueAsDouble}" } + module.storeAzimuthZeroReference() + logger.debug { "azimuth ${module.azimuthTalon.deviceID} store zero, after=${module.azimuthTalon.position.valueAsDouble}" } + } + } else { + canifierSwerve.swerveModules.forEach { + val module = it as FXSwerveModule + logger.debug { "azimuth ${module.azimuthTalon.deviceID}: store zero, before=${module.azimuthTalon.position.valueAsDouble}" } + module.storeAzimuthZeroReference() + logger.debug { "azimuth ${module.azimuthTalon.deviceID} store zero, after=${module.azimuthTalon.position.valueAsDouble}" } + } + } + + terminal.info("azimuth zero positions were saved") + } + return super.execute() + } catch (e: IllegalArgumentException) { + terminal.warn("please enter a 'y' or 'n") + } + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/P6SetAzimuthCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/P6SetAzimuthCommand.kt new file mode 100644 index 0000000..03d4f05 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/P6SetAzimuthCommand.kt @@ -0,0 +1,85 @@ +package org.strykeforce.thirdcoast.swerve + +import com.ctre.phoenix6.controls.MotionMagicVoltage +import edu.wpi.first.wpilibj.Preferences +import edu.wpi.first.wpilibj.Timer +import mu.KotlinLogging +import net.consensys.cava.toml.TomlTable +import org.strykeforce.swerve.FXSwerveModule +import org.strykeforce.swerve.SwerveDrive +import org.strykeforce.thirdcoast.command.AbstractCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.command.prompt +import org.strykeforce.thirdcoast.readDouble +import org.strykeforce.thirdcoast.warn +import org.koin.core.component.inject +import org.koin.core.qualifier.named + +private const val DELAY = 20.0/1000.0 +private const val WHEEL_PREF_KEY = "SwerveDrive/wheel.0" + +private val logger = KotlinLogging.logger { } +class P6SetAzimuthCommand( + parent: Command?, key: String, toml: TomlTable +): AbstractCommand(parent, key, toml) { + val swerve: SwerveDrive by inject(named("FX")) + val canifierSwerve: SwerveDrive by inject(named("FX-CANifier")) + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + + override val menu: String + get() { + if (bus == "rio") return formatMenu(swerve.swerveModules.map { + (it as FXSwerveModule).azimuthTalon.getPosition().valueAsDouble + }.joinToString()) + else return formatMenu(canifierSwerve.swerveModules.map { + (it as FXSwerveModule).azimuthTalon.getPosition().valueAsDouble + }.joinToString()) + } + + override fun execute(): Command { + if(!wheelZeroSaved()) { + terminal.warn("swerve drive wheels are not zeroed") + return super.execute() + } + + if(bus == "rio") swerve.swerveModules.forEach { it.loadAndSetAzimuthZeroReference() } + else canifierSwerve.swerveModules.forEach { it.loadAndSetAzimuthZeroReference() } + + while (true) { + try { + val setpoint = reader.readDouble(prompt()) + if(bus=="rio") { + swerve.swerveModules.forEach { + (it as FXSwerveModule).azimuthTalon.setControl(MotionMagicVoltage(setpoint)) + } + val swerveModule = swerve.swerveModules[0] as FXSwerveModule + while (!swerveModule.onTarget(setpoint)) Timer.delay(DELAY) + Timer.delay(5* DELAY) + logger.info { + swerve.swerveModules.map { + (it as FXSwerveModule).azimuthTalon.getPosition().valueAsDouble + }.joinToString() + } + } else { + canifierSwerve.swerveModules.forEach { + (it as FXSwerveModule).azimuthTalon.setControl(MotionMagicVoltage(setpoint)) + } + val swerveModule = canifierSwerve.swerveModules[0] as FXSwerveModule + while (!swerveModule.onTarget(setpoint)) Timer.delay(DELAY) + Timer.delay(5* DELAY) + logger.info { + canifierSwerve.swerveModules.map { + (it as FXSwerveModule).azimuthTalon.getPosition().valueAsDouble + }.joinToString() + } + } + + return super.execute() + } catch (e: Exception) { + terminal.warn("Please enter a double") + } + } + } + + private fun wheelZeroSaved() = Preferences.containsKey(WHEEL_PREF_KEY) +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SaveZeroCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SaveZeroCommand.kt index ce58d63..3d19661 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SaveZeroCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SaveZeroCommand.kt @@ -3,6 +3,7 @@ package org.strykeforce.thirdcoast.swerve import mu.KotlinLogging import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject +import org.koin.core.qualifier.named import org.strykeforce.swerve.SwerveDrive import org.strykeforce.swerve.V6TalonSwerveModule import org.strykeforce.thirdcoast.command.AbstractCommand @@ -18,7 +19,7 @@ class SaveZeroCommand( parent: Command?, key: String, toml: TomlTable ) : AbstractCommand(parent, key, toml) { - val swerve: SwerveDrive by inject() + val swerve: SwerveDrive by inject(named("V6")) override fun execute(): Command { while (true) { diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SetAzimuthCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SetAzimuthCommand.kt index cd19382..c7063bc 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SetAzimuthCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/swerve/SetAzimuthCommand.kt @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.Timer import mu.KotlinLogging import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject +import org.koin.core.qualifier.named import org.strykeforce.swerve.SwerveDrive import org.strykeforce.swerve.V6TalonSwerveModule import org.strykeforce.thirdcoast.command.AbstractCommand @@ -25,7 +26,8 @@ class SetAzimuthCommand( toml: TomlTable ) : AbstractCommand(parent, key, toml) { - val swerve: SwerveDrive by inject() + val swerve: SwerveDrive by inject(named("V6")) + override val menu: String get() = formatMenu(swerve.swerveModules.map { diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/FeedbackCoefficientCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/FeedbackCoefficientCommand.kt index 2ea4547..47573e0 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/FeedbackCoefficientCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/FeedbackCoefficientCommand.kt @@ -6,7 +6,6 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService private val logger = KotlinLogging.logger {} @@ -18,7 +17,6 @@ class FeedbackCoefficientCommand( ) : AbstractCommand(parent, key, toml) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val timeout = talonService.timeout @@ -29,23 +27,17 @@ class FeedbackCoefficientCommand( get() = formatMenu( when (pidIndex) { 0 -> { - if(type == "srx") talonService.activeConfiguration.primaryPID.selectedFeedbackCoefficient - else if(type == "fx") legacyTalonFxService.activeConfiguration.primaryPID.selectedFeedbackCoefficient - else throw IllegalArgumentException() + talonService.activeConfiguration.primaryPID.selectedFeedbackCoefficient } else -> { - if(type == "srx") talonService.activeConfiguration.auxiliaryPID.selectedFeedbackCoefficient - else if(type == "fx") legacyTalonFxService.activeConfiguration.auxiliaryPID.selectedFeedbackCoefficient - else throw IllegalArgumentException() + talonService.activeConfiguration.auxiliaryPID.selectedFeedbackCoefficient } } ) override fun execute(): Command { var config: BaseTalonConfiguration - if(type == "srx") config = talonService.activeConfiguration - else if(type == "fx") config = legacyTalonFxService.activeConfiguration - else throw IllegalArgumentException() + config = talonService.activeConfiguration val default = when (pidIndex) { 0 -> config.primaryPID.selectedFeedbackCoefficient @@ -53,8 +45,7 @@ class FeedbackCoefficientCommand( } val paramValue = param.readDouble(reader, default) - if(type == "srx") talonService.active.forEach { it.configSelectedFeedbackCoefficient(paramValue, pidIndex, talonService.timeout) } - else if(type == "fx") legacyTalonFxService.active.forEach { it.configSelectedFeedbackCoefficient(paramValue, pidIndex, legacyTalonFxService.timeout) } + talonService.active.forEach { it.configSelectedFeedbackCoefficient(paramValue, pidIndex, talonService.timeout) } logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } return super.execute() diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/HardLimitCommands.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/HardLimitCommands.kt index 16c605e..c475eb9 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/HardLimitCommands.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/HardLimitCommands.kt @@ -12,7 +12,6 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService private val logger = KotlinLogging.logger {} @@ -30,7 +29,6 @@ class SelectHardLimitSourceCommand( ) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") val isForward = toml.getBoolean("forward") ?: true @@ -41,11 +39,7 @@ class SelectHardLimitSourceCommand( override val activeIndex: Int get() { - var config: BaseTalonConfiguration = when (type) { - "srx" -> talonService.activeConfiguration - "fx" -> legacyTalonFxService.activeConfiguration - else -> throw IllegalArgumentException() - } + var config: BaseTalonConfiguration = talonService.activeConfiguration return values.indexOf( if (isForward) config.forwardLimitSwitchSource @@ -57,15 +51,10 @@ class SelectHardLimitSourceCommand( override fun setActive(index: Int) { var active: Set var config: BaseTalonConfiguration - if(type == "srx") { - active = talonService.active - config = talonService.activeConfiguration - } - else if(type == "fx") { - active = legacyTalonFxService.active - config = legacyTalonFxService.activeConfiguration - } - else throw IllegalArgumentException() + + active = talonService.active + config = talonService.activeConfiguration + val source = values[index] val normal = getNormal() if (isForward) { @@ -102,7 +91,6 @@ class SelectHardLimitNormalCommand( ) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") val isForward = toml.getBoolean("forward") ?: true @@ -113,9 +101,7 @@ class SelectHardLimitNormalCommand( override val activeIndex: Int get() { - if(type == "srx") activeConfig = talonService.activeConfiguration - else if(type == "fx") activeConfig = legacyTalonFxService.activeConfiguration - else throw IllegalArgumentException() + activeConfig = talonService.activeConfiguration return values.indexOf( if (isForward) activeConfig.forwardLimitSwitchNormal @@ -126,13 +112,10 @@ class SelectHardLimitNormalCommand( override fun setActive(index: Int) { val active: Set - if(type == "srx"){ - activeConfig = talonService.activeConfiguration - active = talonService.active - } else if(type == "fx"){ - activeConfig = legacyTalonFxService.activeConfiguration - active = legacyTalonFxService.active - } else throw IllegalArgumentException() + + activeConfig = talonService.activeConfiguration + active = talonService.active + val source = getSource() val normal = values[index] if (isForward) { diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/RunTalonsCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/RunTalonsCommand.kt index 364ce36..5e1955a 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/RunTalonsCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/RunTalonsCommand.kt @@ -10,7 +10,6 @@ import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.prompt -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService import org.strykeforce.thirdcoast.warn import java.lang.IllegalArgumentException @@ -26,7 +25,6 @@ class RunTalonsCommand( val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() override fun execute(): Command { @@ -40,11 +38,8 @@ class RunTalonsCommand( val setpoint = setpoints[0].toDouble() val duration = if (setpoints.size > 1) setpoints[1].toDouble() else 0.0 val mode: ControlMode - if (type == "srx") { - mode = talonService.controlMode - } else if (type == "fx") { - mode = legacyTalonFxService.controlMode - } else throw IllegalArgumentException() + + mode = talonService.controlMode // sanity checks if (mode == PercentOutput && !(-1.0..1.0).contains(setpoint)) { @@ -58,23 +53,13 @@ class RunTalonsCommand( } // run the talons - if (type == "srx"){ - talonService.active.forEach { it.set(mode, setpoint) } - } else if(type == "fx"){ - legacyTalonFxService.active.forEach { it.set(mode, setpoint) } - } - + talonService.active.forEach { it.set(mode, setpoint) } if (duration > 0.0) { logger.debug { "run duration = $duration seconds" } Timer.delay(duration) logger.debug { "run duration expired, setting output = 0.0" } - if(type == "srx"){ - talonService.active.forEach { it.set(mode, 0.0) } - } else if(type == "fx"){ - legacyTalonFxService.active.forEach { it.set(mode, 0.0) } - } - + talonService.active.forEach { it.set(mode, 0.0) } } } catch (e: Exception) { done = true diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectAbsoluteSensorRange.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectAbsoluteSensorRange.kt deleted file mode 100644 index ade697b..0000000 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectAbsoluteSensorRange.kt +++ /dev/null @@ -1,36 +0,0 @@ -package org.strykeforce.thirdcoast.talon - -import com.ctre.phoenix.sensors.AbsoluteSensorRange -import net.consensys.cava.toml.TomlTable -import org.koin.core.component.inject -import org.strykeforce.thirdcoast.command.AbstractSelectCommand -import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService - -private val RANGE = listOf( - AbsoluteSensorRange.Signed_PlusMinus180, - AbsoluteSensorRange.Unsigned_0_to_360 -) - -private val LABELS = listOf( - "-180 - +180 degrees", - "0 - 360 degrees" -) - -class SelectAbsoluteSensorRange( - parent: Command?, - key: String, - toml: TomlTable -) : AbstractSelectCommand(parent, key, toml, RANGE, LABELS) { - - private val legacyTalonFxService: LegacyTalonFxService by inject() - - override val activeIndex: Int - get() = values.indexOf(legacyTalonFxService.activeConfiguration.absoluteSensorRange) - - override fun setActive(index: Int) { - val range = values[index] - legacyTalonFxService.active.forEach { it.configIntegratedSensorAbsoluteRange(range, legacyTalonFxService.timeout) } - legacyTalonFxService.activeConfiguration.absoluteSensorRange = range - } -} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectBrakeModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectBrakeModeCommand.kt index b212477..39d1ea5 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectBrakeModeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectBrakeModeCommand.kt @@ -6,7 +6,6 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService class SelectBrakeModeCommand( @@ -21,24 +20,16 @@ class SelectBrakeModeCommand( listOf("B/C CAL Button", "Brake", "Coast") ) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(type == "srx") return values.indexOf(talonService.neutralMode) - else if(type == "fx") return values.indexOf(legacyTalonFxService.neutralMode) - else throw IllegalArgumentException() + return values.indexOf(talonService.neutralMode) } override fun setActive(index: Int) { - if(type == "srx"){ - talonService.neutralMode = values[index] - talonService.active.forEach { it.setNeutralMode(talonService.neutralMode) } - } else if(type == "fx"){ - legacyTalonFxService.neutralMode = values[index] - legacyTalonFxService.active.forEach { it.setNeutralMode(legacyTalonFxService.neutralMode) } - } + talonService.neutralMode = values[index] + talonService.active.forEach { it.setNeutralMode(talonService.neutralMode)} } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectControlModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectControlModeCommand.kt index da124ed..0b63db4 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectControlModeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectControlModeCommand.kt @@ -6,7 +6,6 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService @@ -22,19 +21,15 @@ class SelectControlModeCommand( listOf("Percent Output", "Motion Magic", "Velocity", "Position", "Follower") ) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(type == "srx") return values.indexOf(talonService.controlMode) - else if(type == "fx") return values.indexOf(legacyTalonFxService.controlMode) - else throw IllegalArgumentException() + return values.indexOf(talonService.controlMode) } override fun setActive(index: Int) { - if(type == "srx") talonService.controlMode = values[index] - else if(type == "fx") legacyTalonFxService.controlMode = values[index] + talonService.controlMode = values[index] } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectFeedbackSensorCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectFeedbackSensorCommand.kt index f78d6d6..c427107 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectFeedbackSensorCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectFeedbackSensorCommand.kt @@ -7,7 +7,6 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService private val SENSORS = listOf( @@ -20,21 +19,23 @@ private val SENSORS = listOf( SensorSum, SoftwareEmulatedSensor, Tachometer, - IntegratedSensor, - None + None, + CTRE_MagEncoder_Absolute, + CTRE_MagEncoder_Relative ) private val LABELS = listOf( "Analog", - "CTRE Relative or Quad Encoder", - "CTRE Absolute or Pulse-width Encoder", + "Quad Encoder", + "Pulse-width Encoder", "Remote Sensor 0", "Remote Sensor 1", "Sensor Difference", "Sensor Sum", "Software Emulated Sensor", "Tachometer", - "Integrated Sensor", - "None" + "None", + "CTRE Absolute", + "CTRE Relative" ) class SelectFeedbackSensorCommand( @@ -44,16 +45,13 @@ class SelectFeedbackSensorCommand( ) : AbstractSelectCommand(parent, key, toml, SENSORS, LABELS) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val pidIndex = toml.getLong("pid")?.toInt() ?: 0 override val activeIndex: Int get() { val config: BaseTalonConfiguration - if (type == "srx") config = talonService.activeConfiguration - else if (type == "fx") config = legacyTalonFxService.activeConfiguration - else throw IllegalArgumentException() + config = talonService.activeConfiguration val sensor = when (pidIndex) { 0 -> config.primaryPID.selectedFeedbackSensor @@ -72,13 +70,10 @@ class SelectFeedbackSensorCommand( override fun setActive(index: Int) { val sensor = values[index] val config: BaseTalonConfiguration - if (type == "srx") { - config = talonService.activeConfiguration - talonService.active.forEach { it.configSelectedFeedbackSensor(sensor, pidIndex, talonService.timeout) } - } else if (type == "fx") { - config = legacyTalonFxService.activeConfiguration - legacyTalonFxService.active.forEach { it.configSelectedFeedbackSensor(sensor, pidIndex, legacyTalonFxService.timeout) } - } else throw IllegalArgumentException() + + config = talonService.activeConfiguration + talonService.active.forEach { it.configSelectedFeedbackSensor(sensor, pidIndex, talonService.timeout) } + when (pidIndex) { 0 -> config.primaryPID.selectedFeedbackSensor = sensor else -> config.auxiliaryPID.selectedFeedbackSensor = sensor diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectInitializationStrategy.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectInitializationStrategy.kt deleted file mode 100644 index 0767fd4..0000000 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectInitializationStrategy.kt +++ /dev/null @@ -1,36 +0,0 @@ -package org.strykeforce.thirdcoast.talon - -import com.ctre.phoenix.sensors.SensorInitializationStrategy -import net.consensys.cava.toml.TomlTable -import org.koin.core.component.inject -import org.strykeforce.thirdcoast.command.AbstractSelectCommand -import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService - -private val STRATEGY = listOf( - SensorInitializationStrategy.BootToAbsolutePosition, - SensorInitializationStrategy.BootToZero -) - -private val LABELS = listOf( - "Absolute Position", - "Zero" -) - -class SelectInitializationStrategy( - parent: Command?, - key: String, - toml: TomlTable -) : AbstractSelectCommand(parent, key, toml, STRATEGY, LABELS) { - - private val legacyTalonFxService: LegacyTalonFxService by inject() - - override val activeIndex: Int - get() = values.indexOf(legacyTalonFxService.activeConfiguration.initializationStrategy) - - override fun setActive(index: Int) { - val mode = values[index] - legacyTalonFxService.active.forEach { it.configIntegratedSensorInitializationStrategy(mode, legacyTalonFxService.timeout) } - legacyTalonFxService.activeConfiguration.initializationStrategy = mode - } -} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectMotorCommutationCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectMotorCommutationCommand.kt deleted file mode 100644 index 065c9cf..0000000 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectMotorCommutationCommand.kt +++ /dev/null @@ -1,34 +0,0 @@ -package org.strykeforce.thirdcoast.talon - -import com.ctre.phoenix.motorcontrol.MotorCommutation -import net.consensys.cava.toml.TomlTable -import org.koin.core.component.inject -import org.strykeforce.thirdcoast.command.AbstractSelectCommand -import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService - -private val COMMUTATION = listOf( - MotorCommutation.Trapezoidal -) - -private val LABELS = listOf( - "Trapezoidal" -) - -class SelectMotorCommutationCommand( - parent: Command?, - key: String, - toml: TomlTable -) : AbstractSelectCommand(parent, key, toml, COMMUTATION, LABELS) { - - private val legacyTalonFxService: LegacyTalonFxService by inject() - - override val activeIndex: Int - get() = values.indexOf(legacyTalonFxService.activeConfiguration.motorCommutation) - - override fun setActive(index: Int) { - val mode = values[index] - legacyTalonFxService.active.forEach { it.configMotorCommutation(mode,legacyTalonFxService.timeout) } - legacyTalonFxService.activeConfiguration.motorCommutation = mode - } -} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectSlotCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectSlotCommand.kt index 43268a6..0d471e2 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectSlotCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectSlotCommand.kt @@ -4,7 +4,6 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService private val SLOTS = listOf(0, 1, 2, 3) @@ -18,17 +17,13 @@ class SelectSlotCommand( val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() override val activeIndex: Int get() { - if(type == "srx") return talonService.activeSlotIndex - else if(type == "fx") return legacyTalonFxService.activeSlotIndex - else throw IllegalArgumentException() + return talonService.activeSlotIndex } override fun setActive(index: Int) { - if(type == "srx") talonService.activeSlotIndex = index - else if(type == "fx") legacyTalonFxService.activeSlotIndex = index + talonService.activeSlotIndex = index } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectTalonsCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectTalonsCommand.kt index 8111f67..e7b71d1 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectTalonsCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectTalonsCommand.kt @@ -1,13 +1,11 @@ package org.strykeforce.thirdcoast.talon -import com.ctre.phoenix.motorcontrol.can.TalonFX import com.ctre.phoenix.motorcontrol.can.TalonSRX import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.prompt -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService import org.strykeforce.thirdcoast.info import org.strykeforce.thirdcoast.readIntList @@ -19,21 +17,12 @@ class SelectTalonsCommand( toml: TomlTable ) : AbstractCommand(parent, key, toml) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val menu: String get() { - when (type) { - "srx" -> { - return formatMenu(talonService.active.map(TalonSRX::getDeviceID).joinToString()) - } - "fx" -> { - return formatMenu(legacyTalonFxService.active.map(TalonFX::getDeviceID).joinToString()) - } - else -> throw IllegalArgumentException() - } + return formatMenu(talonService.active.map(TalonSRX::getDeviceID).joinToString()) } override fun execute(): Command { @@ -42,14 +31,8 @@ class SelectTalonsCommand( var ids: List var new: Set - if (type == "srx") { - ids = reader.readIntList(this.prompt("ids"), talonService.active.map(TalonSRX::getDeviceID)) - new = talonService.activate(ids) - } else if (type == "fx") { - ids = reader.readIntList(this.prompt("ids"), legacyTalonFxService.active.map(TalonFX::getDeviceID)) - new = legacyTalonFxService.activate(ids) - - } else throw IllegalArgumentException() + ids = reader.readIntList(this.prompt("ids"), talonService.active.map(TalonSRX::getDeviceID)) + new = talonService.activate(ids) if (new.isNotEmpty()) terminal.info( diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectVelocityMeasurmentPeriodCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectVelocityMeasurmentPeriodCommand.kt index 0e345ab..7df760c 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectVelocityMeasurmentPeriodCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectVelocityMeasurmentPeriodCommand.kt @@ -6,7 +6,6 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService class SelectVelocityMeasurmentPeriodCommand( @@ -21,23 +20,15 @@ class SelectVelocityMeasurmentPeriodCommand( listOf("1 ms", "2 ms", "5 ms", "10 ms", "20 ms", "50 ms", "100 ms") ) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(type == "srx") return values.indexOf(talonService.activeConfiguration.velocityMeasurementPeriod) - else if(type == "fx") return values.indexOf(legacyTalonFxService.activeConfiguration.velocityMeasurementPeriod) - else throw IllegalArgumentException() + return values.indexOf(talonService.activeConfiguration.velocityMeasurementPeriod) } override fun setActive(index: Int) { val period = values[index] - if(type == "srx"){ - talonService.active.forEach { it.configVelocityMeasurementPeriod(period, talonService.timeout) } - talonService.activeConfiguration.velocityMeasurementPeriod = period - }else if(type == "fx"){ - legacyTalonFxService.active.forEach { it.configVelocityMeasurementPeriod(period, legacyTalonFxService.timeout) } - legacyTalonFxService.activeConfiguration.velocityMeasurementPeriod = period - } + talonService.active.forEach { it.configVelocityMeasurementPeriod(period, talonService.timeout) } + talonService.activeConfiguration.velocityMeasurementPeriod = period } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SetSensorPositionCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SetSensorPositionCommand.kt index a5246a5..f29066c 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/SetSensorPositionCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/SetSensorPositionCommand.kt @@ -5,7 +5,6 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService private val logger = KotlinLogging.logger {} @@ -17,7 +16,6 @@ class SetSensorPositionCommand( ) : AbstractCommand(parent, key, toml) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val timeout = talonService.timeout private val param = TalonParameter.create(this, toml.getString("param") ?: "UNKNOWN") @@ -25,23 +23,15 @@ class SetSensorPositionCommand( override val menu: String get() { - if(type == "srx") return formatMenu(talonService.active.map { it.getSelectedSensorPosition(pidIndex) }.joinToString()) - else if(type == "fx") return formatMenu(legacyTalonFxService.active.map{ it.getSelectedSensorPosition(pidIndex)}.joinToString()) - else throw IllegalArgumentException() + return formatMenu(talonService.active.map { it.getSelectedSensorPosition(pidIndex) }.joinToString()) } override fun execute(): Command { val default = 0 val paramValue = param.readInt(reader, default) - if(type == "srx") { - talonService.active.forEach { it.setSelectedSensorPosition(paramValue.toDouble(), pidIndex, timeout) } - logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } - } - else if(type == "fx") { - legacyTalonFxService.active.forEach { it.setSelectedSensorPosition(paramValue.toDouble(), pidIndex, legacyTalonFxService.timeout) } - logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } - } + talonService.active.forEach { it.setSelectedSensorPosition(paramValue.toDouble(), pidIndex, timeout) } + logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } return super.execute() } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt index 3658e78..711ed15 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt @@ -9,7 +9,6 @@ import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.DOUBLE_FORMAT_4 -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService import org.strykeforce.thirdcoast.talon.TalonParameter.Enum.* @@ -23,7 +22,6 @@ class TalonParameterCommand( ) : AbstractCommand(parent, key, toml) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val timeout = talonService.timeout private val param = TalonParameter.create(this, toml.getString("param") ?: "UNKNOWN") @@ -32,19 +30,9 @@ class TalonParameterCommand( get() { val baseConfig: BaseTalonConfiguration val slot: SlotConfiguration - when (type) { - "srx" -> { - baseConfig = talonService.activeConfiguration - slot = talonService.activeSlot - } - "fx" -> { - baseConfig = legacyTalonFxService.activeConfiguration - slot = legacyTalonFxService.activeSlot - } - else -> throw IllegalArgumentException() - } + baseConfig = talonService.activeConfiguration + slot = talonService.activeSlot val srxConfig = talonService.activeConfiguration - val fxConfig = legacyTalonFxService.activeConfiguration return when (param.enum) { SLOT_P -> formatMenu(slot.kP, DOUBLE_FORMAT_4) SLOT_I -> formatMenu(slot.kI, DOUBLE_FORMAT_4) @@ -54,11 +42,7 @@ class TalonParameterCommand( SLOT_ALLOWABLE_ERR -> formatMenu(slot.allowableClosedloopError) SLOT_MAX_I_ACCUM -> formatMenu(slot.maxIntegralAccumulator) SLOT_PEAK_OUTPUT -> formatMenu(slot.closedLoopPeakOutput) - OUTPUT_REVERSED -> when(type){ - "srx" -> formatMenu(talonService.outputInverted) - "fx" -> formatMenu(legacyTalonFxService.outputInverted) - else -> throw IllegalArgumentException() - } + OUTPUT_REVERSED -> formatMenu(talonService.outputInverted) OPEN_LOOP_RAMP -> formatMenu(baseConfig.openloopRamp) CLOSED_LOOP_RAMP -> formatMenu(baseConfig.closedloopRamp) PEAK_OUTPUT_FORWARD -> formatMenu(baseConfig.peakOutputForward) @@ -66,48 +50,20 @@ class TalonParameterCommand( NOMINAL_OUTPUT_FORWARD -> formatMenu(baseConfig.nominalOutputForward) NOMINAL_OUTPUT_REVERSE -> formatMenu(baseConfig.nominalOutputReverse) NEUTRAL_DEADBAND -> formatMenu(baseConfig.neutralDeadband) - VOLTAGE_COMP_ENABLE -> when (type) { - "srx" -> formatMenu(talonService.voltageCompensation) - "fx" -> formatMenu(legacyTalonFxService.voltageCompensation) - else -> throw IllegalArgumentException() - } + VOLTAGE_COMP_ENABLE -> formatMenu(talonService.voltageCompensation) VOLTAGE_COMP_SATURATION -> formatMenu(baseConfig.voltageCompSaturation) VOLTAGE_MEASUREMENT_FILTER -> formatMenu(baseConfig.voltageMeasurementFilter) MOTION_CRUISE_VELOCITY -> formatMenu(baseConfig.motionCruiseVelocity) MOTION_ACCELERATION -> formatMenu(baseConfig.motionAcceleration) - SENSOR_PHASE -> when (type) { - "srx" -> formatMenu(talonService.sensorPhase) - "fx" -> formatMenu(legacyTalonFxService.sensorPhase) - else -> throw IllegalArgumentException() - } + SENSOR_PHASE -> formatMenu(talonService.sensorPhase) CURRENT_LIMIT_ENABLE -> formatMenu(talonService.currentLimit) CURRENT_LIMIT_CONT -> formatMenu(srxConfig.continuousCurrentLimit) CURRENT_LIMIT_PEAK -> formatMenu(srxConfig.peakCurrentLimit) CURRENT_LIMIT_PEAK_DURATION -> formatMenu(srxConfig.peakCurrentDuration) - STATOR_CURRENT_LIMIT_ENABLE -> formatMenu(fxConfig.statorCurrLimit.enable) - STATOR_CURRENT_LIMIT -> formatMenu(fxConfig.statorCurrLimit.currentLimit) - STATOR_CURRENT_LIMIT_THRES_CURRENT -> formatMenu(fxConfig.statorCurrLimit.triggerThresholdCurrent) - STATOR_CURRENT_LIMIT_THRES_TIME -> formatMenu(fxConfig.statorCurrLimit.triggerThresholdTime) - SUPPLY_CURRENT_LIMIT_ENABLE -> when (type) { - "srx" -> formatMenu(talonService.supplyCurrentLimit.enable) - "fx" -> formatMenu(fxConfig.supplyCurrLimit.enable) - else -> throw IllegalArgumentException() - } - SUPPLY_CURRENT_LIMIT -> when (type) { - "srx" -> formatMenu(talonService.supplyCurrentLimit.currentLimit) - "fx" -> formatMenu(fxConfig.supplyCurrLimit.currentLimit) - else -> throw IllegalArgumentException() - } - SUPPLY_CURRENT_LIMIT_THRES_CURRENT -> when (type) { - "srx" -> formatMenu(talonService.supplyCurrentLimit.triggerThresholdCurrent) - "fx" -> formatMenu(fxConfig.supplyCurrLimit.triggerThresholdCurrent) - else -> throw IllegalArgumentException() - } - SUPPLY_CURRENT_LIMIT_THRES_TIME -> when (type) { - "srx" -> formatMenu(talonService.supplyCurrentLimit.triggerThresholdTime) - "fx" -> formatMenu(fxConfig.supplyCurrLimit.triggerThresholdTime) - else -> throw IllegalArgumentException() - } + SUPPLY_CURRENT_LIMIT_ENABLE -> formatMenu(talonService.supplyCurrentLimit.enable) + SUPPLY_CURRENT_LIMIT -> formatMenu(talonService.supplyCurrentLimit.currentLimit) + SUPPLY_CURRENT_LIMIT_THRES_CURRENT -> formatMenu(talonService.supplyCurrentLimit.triggerThresholdCurrent) + SUPPLY_CURRENT_LIMIT_THRES_TIME -> formatMenu(talonService.supplyCurrentLimit.triggerThresholdTime) STATUS_GENERAL -> formatMenu(defaultFor(Status_1_General)) STATUS_FEEDBACK0 -> formatMenu(defaultFor(Status_2_Feedback0)) STATUS_QUAD_ENCODER -> formatMenu(defaultFor(Status_3_Quadrature)) @@ -127,7 +83,6 @@ class TalonParameterCommand( SOFT_LIMIT_THRESHOLD_FORWARD -> formatMenu(baseConfig.forwardSoftLimitThreshold) SOFT_LIMIT_THRESHOLD_REVERSE -> formatMenu(baseConfig.reverseSoftLimitThreshold) VELOCITY_MEASUREMENT_WINDOW -> formatMenu(baseConfig.velocityMeasurementWindow) - INTEGRATED_SENSOR_OFFSET_DEGREES -> formatMenu(fxConfig.integratedSensorOffsetDegrees) FACTORY_DEFAULTS -> tomlMenu else -> TODO("${param.enum} not implemented") } @@ -138,23 +93,11 @@ class TalonParameterCommand( val slot: SlotConfiguration val activeSlotIndex: Int val timeout: Int - when (type) { - "srx" -> { - config = talonService.activeConfiguration - slot = talonService.activeSlot - activeSlotIndex = talonService.activeSlotIndex - timeout = talonService.timeout - } - "fx" -> { - config = legacyTalonFxService.activeConfiguration - slot = legacyTalonFxService.activeSlot - activeSlotIndex = legacyTalonFxService.activeSlotIndex - timeout = legacyTalonFxService.timeout - } - else -> throw IllegalArgumentException() - } + config = talonService.activeConfiguration + slot = talonService.activeSlot + activeSlotIndex = talonService.activeSlotIndex + timeout = talonService.timeout val srxConfig = talonService.activeConfiguration - val fxConfig = legacyTalonFxService.activeConfiguration when (param.enum) { SLOT_P -> configDoubleParam(slot.kP) { baseTalon, value -> @@ -223,11 +166,7 @@ class TalonParameterCommand( } VOLTAGE_COMP_ENABLE -> configBooleanParam(talonService.voltageCompensation) { baseTalon, value -> baseTalon.enableVoltageCompensation(value) - when (type) { - "srx" -> talonService.voltageCompensation = value - "fx" -> legacyTalonFxService.voltageCompensation = value - else -> throw IllegalArgumentException() - } + talonService.voltageCompensation = value } VOLTAGE_COMP_SATURATION -> configDoubleParam(config.voltageCompSaturation) { baseTalon, value -> baseTalon.configVoltageCompSaturation(value, timeout) @@ -247,11 +186,7 @@ class TalonParameterCommand( } SENSOR_PHASE -> configBooleanParam(talonService.sensorPhase) { baseTalon, value -> baseTalon.setSensorPhase(value) - when (type) { - "srx" -> talonService.sensorPhase = value - "fx" -> legacyTalonFxService.sensorPhase = value - else -> throw IllegalArgumentException() - } + talonService.sensorPhase = value } CURRENT_LIMIT_ENABLE -> configBooleanSrxParam(talonService.currentLimit) { talon, value -> talon.enableCurrentLimit(value) @@ -269,72 +204,28 @@ class TalonParameterCommand( talon.configPeakCurrentDuration(value, timeout) srxConfig.peakCurrentDuration = value } - STATOR_CURRENT_LIMIT_ENABLE -> configBooleanFxParam(fxConfig.statorCurrLimit.enable) { talonFx, value -> - fxConfig.statorCurrLimit.enable = value - talonFx.configStatorCurrentLimit(fxConfig.statorCurrLimit, timeout) - } - STATOR_CURRENT_LIMIT -> configDoubleFxParam(fxConfig.statorCurrLimit.currentLimit) { talonFx, value -> - fxConfig.statorCurrLimit.currentLimit = value - talonFx.configStatorCurrentLimit(fxConfig.statorCurrLimit, timeout) - } - STATOR_CURRENT_LIMIT_THRES_CURRENT -> configDoubleFxParam(fxConfig.statorCurrLimit.triggerThresholdCurrent) { talonFx, value -> - fxConfig.statorCurrLimit.triggerThresholdCurrent = value - talonFx.configStatorCurrentLimit(fxConfig.statorCurrLimit, timeout) - } - STATOR_CURRENT_LIMIT_THRES_TIME -> configDoubleFxParam(fxConfig.statorCurrLimit.triggerThresholdTime) { talonFx, value -> - fxConfig.statorCurrLimit.triggerThresholdTime = value - talonFx.configStatorCurrentLimit(fxConfig.statorCurrLimit, timeout) - } SUPPLY_CURRENT_LIMIT_ENABLE -> { - when (type) { - "srx" -> configBooleanSrxParam(talonService.supplyCurrentLimit.enable) { talon, value -> - talonService.supplyCurrentLimit.enable = value - talon.configSupplyCurrentLimit(talonService.supplyCurrentLimit, timeout) - } - "fx" -> configBooleanFxParam(fxConfig.supplyCurrLimit.enable) { talonFx, value -> - fxConfig.supplyCurrLimit.enable = value - talonFx.configSupplyCurrentLimit(fxConfig.supplyCurrLimit, timeout) - } - else -> throw IllegalArgumentException() + configBooleanSrxParam(talonService.supplyCurrentLimit.enable) { talon, value -> + talonService.supplyCurrentLimit.enable = value + talon.configSupplyCurrentLimit(talonService.supplyCurrentLimit, timeout) } } SUPPLY_CURRENT_LIMIT -> { - when (type) { - "srx" -> configDoubleSrxParam(talonService.supplyCurrentLimit.currentLimit) { talon, value -> - talonService.supplyCurrentLimit.currentLimit = value - talon.configSupplyCurrentLimit(talonService.supplyCurrentLimit, timeout) - } - "fx" -> configDoubleFxParam(fxConfig.supplyCurrLimit.currentLimit) { talonFx, value -> - fxConfig.supplyCurrLimit.currentLimit = value - talonFx.configSupplyCurrentLimit(fxConfig.supplyCurrLimit, timeout) - } - else -> throw IllegalArgumentException() + configDoubleSrxParam(talonService.supplyCurrentLimit.currentLimit) { talon, value -> + talonService.supplyCurrentLimit.currentLimit = value + talon.configSupplyCurrentLimit(talonService.supplyCurrentLimit, timeout) } } SUPPLY_CURRENT_LIMIT_THRES_CURRENT -> { - when(type){ - "srx" -> configDoubleSrxParam(talonService.supplyCurrentLimit.triggerThresholdCurrent) { talon, value -> - talonService.supplyCurrentLimit.triggerThresholdCurrent = value - talon.configSupplyCurrentLimit(talonService.supplyCurrentLimit, timeout) - } - "fx" -> configDoubleFxParam(fxConfig.supplyCurrLimit.triggerThresholdCurrent) { talonFx, value -> - fxConfig.supplyCurrLimit.triggerThresholdCurrent = value - talonFx.configSupplyCurrentLimit(fxConfig.supplyCurrLimit, timeout) - } - else -> throw IllegalArgumentException() + configDoubleSrxParam(talonService.supplyCurrentLimit.triggerThresholdCurrent) { talon, value -> + talonService.supplyCurrentLimit.triggerThresholdCurrent = value + talon.configSupplyCurrentLimit(talonService.supplyCurrentLimit, timeout) } } SUPPLY_CURRENT_LIMIT_THRES_TIME -> { - when(type){ - "srx" -> configDoubleSrxParam(talonService.supplyCurrentLimit.triggerThresholdTime) { talon, value -> - talonService.supplyCurrentLimit.triggerThresholdTime = value - talon.configSupplyCurrentLimit(talonService.supplyCurrentLimit, timeout) - } - "fx" -> configDoubleFxParam(fxConfig.supplyCurrLimit.triggerThresholdTime) { talonFx, value -> - fxConfig.supplyCurrLimit.triggerThresholdTime = value - talonFx.configSupplyCurrentLimit(fxConfig.supplyCurrLimit, timeout) - } - else -> throw IllegalArgumentException() + configDoubleSrxParam(talonService.supplyCurrentLimit.triggerThresholdTime) { talon, value -> + talonService.supplyCurrentLimit.triggerThresholdTime = value + talon.configSupplyCurrentLimit(talonService.supplyCurrentLimit, timeout) } } STATUS_GENERAL -> configIntParam(defaultFor(Status_1_General)) { baseTalon, value -> @@ -398,15 +289,7 @@ class TalonParameterCommand( VELOCITY_MEASUREMENT_WINDOW -> configIntParam(config.velocityMeasurementWindow) { baseTalon, value -> baseTalon.configVelocityMeasurementWindow(value, timeout) // Let the Talon round down then number to a legal value - when (type) { - "srx" -> talonService.dirty = true - "fx" -> legacyTalonFxService.dirty = true - else -> throw IllegalArgumentException() - } - } - INTEGRATED_SENSOR_OFFSET_DEGREES -> configDoubleFxParam(fxConfig.integratedSensorOffsetDegrees) { talonFx, value -> - talonFx.configIntegratedSensorOffset(value, timeout) - fxConfig.integratedSensorOffsetDegrees = value + talonService.dirty = true } FACTORY_DEFAULTS -> configBooleanParam(false) { baseTalon, value -> if (value) baseTalon.configFactoryDefault(timeout) @@ -418,17 +301,8 @@ class TalonParameterCommand( private fun configBooleanParam(default: Boolean, config: (BaseTalon, Boolean) -> Unit) { val paramValue = param.readBoolean(reader, default) - when (type) { - "srx" -> { - talonService.active.forEach { config(it, paramValue) } - logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } - } - "fx" -> { - legacyTalonFxService.active.forEach { config(it, paramValue) } - logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } - } - else -> throw IllegalArgumentException() - } + talonService.active.forEach { config(it, paramValue) } + logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } } private fun configBooleanSrxParam(default: Boolean, config: (TalonSRX, Boolean) -> Unit) { @@ -437,25 +311,11 @@ class TalonParameterCommand( logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } } - private fun configBooleanFxParam(default: Boolean, config: (TalonFX, Boolean) -> Unit) { - val paramValue = param.readBoolean(reader, default) - legacyTalonFxService.active.forEach { config(it, paramValue) } - logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } - } private fun configIntParam(default: Int, config: (BaseTalon, Int) -> Unit) { val paramValue = param.readInt(reader, default) - when (type) { - "srx" -> { - talonService.active.forEach { config(it, paramValue) } - logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } - } - "fx" -> { - legacyTalonFxService.active.forEach { config(it, paramValue) } - logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } - } - else -> throw IllegalArgumentException() - } + talonService.active.forEach { config(it, paramValue) } + logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } } private fun configIntSrxParam(default: Int, config: (TalonSRX, Int) -> Unit) { @@ -466,17 +326,8 @@ class TalonParameterCommand( private fun configDoubleParam(default: Double, config: (BaseTalon, Double) -> Unit) { val paramValue = param.readDouble(reader, default) - when (type) { - "srx" -> { - talonService.active.forEach { config(it, paramValue) } - logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } - } - "fx" -> { - legacyTalonFxService.active.forEach { config(it, paramValue) } - logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } - } - else -> throw IllegalArgumentException() - } + talonService.active.forEach { config(it, paramValue) } + logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } } private fun configDoubleSrxParam(default: Double, config: (TalonSRX, Double) -> Unit) { @@ -485,18 +336,8 @@ class TalonParameterCommand( logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" } } - private fun configDoubleFxParam(default: Double, config: (TalonFX, Double) -> Unit) { - val paramValue = param.readDouble(reader, default) - legacyTalonFxService.active.forEach { config(it, paramValue) } - logger.debug { "set ${legacyTalonFxService.active.size} talonfx ${param.name}: $paramValue" } - } - private fun defaultFor(frame: StatusFrameEnhanced): Int { - when (type) { - "srx" -> return talonService.active.first().getStatusFramePeriod(frame) - "fx" -> return legacyTalonFxService.active.first().getStatusFramePeriod(frame) - else -> throw IllegalArgumentException() - } + return talonService.active.first().getStatusFramePeriod(frame) } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonsStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonsStatusCommand.kt index 34dddf7..9c8a48c 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonsStatusCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonsStatusCommand.kt @@ -1,12 +1,10 @@ package org.strykeforce.thirdcoast.talon -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.LegacyTalonFxService import org.strykeforce.thirdcoast.device.TalonService class TalonsStatusCommand( @@ -16,23 +14,14 @@ class TalonsStatusCommand( ) : AbstractCommand(parent, key, toml) { private val talonService: TalonService by inject() - private val legacyTalonFxService: LegacyTalonFxService by inject() val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override fun execute(): Command { val writer = terminal.writer() - if(type == "srx"){ - talonService.active.forEach { - val config = TalonSRXConfiguration() - it.getAllConfigs(config) - writer.println(config.toString(it.deviceID.toString())) - } - } else if(type == "fx"){ - legacyTalonFxService.active.forEach { - val config = TalonFXConfiguration() - it.getAllConfigs(config) - writer.println(config.toString(it.deviceID.toString())) - } + talonService.active.forEach { + val config = TalonSRXConfiguration() + it.getAllConfigs(config) + writer.println(config.toString(it.deviceID.toString())) } return super.execute() } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt index b87c34f..5ccaf81 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6DefaultStatusFrameCommand.kt @@ -4,8 +4,11 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService +import java.lang.IllegalArgumentException class P6DefaultStatusFrameCommand( parent: Command?, @@ -15,102 +18,218 @@ class P6DefaultStatusFrameCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override fun execute(): Command { val timeout = talonFxService.timeout val value = 10.0 - if(bus == "rio") { - talonFxService.active.forEach { - talonFxService.grapherStatusFrameHz = 0.0; - it.acceleration.setUpdateFrequency(50.0, timeout) - it.bridgeOutput.setUpdateFrequency(100.0, timeout) - it.deviceTemp.setUpdateFrequency(4.0, timeout) - it.differentialAveragePosition.setUpdateFrequency(4.0, timeout) - it.differentialAverageVelocity.setUpdateFrequency(4.0, timeout) - it.differentialDifferencePosition.setUpdateFrequency(4.0, timeout) - it.differentialDifferenceVelocity.setUpdateFrequency(4.0, timeout) - it.dutyCycle.setUpdateFrequency(100.0, timeout) - it.forwardLimit.setUpdateFrequency(100.0, timeout) - it.reverseLimit.setUpdateFrequency(100.0, timeout) - it.isProLicensed.setUpdateFrequency(4.0, timeout) - it.motionMagicIsRunning.setUpdateFrequency(4.0, timeout) - it.motorVoltage.setUpdateFrequency(100.0, timeout) - it.position.setUpdateFrequency(50.0, timeout) - it.processorTemp.setUpdateFrequency(4.0, timeout) - it.rotorPosition.setUpdateFrequency(4.0, timeout) - it.rotorVelocity.setUpdateFrequency(4.0, timeout) - it.statorCurrent.setUpdateFrequency(4.0, timeout) - it.supplyCurrent.setUpdateFrequency(4.0, timeout) - it.supplyVoltage.setUpdateFrequency(4.0, timeout) - it.torqueCurrent.setUpdateFrequency(100.0, timeout) - it.velocity.setUpdateFrequency(50.0, timeout) + when(device) { + "fx" -> { + if (bus == "rio") { + talonFxService.active.forEach { + talonFxService.grapherStatusFrameHz = 0.0; + it.acceleration.setUpdateFrequency(50.0, timeout) + it.bridgeOutput.setUpdateFrequency(100.0, timeout) + it.deviceTemp.setUpdateFrequency(4.0, timeout) + it.differentialAveragePosition.setUpdateFrequency(4.0, timeout) + it.differentialAverageVelocity.setUpdateFrequency(4.0, timeout) + it.differentialDifferencePosition.setUpdateFrequency(4.0, timeout) + it.differentialDifferenceVelocity.setUpdateFrequency(4.0, timeout) + it.dutyCycle.setUpdateFrequency(100.0, timeout) + it.forwardLimit.setUpdateFrequency(100.0, timeout) + it.reverseLimit.setUpdateFrequency(100.0, timeout) + it.isProLicensed.setUpdateFrequency(4.0, timeout) + it.motionMagicIsRunning.setUpdateFrequency(4.0, timeout) + it.motorVoltage.setUpdateFrequency(100.0, timeout) + it.position.setUpdateFrequency(50.0, timeout) + it.processorTemp.setUpdateFrequency(4.0, timeout) + it.rotorPosition.setUpdateFrequency(4.0, timeout) + it.rotorVelocity.setUpdateFrequency(4.0, timeout) + it.statorCurrent.setUpdateFrequency(4.0, timeout) + it.supplyCurrent.setUpdateFrequency(4.0, timeout) + it.supplyVoltage.setUpdateFrequency(4.0, timeout) + it.torqueCurrent.setUpdateFrequency(100.0, timeout) + it.velocity.setUpdateFrequency(50.0, timeout) + + it.closedLoopDerivativeOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopError.setUpdateFrequency(4.0, timeout) + it.closedLoopFeedForward.setUpdateFrequency(4.0, timeout) + it.closedLoopIntegratedOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopProportionalOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopReference.setUpdateFrequency(4.0, timeout) + it.closedLoopReferenceSlope.setUpdateFrequency(4.0, timeout) + it.closedLoopSlot.setUpdateFrequency(4.0, timeout) + it.differentialClosedLoopDerivativeOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopError.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopFeedForward.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopIntegratedOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopProportionalOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReference.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReferenceSlope.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopSlot.setUpdateFrequency(4.0, timeout) + } + } else if (bus == "canivore") { + talonFxFDService.active.forEach { + talonFxFDService.grapherStatusFrameHz = 0.0; + it.acceleration.setUpdateFrequency(50.0, timeout) + it.bridgeOutput.setUpdateFrequency(100.0, timeout) + it.deviceTemp.setUpdateFrequency(4.0, timeout) + it.differentialAveragePosition.setUpdateFrequency(4.0, timeout) + it.differentialAverageVelocity.setUpdateFrequency(4.0, timeout) + it.differentialDifferencePosition.setUpdateFrequency(4.0, timeout) + it.differentialDifferenceVelocity.setUpdateFrequency(4.0, timeout) + it.dutyCycle.setUpdateFrequency(100.0, timeout) + it.forwardLimit.setUpdateFrequency(100.0, timeout) + it.reverseLimit.setUpdateFrequency(100.0, timeout) + it.isProLicensed.setUpdateFrequency(4.0, timeout) + it.motionMagicIsRunning.setUpdateFrequency(4.0, timeout) + it.motorVoltage.setUpdateFrequency(100.0, timeout) + it.position.setUpdateFrequency(50.0, timeout) + it.processorTemp.setUpdateFrequency(4.0, timeout) + it.rotorPosition.setUpdateFrequency(4.0, timeout) + it.rotorVelocity.setUpdateFrequency(4.0, timeout) + it.statorCurrent.setUpdateFrequency(4.0, timeout) + it.supplyCurrent.setUpdateFrequency(4.0, timeout) + it.supplyVoltage.setUpdateFrequency(4.0, timeout) + it.torqueCurrent.setUpdateFrequency(100.0, timeout) + it.velocity.setUpdateFrequency(50.0, timeout) + + it.closedLoopDerivativeOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopError.setUpdateFrequency(4.0, timeout) + it.closedLoopFeedForward.setUpdateFrequency(4.0, timeout) + it.closedLoopIntegratedOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopProportionalOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopReference.setUpdateFrequency(4.0, timeout) + it.closedLoopReferenceSlope.setUpdateFrequency(4.0, timeout) + it.closedLoopSlot.setUpdateFrequency(4.0, timeout) + it.differentialClosedLoopDerivativeOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopError.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopFeedForward.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopIntegratedOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopProportionalOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReference.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReferenceSlope.setUpdateFrequency(100.0, timeout) + } + } - it.closedLoopDerivativeOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopError.setUpdateFrequency(4.0, timeout) - it.closedLoopFeedForward.setUpdateFrequency(4.0, timeout) - it.closedLoopIntegratedOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopProportionalOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopReference.setUpdateFrequency(4.0, timeout) - it.closedLoopReferenceSlope.setUpdateFrequency(4.0, timeout) - it.closedLoopSlot.setUpdateFrequency(4.0, timeout) - it.differentialClosedLoopDerivativeOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopError.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopFeedForward.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopIntegratedOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopProportionalOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopReference.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopReferenceSlope.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopSlot.setUpdateFrequency(4.0, timeout) } - } else if(bus == "canivore") { - talonFxFDService.active.forEach { - talonFxFDService.grapherStatusFrameHz = 0.0; - it.acceleration.setUpdateFrequency(50.0, timeout) - it.bridgeOutput.setUpdateFrequency(100.0, timeout) - it.deviceTemp.setUpdateFrequency(4.0, timeout) - it.differentialAveragePosition.setUpdateFrequency(4.0, timeout) - it.differentialAverageVelocity.setUpdateFrequency(4.0, timeout) - it.differentialDifferencePosition.setUpdateFrequency(4.0, timeout) - it.differentialDifferenceVelocity.setUpdateFrequency(4.0, timeout) - it.dutyCycle.setUpdateFrequency(100.0, timeout) - it.forwardLimit.setUpdateFrequency(100.0, timeout) - it.reverseLimit.setUpdateFrequency(100.0, timeout) - it.isProLicensed.setUpdateFrequency(4.0, timeout) - it.motionMagicIsRunning.setUpdateFrequency(4.0, timeout) - it.motorVoltage.setUpdateFrequency(100.0, timeout) - it.position.setUpdateFrequency(50.0, timeout) - it.processorTemp.setUpdateFrequency(4.0, timeout) - it.rotorPosition.setUpdateFrequency(4.0, timeout) - it.rotorVelocity.setUpdateFrequency(4.0, timeout) - it.statorCurrent.setUpdateFrequency(4.0, timeout) - it.supplyCurrent.setUpdateFrequency(4.0, timeout) - it.supplyVoltage.setUpdateFrequency(4.0, timeout) - it.torqueCurrent.setUpdateFrequency(100.0, timeout) - it.velocity.setUpdateFrequency(50.0, timeout) - it.closedLoopDerivativeOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopError.setUpdateFrequency(4.0, timeout) - it.closedLoopFeedForward.setUpdateFrequency(4.0, timeout) - it.closedLoopIntegratedOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopProportionalOutput.setUpdateFrequency(4.0, timeout) - it.closedLoopReference.setUpdateFrequency(4.0, timeout) - it.closedLoopReferenceSlope.setUpdateFrequency(4.0, timeout) - it.closedLoopSlot.setUpdateFrequency(4.0, timeout) - it.differentialClosedLoopDerivativeOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopError.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopFeedForward.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopIntegratedOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopProportionalOutput.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopReference.setUpdateFrequency(100.0, timeout) - it.differentialClosedLoopReferenceSlope.setUpdateFrequency(100.0, timeout) + "fxs" -> { + if (bus == "rio") { + talonFxsService.active.forEach { + talonFxsService.grapherStatusFrameHz = 0.0; + it.acceleration.setUpdateFrequency(50.0, timeout) + it.bridgeOutput.setUpdateFrequency(100.0, timeout) + it.deviceTemp.setUpdateFrequency(4.0, timeout) + it.differentialAveragePosition.setUpdateFrequency(4.0, timeout) + it.differentialAverageVelocity.setUpdateFrequency(4.0, timeout) + it.differentialDifferencePosition.setUpdateFrequency(4.0, timeout) + it.differentialDifferenceVelocity.setUpdateFrequency(4.0, timeout) + it.dutyCycle.setUpdateFrequency(100.0, timeout) + it.forwardLimit.setUpdateFrequency(100.0, timeout) + it.reverseLimit.setUpdateFrequency(100.0, timeout) + it.isProLicensed.setUpdateFrequency(4.0, timeout) + it.motionMagicIsRunning.setUpdateFrequency(4.0, timeout) + it.motorVoltage.setUpdateFrequency(100.0, timeout) + it.position.setUpdateFrequency(50.0, timeout) + it.processorTemp.setUpdateFrequency(4.0, timeout) + it.rotorPosition.setUpdateFrequency(4.0, timeout) + it.rotorVelocity.setUpdateFrequency(4.0, timeout) + it.statorCurrent.setUpdateFrequency(4.0, timeout) + it.supplyCurrent.setUpdateFrequency(4.0, timeout) + it.supplyVoltage.setUpdateFrequency(4.0, timeout) + it.torqueCurrent.setUpdateFrequency(100.0, timeout) + it.velocity.setUpdateFrequency(50.0, timeout) + + it.closedLoopDerivativeOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopError.setUpdateFrequency(4.0, timeout) + it.closedLoopFeedForward.setUpdateFrequency(4.0, timeout) + it.closedLoopIntegratedOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopProportionalOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopReference.setUpdateFrequency(4.0, timeout) + it.closedLoopReferenceSlope.setUpdateFrequency(4.0, timeout) + it.closedLoopSlot.setUpdateFrequency(4.0, timeout) + it.differentialClosedLoopDerivativeOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopError.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopFeedForward.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopIntegratedOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopProportionalOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReference.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReferenceSlope.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopSlot.setUpdateFrequency(4.0, timeout) + it.ancillaryDeviceTemp.setUpdateFrequency(4.0, timeout) + it.externalMotorTemp.setUpdateFrequency(4.0, timeout) + it.rawPulseWidthPosition.setUpdateFrequency(0.0,timeout) + it.rawQuadraturePosition.setUpdateFrequency(0.0, timeout) + it.rawPulseWidthVelocity.setUpdateFrequency(0.0, timeout) + it.rawQuadratureVelocity.setUpdateFrequency(0.0, timeout) + } + } else if (bus == "canivore") { + talonFxsFDService.active.forEach { + talonFxFDService.grapherStatusFrameHz = 0.0; + it.acceleration.setUpdateFrequency(50.0, timeout) + it.bridgeOutput.setUpdateFrequency(100.0, timeout) + it.deviceTemp.setUpdateFrequency(4.0, timeout) + it.differentialAveragePosition.setUpdateFrequency(4.0, timeout) + it.differentialAverageVelocity.setUpdateFrequency(4.0, timeout) + it.differentialDifferencePosition.setUpdateFrequency(4.0, timeout) + it.differentialDifferenceVelocity.setUpdateFrequency(4.0, timeout) + it.dutyCycle.setUpdateFrequency(100.0, timeout) + it.forwardLimit.setUpdateFrequency(100.0, timeout) + it.reverseLimit.setUpdateFrequency(100.0, timeout) + it.isProLicensed.setUpdateFrequency(4.0, timeout) + it.motionMagicIsRunning.setUpdateFrequency(4.0, timeout) + it.motorVoltage.setUpdateFrequency(100.0, timeout) + it.position.setUpdateFrequency(50.0, timeout) + it.processorTemp.setUpdateFrequency(4.0, timeout) + it.rotorPosition.setUpdateFrequency(4.0, timeout) + it.rotorVelocity.setUpdateFrequency(4.0, timeout) + it.statorCurrent.setUpdateFrequency(4.0, timeout) + it.supplyCurrent.setUpdateFrequency(4.0, timeout) + it.supplyVoltage.setUpdateFrequency(4.0, timeout) + it.torqueCurrent.setUpdateFrequency(100.0, timeout) + it.velocity.setUpdateFrequency(50.0, timeout) + + it.closedLoopDerivativeOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopError.setUpdateFrequency(4.0, timeout) + it.closedLoopFeedForward.setUpdateFrequency(4.0, timeout) + it.closedLoopIntegratedOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopProportionalOutput.setUpdateFrequency(4.0, timeout) + it.closedLoopReference.setUpdateFrequency(4.0, timeout) + it.closedLoopReferenceSlope.setUpdateFrequency(4.0, timeout) + it.closedLoopSlot.setUpdateFrequency(4.0, timeout) + it.differentialClosedLoopDerivativeOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopError.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopFeedForward.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopIntegratedOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopProportionalOutput.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReference.setUpdateFrequency(100.0, timeout) + it.differentialClosedLoopReferenceSlope.setUpdateFrequency(100.0, timeout) + + it.ancillaryDeviceTemp.setUpdateFrequency(4.0, timeout) + it.externalMotorTemp.setUpdateFrequency(4.0, timeout) + it.rawPulseWidthPosition.setUpdateFrequency(0.0,timeout) + it.rawQuadraturePosition.setUpdateFrequency(0.0, timeout) + it.rawPulseWidthVelocity.setUpdateFrequency(0.0, timeout) + it.rawQuadratureVelocity.setUpdateFrequency(0.0, timeout) + } } + + } + else -> throw IllegalArgumentException() } return super.execute() } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt index 33c1f27..62a4be0 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6FactoryDefaultCommand.kt @@ -1,12 +1,15 @@ package org.strykeforce.thirdcoast.talon.phoenix6 import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.configs.TalonFXSConfiguration import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService class P6FactoryDefaultCommand( parent: Command?, @@ -16,18 +19,36 @@ class P6FactoryDefaultCommand( private val talonFxService: TalonFxService by inject() private val talonFxFdService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override fun execute(): Command { - val config = TalonFXConfiguration() - if(bus == "rio") { - talonFxService.activeConfiguration = config - talonFxService.active.forEach { it.configurator.apply(config) } - } else if(bus == "canivore") { - talonFxFdService.activeConfiguration = config - talonFxFdService.active.forEach { it.configurator.apply(config)} - } else throw IllegalArgumentException() + when(device) { + "fx" -> { + val config = TalonFXConfiguration() + if(bus == "rio") { + talonFxService.activeConfiguration = config + talonFxService.active.forEach { it.configurator.apply(config) } + } else if(bus == "canivore") { + talonFxFdService.activeConfiguration = config + talonFxFdService.active.forEach { it.configurator.apply(config)} + } else throw IllegalArgumentException() + } + "fxs" -> { + val config = TalonFXSConfiguration() + if(bus == "rio") { + talonFxsService.activeConfiguration = config + talonFxsService.active.forEach { it.configurator.apply(config) } + } else if(bus == "canivore") { + talonFxsFDService.activeConfiguration = config + talonFxsFDService.active.forEach { it.configurator.apply(config)} + } else throw IllegalArgumentException() + } + } + return super.execute() } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt index 46ab21c..c262ed5 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6HardLimitCommand.kt @@ -10,39 +10,70 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService class P6SelectFwdHardLimitSourceCommand( parent: Command?, key: String, toml: TomlTable ): AbstractSelectCommand(parent, key, toml, - listOf(LimitSwitchPin, RemoteTalonFX, RemoteCANifier), - listOf("Talon FX pin", "Remote TalonFx", "Remote CANifier")) { + listOf(LimitSwitchPin, RemoteTalonFX, RemoteCANifier, RemoteCANcoder, RemoteCANrange, RemoteCANdiS1, RemoteCANdiS2, Disabled), + listOf("Talon FXS pin", "Remote TalonFx", "Remote CANifier", "Remote CANcoder", "Remote CANrange", "Remote CANdi S1", "Remote CANdi S2", "Disabled")) { private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource.ordinal - else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource.ordinal + else if(bus == "canivore") return talonFxsFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val source = values[index] - if(bus == "rio") { - talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource = source - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } - } else if(bus == "canivore") { - talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource = source - talonFxFDService.active.forEach{ it.configurator.apply(talonFxFDService.activeConfiguration)} - } else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") { + talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource = source + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource = source + talonFxFDService.active.forEach{ it.configurator.apply(talonFxFDService.activeConfiguration)} + } else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") { + talonFxsService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource = source + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration) } + } else if(bus == "canivore") { + talonFxsFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitSource = source + talonFxsFDService.active.forEach{ it.configurator.apply(talonFxsFDService.activeConfiguration)} + } else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } @@ -51,32 +82,61 @@ class P6SelectRevHardLimitSourceCommand( key: String, toml: TomlTable ): AbstractSelectCommand(parent, key, toml, - listOf(ReverseLimitSourceValue.LimitSwitchPin, ReverseLimitSourceValue.RemoteTalonFX, ReverseLimitSourceValue.RemoteCANifier), - listOf("Talon FX pin", "Remote TalonFX", "Remote CANifier")) { + listOf(ReverseLimitSourceValue.LimitSwitchPin, ReverseLimitSourceValue.RemoteTalonFX, ReverseLimitSourceValue.RemoteCANifier, ReverseLimitSourceValue.RemoteCANcoder, ReverseLimitSourceValue.RemoteCANrange, ReverseLimitSourceValue.RemoteCANdiS1, ReverseLimitSourceValue.RemoteCANdiS2, ReverseLimitSourceValue.Disabled), + listOf("Talon FXS pin", "Remote TalonFX", "Remote CANifier", "Remote CANcoder", "Remote CANrange", "Remote CANdi S1", "Remote CANdi S2", "Disabled")) { private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource.ordinal - else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource.ordinal + else if(bus == "canivore") return talonFxsFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val source = values[index] - if(bus == "rio") { - talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource = source - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } - } else if(bus == "canivore") { - talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource = source - talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } - } else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") { + talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource = source + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource = source + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } + } else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") { + talonFxsService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource = source + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration) } + } else if(bus == "canivore") { + talonFxsFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitSource = source + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration) } + } else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } @@ -91,26 +151,55 @@ class P6SelectFwdHardLimitNormalCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType.ordinal - else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType.ordinal + else if(bus == "canivore") return talonFxsFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val type = values[index] - if(bus == "rio") { - talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType = type - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } - } else if (bus == "canivore") { - talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType = type - talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } - } else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") { + talonFxService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType = type + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if (bus == "canivore") { + talonFxFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType = type + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } + } else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") { + talonFxsService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType = type + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration) } + } else if (bus == "canivore") { + talonFxsFDService.activeConfiguration.HardwareLimitSwitch.ForwardLimitType = type + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration) } + } else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } @@ -124,27 +213,56 @@ class P6SelectRevHardLimitNormalCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType.ordinal - else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType.ordinal + else if(bus == "canivore") return talonFxsFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val type = values[index] - if(bus == "rio") { - talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType = type - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } - } else if(bus == "canivore") { - talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType = type - talonFxFDService.active.forEach{ it.configurator.apply(talonFxFDService.activeConfiguration)} - } else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") { + talonFxService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType = type + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType = type + talonFxFDService.active.forEach{ it.configurator.apply(talonFxFDService.activeConfiguration)} + } else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") { + talonFxsService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType = type + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration) } + } else if(bus == "canivore") { + talonFxsFDService.activeConfiguration.HardwareLimitSwitch.ReverseLimitType = type + talonFxsFDService.active.forEach{ it.configurator.apply(talonFxsFDService.activeConfiguration)} + } else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt index 4a9435b..d938c1b 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusCommand.kt @@ -4,8 +4,10 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService class P6ModeStatusCommand( parent: Command?, @@ -15,15 +17,29 @@ class P6ModeStatusCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val menu: String get() { - if(bus == "rio") return formatMenu(talonFxService.controlMode) - else if(bus == "canivore") return formatMenu(talonFxFDService.controlMode) - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return formatMenu(talonFxService.controlMode) + else if(bus == "canivore") return formatMenu(talonFxFDService.controlMode) + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return formatMenu(talonFxsService.controlMode) + else if(bus == "canivore") return formatMenu(talonFxsFDService.controlMode) + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun execute(): Command { diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt index 972293e..874ae01 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6ModeStatusMenuCommand.kt @@ -5,8 +5,10 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.* import org.strykeforce.thirdcoast.command.* +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService import org.strykeforce.thirdcoast.invalidMenuChoice import java.util.* @@ -19,8 +21,11 @@ class P6ModeStatusMenuCommand( ): MenuCommand(parent, key, toml) { private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val children = TreeSet(compareBy(Command::order, Command::key)) @@ -28,9 +33,21 @@ class P6ModeStatusMenuCommand( override val menu: String get() { logger.info { "Getting Menu Option..." } - if(bus == "rio") return formatMenu(talonFxService.controlMode) - else if(bus == "canivore") return formatMenu(talonFxFDService.controlMode) - else throw IllegalArgumentException() + logger.info { "Device = " + device } + when(device) { + "fx" -> { + if(bus == "rio") return formatMenu(talonFxService.controlMode) + else if(bus == "canivore") return formatMenu(talonFxFDService.controlMode) + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return formatMenu(talonFxsService.controlMode) + else if(bus == "canivore") return formatMenu(talonFxsFDService.controlMode) + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt index bd61090..09b36b9 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6RunCommand.kt @@ -59,8 +59,11 @@ class P6RunCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override fun execute(): Command { @@ -72,25 +75,69 @@ class P6RunCommand( val setpoints = line.split(',') val setpoint = setpoints[0].toDouble() val duration = if (setpoints.size > 1) setpoints[1].toDouble() else 0.0 - val setpointType = if(bus == "rio") talonFxService.setpointType else talonFxFDService.setpointType - val units = if(bus == "rio") talonFxService.activeUnits else talonFxFDService.activeUnits - val motionType = if(bus == "rio") talonFxService.active_MM_type else talonFxFDService.active_MM_type - val differentialType = if(bus == "rio") talonFxService.differentialType else talonFxFDService.differentialType - val followerType = if(bus == "rio") talonFxService.activeFollowerType else talonFxFDService.activeFollowerType - val enableFOC = if(bus == "rio") talonFxService.activeFOC else talonFxFDService.activeFOC - val overrideNeutral = if(bus == "rio") talonFxService.activeOverrideNeutral else talonFxFDService.activeOverrideNeutral - val limFwdMotion = if(bus == "rio") talonFxService.limFwdMotion else talonFxFDService.limFwdMotion - val limRevMotion = if(bus == "rio") talonFxService.limRevMotion else talonFxFDService.limRevMotion - val velocity = if(bus == "rio") talonFxService.activeVelocity else talonFxFDService.activeVelocity - val acceleration = if(bus == "rio") talonFxService.activeAcceleration else talonFxFDService.activeAcceleration - val jerk = if(bus == "rio") talonFxService.activeJerk else talonFxFDService.activeJerk - val feedFwd = if(bus == "rio") talonFxService.activeFeedForward else talonFxFDService.activeFeedForward - val slot = if(bus == "rio") talonFxService.activeSlotIndex else talonFxFDService.activeSlotIndex - val diffSlot = if(bus == "rio") talonFxService.activeDifferentialSlot else talonFxFDService.activeDifferentialSlot - val diffPos = if(bus == "rio") talonFxService.activeDifferentialTarget else talonFxFDService.activeDifferentialTarget - var controlRequest: ControlRequest = DutyCycleOut(0.0, false, false,limFwdMotion,limRevMotion) - val torqueCurrentMaxOut = if(bus == "rio") talonFxService.activeTorqueCurrentMaxOut else talonFxFDService.activeTorqueCurrentMaxOut - val torqueCurrentDeadband = if(bus == "rio") talonFxService.activeTorqueCurrentDeadband else talonFxFDService.activeTorqueCurrentDeadband + var setpointType: SetpointType + var units: Units + var motionType: MM_Type + var differentialType: DifferentialType + var followerType: FollowerType + var enableFOC: Boolean + var overrideNeutral: Boolean + var limFwdMotion: Boolean + var limRevMotion: Boolean + var velocity: Double + var acceleration: Double + var jerk: Double + var feedFwd: Double + var slot: Int + var diffSlot: Int + var diffPos: Double + var torqueCurrentMaxOut: Double + var torqueCurrentDeadband: Double + when(device) { + "fx" -> { + setpointType = if(bus == "rio") talonFxService.setpointType else talonFxFDService.setpointType + units = if(bus == "rio") talonFxService.activeUnits else talonFxFDService.activeUnits + motionType = if(bus == "rio") talonFxService.active_MM_type else talonFxFDService.active_MM_type + differentialType = if(bus == "rio") talonFxService.differentialType else talonFxFDService.differentialType + followerType = if(bus == "rio") talonFxService.activeFollowerType else talonFxFDService.activeFollowerType + enableFOC = if(bus == "rio") talonFxService.activeFOC else talonFxFDService.activeFOC + overrideNeutral = if(bus == "rio") talonFxService.activeOverrideNeutral else talonFxFDService.activeOverrideNeutral + limFwdMotion = if(bus == "rio") talonFxService.limFwdMotion else talonFxFDService.limFwdMotion + limRevMotion = if(bus == "rio") talonFxService.limRevMotion else talonFxFDService.limRevMotion + velocity = if(bus == "rio") talonFxService.activeVelocity else talonFxFDService.activeVelocity + acceleration = if(bus == "rio") talonFxService.activeAcceleration else talonFxFDService.activeAcceleration + jerk = if(bus == "rio") talonFxService.activeJerk else talonFxFDService.activeJerk + feedFwd = if(bus == "rio") talonFxService.activeFeedForward else talonFxFDService.activeFeedForward + slot = if(bus == "rio") talonFxService.activeSlotIndex else talonFxFDService.activeSlotIndex + diffSlot = if(bus == "rio") talonFxService.activeDifferentialSlot else talonFxFDService.activeDifferentialSlot + diffPos = if(bus == "rio") talonFxService.activeDifferentialTarget else talonFxFDService.activeDifferentialTarget + torqueCurrentMaxOut = if(bus == "rio") talonFxService.activeTorqueCurrentMaxOut else talonFxFDService.activeTorqueCurrentMaxOut + torqueCurrentDeadband = if(bus == "rio") talonFxService.activeTorqueCurrentDeadband else talonFxFDService.activeTorqueCurrentDeadband + } + "fxs" -> { + setpointType = if(bus == "rio") talonFxsService.setpointType else talonFxsFDService.setpointType + units = if(bus == "rio") talonFxsService.activeUnits else talonFxsFDService.activeUnits + motionType = if(bus == "rio") talonFxsService.active_MM_type else talonFxsFDService.active_MM_type + differentialType = if(bus == "rio") talonFxsService.differentialType else talonFxsFDService.differentialType + followerType = if(bus == "rio") talonFxsService.activeFollowerType else talonFxsFDService.activeFollowerType + enableFOC = if(bus == "rio") talonFxsService.activeFOC else talonFxsFDService.activeFOC + overrideNeutral = if(bus == "rio") talonFxsService.activeOverrideNeutral else talonFxsFDService.activeOverrideNeutral + limFwdMotion = if(bus == "rio") talonFxsService.limFwdMotion else talonFxsFDService.limFwdMotion + limRevMotion = if(bus == "rio") talonFxsService.limRevMotion else talonFxsFDService.limRevMotion + velocity = if(bus == "rio") talonFxsService.activeVelocity else talonFxsFDService.activeVelocity + acceleration = if(bus == "rio") talonFxsService.activeAcceleration else talonFxsFDService.activeAcceleration + jerk = if(bus == "rio") talonFxsService.activeJerk else talonFxsFDService.activeJerk + feedFwd = if(bus == "rio") talonFxsService.activeFeedForward else talonFxsFDService.activeFeedForward + slot = if(bus == "rio") talonFxsService.activeSlotIndex else talonFxsFDService.activeSlotIndex + diffSlot = if(bus == "rio") talonFxsService.activeDifferentialSlot else talonFxsFDService.activeDifferentialSlot + diffPos = if(bus == "rio") talonFxsService.activeDifferentialTarget else talonFxsFDService.activeDifferentialTarget + torqueCurrentMaxOut = if(bus == "rio") talonFxsService.activeTorqueCurrentMaxOut else talonFxsFDService.activeTorqueCurrentMaxOut + torqueCurrentDeadband = if(bus == "rio") talonFxsService.activeTorqueCurrentDeadband else talonFxsFDService.activeTorqueCurrentDeadband + } + else -> throw IllegalArgumentException() + } + var controlRequest: ControlRequest = DutyCycleOut(0.0).withEnableFOC(false).withOverrideBrakeDurNeutral(false).withLimitForwardMotion(limFwdMotion).withLimitReverseMotion(limRevMotion) + //sanity checks if (units == Units.PERCENT && setpointType == SetpointType.OPEN_LOOP && !(-1.0..1.0).contains(setpoint)) { @@ -109,40 +156,88 @@ class P6RunCommand( when (setpointType) { SetpointType.OPEN_LOOP -> { when (units) { - Units.PERCENT -> controlRequest = DutyCycleOut(setpoint, enableFOC, overrideNeutral,limFwdMotion, limRevMotion) - Units.VOLTAGE -> controlRequest = VoltageOut(setpoint, enableFOC, overrideNeutral, limFwdMotion, limRevMotion) - Units.TORQUE_CURRENT -> controlRequest = TorqueCurrentFOC( - setpoint, - torqueCurrentMaxOut, - torqueCurrentDeadband, - overrideNeutral,limFwdMotion,limRevMotion - ) + Units.PERCENT -> controlRequest = DutyCycleOut(setpoint) + .withEnableFOC(enableFOC) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + Units.VOLTAGE -> controlRequest = VoltageOut(setpoint) + .withEnableFOC(enableFOC) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + Units.TORQUE_CURRENT -> controlRequest = TorqueCurrentFOC(setpoint) + .withMaxAbsDutyCycle(torqueCurrentMaxOut) + .withDeadband(torqueCurrentDeadband) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + .withOverrideCoastDurNeutral(overrideNeutral) } } SetpointType.POSITION -> { when (units) { Units.PERCENT -> controlRequest = - PositionDutyCycle(setpoint, velocity, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) + PositionDutyCycle(setpoint) + .withVelocity(velocity) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) Units.VOLTAGE -> controlRequest = - PositionVoltage(setpoint, velocity, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) + PositionVoltage(setpoint) + .withVelocity(velocity) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) Units.TORQUE_CURRENT -> controlRequest = - PositionTorqueCurrentFOC(setpoint, velocity, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) + PositionTorqueCurrentFOC(setpoint) + .withVelocity(velocity) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideCoastDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) } } SetpointType.VELOCITY -> { when (units) { Units.PERCENT -> controlRequest = - VelocityDutyCycle(setpoint, acceleration, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) + VelocityDutyCycle(setpoint) + .withAcceleration(acceleration) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) Units.VOLTAGE -> controlRequest = - VelocityVoltage(setpoint, acceleration, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) + VelocityVoltage(setpoint) + .withAcceleration(acceleration) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) Units.TORQUE_CURRENT -> controlRequest = - VelocityTorqueCurrentFOC(setpoint, acceleration, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) + VelocityTorqueCurrentFOC(setpoint) + .withAcceleration(acceleration) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideCoastDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) } } @@ -152,87 +247,112 @@ class P6RunCommand( logger.info { "Motion Magic: $motionType, $units" } when (units) { Units.PERCENT -> controlRequest = - MotionMagicDutyCycle(setpoint, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) + MotionMagicDutyCycle(setpoint) + .withEnableFOC(enableFOC) + .withSlot(slot) + .withFeedForward(feedFwd) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) Units.VOLTAGE -> controlRequest = - MotionMagicVoltage(setpoint, enableFOC, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) + MotionMagicVoltage(setpoint) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) Units.TORQUE_CURRENT -> controlRequest = - MotionMagicTorqueCurrentFOC(setpoint, feedFwd, slot, overrideNeutral,limFwdMotion,limRevMotion) + MotionMagicTorqueCurrentFOC(setpoint) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideCoastDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) } } MM_Type.VELOCITY -> { when (units) { - Units.PERCENT -> controlRequest = MotionMagicVelocityDutyCycle( - setpoint, - acceleration, - enableFOC, - feedFwd, - slot, - overrideNeutral,limFwdMotion,limRevMotion - ) - - Units.VOLTAGE -> controlRequest = MotionMagicVelocityVoltage( - setpoint, - acceleration, - enableFOC, - feedFwd, - slot, - overrideNeutral,limFwdMotion,limRevMotion - ) - - Units.TORQUE_CURRENT -> controlRequest = MotionMagicVelocityTorqueCurrentFOC( - setpoint, - acceleration, - enableFOC, - feedFwd, - slot, - overrideNeutral,limFwdMotion,limRevMotion - ) + Units.PERCENT -> controlRequest = MotionMagicVelocityDutyCycle(setpoint) + .withAcceleration(acceleration) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + + Units.VOLTAGE -> controlRequest = MotionMagicVelocityVoltage(setpoint) + .withAcceleration(acceleration) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + + Units.TORQUE_CURRENT -> controlRequest = MotionMagicVelocityTorqueCurrentFOC(setpoint) + .withAcceleration(acceleration) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideCoastDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) } } MM_Type.DYNAMIC -> { when (units) { - Units.PERCENT -> controlRequest = DynamicMotionMagicDutyCycle( - setpoint, - velocity, - acceleration, - jerk, - enableFOC, - feedFwd, - slot, - overrideNeutral,limFwdMotion,limRevMotion - ) - - Units.VOLTAGE -> controlRequest = DynamicMotionMagicVoltage( - setpoint, - velocity, - acceleration, - jerk, - enableFOC, - feedFwd, - slot, - overrideNeutral,limFwdMotion,limRevMotion - ) - - Units.TORQUE_CURRENT -> controlRequest = DynamicMotionMagicTorqueCurrentFOC( - setpoint, - velocity, - acceleration, - jerk, - feedFwd, - slot, - overrideNeutral,limFwdMotion,limRevMotion - ) + Units.PERCENT -> controlRequest = DynamicMotionMagicDutyCycle(setpoint, velocity, acceleration, jerk) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + + Units.VOLTAGE -> controlRequest = DynamicMotionMagicVoltage(setpoint, velocity, acceleration, jerk) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + + Units.TORQUE_CURRENT -> controlRequest = DynamicMotionMagicTorqueCurrentFOC(setpoint, velocity, acceleration, jerk) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideCoastDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) } } MM_Type.EXPONENTIAL -> { when(units) { - Units.PERCENT -> controlRequest = MotionMagicExpoDutyCycle(setpoint, enableFOC, feedFwd, slot, overrideNeutral, limFwdMotion, limRevMotion) - Units.VOLTAGE -> controlRequest = MotionMagicExpoVoltage(setpoint, enableFOC, feedFwd, slot, overrideNeutral, limFwdMotion, limRevMotion) - Units.TORQUE_CURRENT -> controlRequest = MotionMagicExpoTorqueCurrentFOC(setpoint, feedFwd, slot, overrideNeutral, limFwdMotion, limRevMotion) + Units.PERCENT -> controlRequest = MotionMagicExpoDutyCycle(setpoint) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + Units.VOLTAGE -> controlRequest = MotionMagicExpoVoltage(setpoint) + .withEnableFOC(enableFOC) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + Units.TORQUE_CURRENT -> controlRequest = MotionMagicExpoTorqueCurrentFOC(setpoint) + .withFeedForward(feedFwd) + .withSlot(slot) + .withOverrideCoastDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) } } } @@ -243,10 +363,20 @@ class P6RunCommand( DifferentialType.OPEN_LOOP -> { when (units) { Units.PERCENT -> controlRequest = - DifferentialDutyCycle(setpoint, diffPos, enableFOC, diffSlot, overrideNeutral,limFwdMotion,limRevMotion) + DifferentialDutyCycle(setpoint, diffPos) + .withEnableFOC(enableFOC) + .withDifferentialSlot(diffSlot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) Units.VOLTAGE -> controlRequest = - DifferentialVoltage(setpoint, diffPos, enableFOC, diffSlot, overrideNeutral,limFwdMotion,limRevMotion) + DifferentialVoltage(setpoint, diffPos) + .withEnableFOC(enableFOC) + .withDifferentialSlot(diffSlot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) else -> { terminal.warn("Units chosen not valid for this control mode") @@ -257,23 +387,21 @@ class P6RunCommand( DifferentialType.POSITION -> { when (units) { - Units.PERCENT -> controlRequest = DifferentialPositionDutyCycle( - setpoint, - diffPos, - enableFOC, - slot, - diffSlot, - overrideNeutral,limFwdMotion,limRevMotion - ) - - Units.VOLTAGE -> controlRequest = DifferentialPositionVoltage( - setpoint, - diffPos, - enableFOC, - slot, - diffSlot, - overrideNeutral,limFwdMotion,limRevMotion - ) + Units.PERCENT -> controlRequest = DifferentialPositionDutyCycle(setpoint, diffPos) + .withEnableFOC(enableFOC) + .withTargetSlot(slot) + .withDifferentialSlot(diffSlot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + + Units.VOLTAGE -> controlRequest = DifferentialPositionVoltage(setpoint, diffPos) + .withEnableFOC(enableFOC) + .withTargetSlot(slot) + .withDifferentialSlot(diffSlot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) else -> { terminal.warn("Units chosen not valid for this control mode") @@ -284,23 +412,21 @@ class P6RunCommand( DifferentialType.VELOCITY -> { when (units) { - Units.PERCENT -> controlRequest = DifferentialVelocityDutyCycle( - setpoint, - diffPos, - enableFOC, - slot, - diffSlot, - overrideNeutral,limFwdMotion,limRevMotion - ) - - Units.VOLTAGE -> controlRequest = DifferentialVelocityVoltage( - setpoint, - diffPos, - enableFOC, - slot, - diffSlot, - overrideNeutral,limFwdMotion,limRevMotion - ) + Units.PERCENT -> controlRequest = DifferentialVelocityDutyCycle(setpoint, diffPos) + .withEnableFOC(enableFOC) + .withTargetSlot(slot) + .withDifferentialSlot(diffSlot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + + Units.VOLTAGE -> controlRequest = DifferentialVelocityVoltage(setpoint, diffPos) + .withEnableFOC(enableFOC) + .withTargetSlot(slot) + .withDifferentialSlot(diffSlot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) else -> { terminal.warn("Units chosen not valid for this control mode") @@ -311,23 +437,21 @@ class P6RunCommand( DifferentialType.MOTION_MAGIC -> { when (units) { - Units.PERCENT -> controlRequest = DifferentialMotionMagicDutyCycle( - setpoint, - diffPos, - enableFOC, - slot, - diffSlot, - overrideNeutral,limFwdMotion,limRevMotion - ) - - Units.VOLTAGE -> controlRequest = DifferentialMotionMagicVoltage( - setpoint, - diffPos, - enableFOC, - slot, - diffSlot, - overrideNeutral,limFwdMotion,limRevMotion - ) + Units.PERCENT -> controlRequest = DifferentialMotionMagicDutyCycle(setpoint, diffPos) + .withEnableFOC(enableFOC) + .withTargetSlot(slot) + .withDifferentialSlot(diffSlot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) + + Units.VOLTAGE -> controlRequest = DifferentialMotionMagicVoltage(setpoint, diffPos) + .withEnableFOC(enableFOC) + .withTargetSlot(slot) + .withDifferentialSlot(diffSlot) + .withOverrideBrakeDurNeutral(overrideNeutral) + .withLimitForwardMotion(limFwdMotion) + .withLimitReverseMotion(limRevMotion) else -> { terminal.warn("Units chosen not valid for this control mode") @@ -372,28 +496,60 @@ class P6RunCommand( } //run Talon - if(bus == "rio") { - talonFxService.active.forEach { - logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" } - it.setControl(controlRequest) + when(device) { + "fx" -> { + if(bus == "rio") { + talonFxService.active.forEach { + logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" } + it.setControl(controlRequest) + } + } else if(bus == "canivore") { + talonFxFDService.active.forEach { + logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" } + it.setControl(controlRequest) + } + } else throw IllegalArgumentException() } - } else if(bus == "canivore") { - talonFxFDService.active.forEach { - logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" } - it.setControl(controlRequest) + "fxs" -> { + if(bus == "rio") { + talonFxsService.active.forEach { + logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" } + it.setControl(controlRequest) + } + } else if(bus == "canivore") { + talonFxsFDService.active.forEach { + logger.info { "Control Request: ${controlRequest.name}: ${controlRequest.controlInfo}" } + it.setControl(controlRequest) + } + } else throw IllegalArgumentException() } - } else throw IllegalArgumentException() + else -> throw IllegalArgumentException() + } + //Check Timeout if (duration > 0.0) { logger.debug { "run duration = $duration seconds" } Timer.delay(duration) logger.debug { "run duration expired, setting output = 0.0" } - if(bus == "rio") { - talonFxService.active.forEach { it.set(0.0) } - } else if(bus == "canivore") { - talonFxFDService.active.forEach { it.set(0.0) } - } else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") { + talonFxService.active.forEach { it.set(0.0) } + } else if(bus == "canivore") { + talonFxFDService.active.forEach { it.set(0.0) } + } else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") { + talonFxsService.active.forEach { it.set(0.0) } + } else if(bus == "canivore") { + talonFxsFDService.active.forEach { it.set(0.0) } + } else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } catch (e: Exception) { done = true diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectAdvancedHallSupportCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectAdvancedHallSupportCommand.kt new file mode 100644 index 0000000..22839b9 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectAdvancedHallSupportCommand.kt @@ -0,0 +1,48 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.AdvancedHallSupportValue +import com.ctre.phoenix6.signals.AdvancedHallSupportValue.* +import net.consensys.cava.toml.TomlTable +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService +import org.strykeforce.thirdcoast.device.TalonFxsService +import org.koin.core.component.inject + +private val SUPPORT = listOf( + Disabled, + Enabled +) + +private val LABELS = listOf( + "Disabled", + "(Pro) Enabled" +) +class P6SelectAdvancedHallSupportCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, SUPPORT, LABELS) { + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") + + override val activeIndex: Int + get() { + if(bus=="rio") return talonFxsService.activeConfiguration.Commutation.AdvancedHallSupport.ordinal + else return talonFxsFDService.activeConfiguration.Commutation.AdvancedHallSupport.ordinal + } + + override fun setActive(index: Int) { + val support = values[index] + if(bus=="rio") { + talonFxsService.activeConfiguration.Commutation.AdvancedHallSupport = support + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration.Commutation) } + } else { + talonFxsFDService.activeConfiguration.Commutation.AdvancedHallSupport = support + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration.Commutation) } + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectBrushedWiringCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectBrushedWiringCommand.kt new file mode 100644 index 0000000..0dda3e0 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectBrushedWiringCommand.kt @@ -0,0 +1,52 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.BrushedMotorWiringValue +import com.ctre.phoenix6.signals.BrushedMotorWiringValue.* +import net.consensys.cava.toml.TomlTable +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService +import org.strykeforce.thirdcoast.device.TalonFxsService +import org.koin.core.component.inject + +private val WIRING = listOf( + Leads_A_and_B, + Leads_A_and_C, + Leads_B_and_C +) + +private val LABELS = listOf( + "Leads A & B", + "Leads A & C", + "Leads B & C" +) +class P6SelectBrushedWiringCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, WIRING, LABELS) { + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") + + override val activeIndex: Int + get() { + if(bus=="rio") return talonFxsService.activeConfiguration.Commutation.BrushedMotorWiring.ordinal + else return talonFxsFDService.activeConfiguration.Commutation.BrushedMotorWiring.ordinal + } + + override fun setActive(index: Int) { + val wiring = values[index] + if(bus=="rio") { + talonFxsService.activeConfiguration.Commutation.BrushedMotorWiring = wiring + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration.Commutation) } + } else { + talonFxsFDService.activeConfiguration.Commutation.BrushedMotorWiring = wiring + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration.Commutation) } + } + } + + +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt index 022e668..3172c0c 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialSensorSourceCommand.kt @@ -6,25 +6,27 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService private val SENSOR = listOf( Disabled, - RemoteCANcoder, + RemoteTalonFX_Diff, + RemotePigeon2_Yaw, RemotePigeon2_Pitch, RemotePigeon2_Roll, - RemotePigeon2_Yaw, - RemoteTalonFX_Diff + RemoteCANcoder ) private val LABELS = listOf( "Disabled", - "Remote CANcoder", + "Remote TalonFX Differential", + "Remote Pigeon2 Yaw", "Remote Pigeon2 Pitch", "Remote Pigeon2 Roll", - "Remote Pigeon2 Yaw", - "Remote TalonFX Differential" + "Remote CANcoder" ) class P6SelectDifferentialSensorSourceCommand( @@ -35,25 +37,54 @@ class P6SelectDifferentialSensorSourceCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeConfiguration.DifferentialSensors.DifferentialSensorSource.ordinal - else if(bus == "canivoer") return talonFxFDService.activeConfiguration.DifferentialSensors.DifferentialSensorSource.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeConfiguration.DifferentialSensors.DifferentialSensorSource.ordinal + else if(bus == "canivoer") return talonFxFDService.activeConfiguration.DifferentialSensors.DifferentialSensorSource.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.activeConfiguration.DifferentialSensors.DifferentialSensorSource.ordinal + else if(bus == "canivoer") return talonFxsFDService.activeConfiguration.DifferentialSensors.DifferentialSensorSource.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val sensor = values[index] - if(bus == "rio") { - talonFxService.activeConfiguration.DifferentialSensors.DifferentialSensorSource = sensor - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } - } else if(bus == "canivoer") { - talonFxFDService.activeConfiguration.DifferentialSensors.DifferentialSensorSource = sensor - talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } + when(device) { + "fx" -> { + if(bus == "rio") { + talonFxService.activeConfiguration.DifferentialSensors.DifferentialSensorSource = sensor + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } + } else if(bus == "canivoer") { + talonFxFDService.activeConfiguration.DifferentialSensors.DifferentialSensorSource = sensor + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } + } + } + "fxs" -> { + if(bus == "rio") { + talonFxsService.activeConfiguration.DifferentialSensors.DifferentialSensorSource = sensor + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration) } + } else if(bus == "canivoer") { + talonFxsFDService.activeConfiguration.DifferentialSensors.DifferentialSensorSource = sensor + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration) } + } + } + else -> throw IllegalArgumentException() } + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt index 84bdd35..093b54b 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectDifferentialTypeCommand.kt @@ -4,10 +4,8 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.DifferentialType +import org.strykeforce.thirdcoast.device.* import org.strykeforce.thirdcoast.device.DifferentialType.* -import org.strykeforce.thirdcoast.device.TalonFxFDService -import org.strykeforce.thirdcoast.device.TalonFxService class P6SelectDifferentialTypeCommand( parent: Command?, @@ -19,23 +17,49 @@ class P6SelectDifferentialTypeCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.differentialType.ordinal - else if(bus == "canivore") return talonFxFDService.differentialType.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.differentialType.ordinal + else if(bus == "canivore") return talonFxFDService.differentialType.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.differentialType.ordinal + else if(bus == "canivore") return talonFxsFDService.differentialType.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val type = values[index] - if(bus == "rio") { - talonFxService.differentialType = type - } else if(bus == "canivore") { - talonFxFDService.differentialType = type - } else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") { + talonFxService.differentialType = type + } else if(bus == "canivore") { + talonFxFDService.differentialType = type + } else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") { + talonFxsService.differentialType = type + } else if(bus == "canivore") { + talonFxsFDService.differentialType = type + } else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectExternalFeedbackSourceCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectExternalFeedbackSourceCommand.kt new file mode 100644 index 0000000..e582e1f --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectExternalFeedbackSourceCommand.kt @@ -0,0 +1,78 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue.* +import net.consensys.cava.toml.TomlTable +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService +import org.strykeforce.thirdcoast.device.TalonFxsService +import org.koin.core.component.inject + +private val FEEDBACK = listOf( + Commutation, + RemoteCANcoder, + RemotePigeon2_Yaw, + RemotePigeon2_Pitch, + RemotePigeon2_Roll, + FusedCANcoder, + SyncCANcoder, + Quadrature, + PulseWidth, + RemoteCANdiPWM1, + RemoteCANdiPWM2, + RemoteCANdiQuadrature, + FusedCANdiPWM1, + FusedCANdiPWM2, + FusedCANdiQuadrature, + SyncCANdiPWM1, + SyncCANdiPWM2 +) + +private val LABELS = listOf( + "Commutation", + "Remote CANcoder", + "Remote Pigeon2 Yaw", + "Remote Pigeon2 Pitch", + "Remote Pigeon2 Roll", + "(Pro) Fused CANcoder", + "(Pro) Synced CANcoder", + "Quadrature", + "Pulse Width", + "Remote CANdi PWM1", + "Remote CANdi PWM2", + "Remote CANdi Quadrature", + "(Pro) Fused CANdi PWM1", + "(Pro) Fused CANdi PWM2", + "(Pro) Fused CANdi Quadrature", + "(Pro) Synced CANdi PWM1", + "(Pro) Synced CANdi PWM2" +) +class P6SelectExternalFeedbackSourceCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, FEEDBACK, LABELS) { + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") + + override val activeIndex: Int + get() { + if(bus=="rio") return talonFxsService.activeConfiguration.ExternalFeedback.ExternalFeedbackSensorSource.ordinal + else return talonFxsFDService.activeConfiguration.ExternalFeedback.ExternalFeedbackSensorSource.ordinal + } + + override fun setActive(index: Int) { + val source = values[index] + if(bus=="rio") { + talonFxsService.activeConfiguration.ExternalFeedback.ExternalFeedbackSensorSource = source + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration.ExternalFeedback) } + } else { + talonFxsFDService.activeConfiguration.ExternalFeedback.ExternalFeedbackSensorSource = source + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration.ExternalFeedback) } + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt index d3e7585..6ff84f3 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFeedbackSensorCommand.kt @@ -10,23 +10,39 @@ import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService private val SENSORS = listOf( - FusedCANcoder, + RotorSensor, RemoteCANcoder, + RemotePigeon2_Yaw, RemotePigeon2_Pitch, RemotePigeon2_Roll, - RemotePigeon2_Yaw, - RotorSensor, - SyncCANcoder + FusedCANcoder, + SyncCANcoder, + RemoteCANdiPWM1, + RemoteCANdiPWM2, + RemoteCANdiQuadrature, + FusedCANdiPWM1, + FusedCANdiPWM2, + FusedCANdiQuadrature, + SyncCANdiPWM1, + SyncCANdiPWM2 ) private val LABELS = listOf( - "(Pro) Fused CANcoder", + "Rotor Sensor", "Remote CANcoder", + "Remote Pigeon2 Yaw", "Remote Pigeon2 Pitch", "Remote Pigeon2 Roll", - "Remote Pigeon2 Yaw", - "Rotor Sensor", - "(Pro) Sync CANcoder" + "(Pro) Fused CANcoder", + "(Pro) Sync CANcoder", + "Remote CANdi PWM1", + "Remote CANdi PWM2", + "Remote CANdi Quadrature", + "(Pro) Fused CANdi PWM1", + "(Pro) Fused CANdi PWM2", + "(Pro) Fused CANdi Quadrature", + "(Pro) Sync CANdi PWM1", + "(Pro) Sync CANdi PWM2" ) class P6SelectFeedbackSensorCommand( diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt index 3b77ce6..e24fc99 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectFollowerTypeCommand.kt @@ -4,9 +4,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.FollowerType -import org.strykeforce.thirdcoast.device.TalonFxFDService -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.* class P6SelectFollowerTypeCommand( parent: Command?, @@ -18,22 +16,44 @@ class P6SelectFollowerTypeCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeFollowerType.ordinal - else if(bus == "canivore") return talonFxFDService.activeFollowerType.ordinal - else throw IllegalArgumentException() - + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeFollowerType.ordinal + else if(bus == "canivore") return talonFxFDService.activeFollowerType.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.activeFollowerType.ordinal + else if(bus == "canivore") return talonFxsFDService.activeFollowerType.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } } override fun setActive(index: Int) { val type = values[index] - if(bus == "rio") talonFxService.activeFollowerType = type - else if(bus == "canivore") talonFxFDService.activeFollowerType = type - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") talonFxService.activeFollowerType = type + else if(bus == "canivore") talonFxFDService.activeFollowerType = type + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") talonFxsService.activeFollowerType = type + else if(bus == "canivore") talonFxsFDService.activeFollowerType = type + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt index f01e424..c380438 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectGravityTypeCommand.kt @@ -6,8 +6,10 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService import org.strykeforce.thirdcoast.warn private val GRAVITY = listOf( @@ -28,60 +30,122 @@ class P6SelectGravityTypeCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") { - val slot = talonFxService.activeSlotIndex - when(slot){ - 0 -> return talonFxService.activeConfiguration.Slot0.GravityType.ordinal - 1 -> return talonFxService.activeConfiguration.Slot1.GravityType.ordinal - 2 -> return talonFxService.activeConfiguration.Slot2.GravityType.ordinal - else -> { - terminal.warn("slot value ${slot} is invalid") - return 2767 - } + when(device) { + "fx" -> { + if(bus == "rio") { + val slot = talonFxService.activeSlotIndex + when(slot){ + 0 -> return talonFxService.activeConfiguration.Slot0.GravityType.ordinal + 1 -> return talonFxService.activeConfiguration.Slot1.GravityType.ordinal + 2 -> return talonFxService.activeConfiguration.Slot2.GravityType.ordinal + else -> { + terminal.warn("slot value ${slot} is invalid") + return 2767 + } + } + } else if(bus == "canivore") { + val slot = talonFxFDService.activeSlotIndex + when(slot) { + 0 -> return talonFxFDService.activeConfiguration.Slot0.GravityType.ordinal + 1 -> return talonFxFDService.activeConfiguration.Slot1.GravityType.ordinal + 2 -> return talonFxFDService.activeConfiguration.Slot2.GravityType.ordinal + else -> { + terminal.warn("slot value ${slot} is invalid") + return 2767 + } + } + } else throw IllegalArgumentException() } - } else if(bus == "canivore") { - val slot = talonFxFDService.activeSlotIndex - when(slot) { - 0 -> return talonFxFDService.activeConfiguration.Slot0.GravityType.ordinal - 1 -> return talonFxFDService.activeConfiguration.Slot1.GravityType.ordinal - 2 -> return talonFxFDService.activeConfiguration.Slot2.GravityType.ordinal - else -> { - terminal.warn("slot value ${slot} is invalid") - return 2767 - } + "fxs" -> { + if(bus == "rio") { + val slot = talonFxsService.activeSlotIndex + when(slot){ + 0 -> return talonFxsService.activeConfiguration.Slot0.GravityType.ordinal + 1 -> return talonFxsService.activeConfiguration.Slot1.GravityType.ordinal + 2 -> return talonFxsService.activeConfiguration.Slot2.GravityType.ordinal + else -> { + terminal.warn("slot value ${slot} is invalid") + return 2767 + } + } + } else if(bus == "canivore") { + val slot = talonFxsFDService.activeSlotIndex + when(slot) { + 0 -> return talonFxsFDService.activeConfiguration.Slot0.GravityType.ordinal + 1 -> return talonFxsFDService.activeConfiguration.Slot1.GravityType.ordinal + 2 -> return talonFxsFDService.activeConfiguration.Slot2.GravityType.ordinal + else -> { + terminal.warn("slot value ${slot} is invalid") + return 2767 + } + } + } else throw IllegalArgumentException() } - } else throw IllegalArgumentException() + else -> throw IllegalArgumentException() + } } override fun setActive(index: Int) { val gravity = values[index] - if(bus == "rio") { - val slot = talonFxService.activeSlotIndex - when(slot) { - 0 -> talonFxService.activeConfiguration.Slot0.GravityType = gravity - 1 -> talonFxService.activeConfiguration.Slot1.GravityType = gravity - 2 -> talonFxService.activeConfiguration.Slot2.GravityType = gravity - else -> terminal.warn("slot value ${slot} is invalid") - } - talonFxService.active.forEach { - it.configurator.apply(talonFxService.activeConfiguration) + when(device) { + "fx" -> { + if(bus == "rio") { + val slot = talonFxService.activeSlotIndex + when(slot) { + 0 -> talonFxService.activeConfiguration.Slot0.GravityType = gravity + 1 -> talonFxService.activeConfiguration.Slot1.GravityType = gravity + 2 -> talonFxService.activeConfiguration.Slot2.GravityType = gravity + else -> terminal.warn("slot value ${slot} is invalid") + } + talonFxService.active.forEach { + it.configurator.apply(talonFxService.activeConfiguration) + } + } else if(bus == "canivore") { + val slot = talonFxFDService.activeSlotIndex + when(slot) { + 0 -> talonFxFDService.activeConfiguration.Slot0.GravityType = gravity + 1 -> talonFxFDService.activeConfiguration.Slot1.GravityType = gravity + 2 -> talonFxFDService.activeConfiguration.Slot2.GravityType = gravity + else -> terminal.warn("slot value ${slot} is invalid") + } + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } + } } - } else if(bus == "canivore") { - val slot = talonFxFDService.activeSlotIndex - when(slot) { - 0 -> talonFxFDService.activeConfiguration.Slot0.GravityType = gravity - 1 -> talonFxFDService.activeConfiguration.Slot1.GravityType = gravity - 2 -> talonFxFDService.activeConfiguration.Slot2.GravityType = gravity - else -> terminal.warn("slot value ${slot} is invalid") + "fxs" -> { + if(bus == "rio") { + val slot = talonFxsService.activeSlotIndex + when(slot) { + 0 -> talonFxsService.activeConfiguration.Slot0.GravityType = gravity + 1 -> talonFxsService.activeConfiguration.Slot1.GravityType = gravity + 2 -> talonFxsService.activeConfiguration.Slot2.GravityType = gravity + else -> terminal.warn("slot value ${slot} is invalid") + } + talonFxsService.active.forEach { + it.configurator.apply(talonFxsService.activeConfiguration) + } + } else if(bus == "canivore") { + val slot = talonFxsFDService.activeSlotIndex + when(slot) { + 0 -> talonFxsFDService.activeConfiguration.Slot0.GravityType = gravity + 1 -> talonFxsFDService.activeConfiguration.Slot1.GravityType = gravity + 2 -> talonFxsFDService.activeConfiguration.Slot2.GravityType = gravity + else -> terminal.warn("slot value ${slot} is invalid") + } + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration) } + } } - talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration) } + else -> throw IllegalArgumentException() } + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt index 3020fde..9abbba8 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectModeCommand.kt @@ -4,10 +4,8 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.SetpointType +import org.strykeforce.thirdcoast.device.* import org.strykeforce.thirdcoast.device.SetpointType.* -import org.strykeforce.thirdcoast.device.TalonFxFDService -import org.strykeforce.thirdcoast.device.TalonFxService class P6SelectModeCommand( parent: Command?, @@ -19,22 +17,47 @@ class P6SelectModeCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.setpointType.ordinal - if(bus == "canivore") return talonFxFDService.setpointType.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.setpointType.ordinal + if(bus == "canivore") return talonFxFDService.setpointType.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.setpointType.ordinal + if(bus == "canivore") return talonFxsFDService.setpointType.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val mode = values[index] - if(bus == "rio") talonFxService.setpointType = mode - else if(bus == "canivore") talonFxFDService.setpointType = mode - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") talonFxService.setpointType = mode + else if(bus == "canivore") talonFxFDService.setpointType = mode + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") talonFxsService.setpointType = mode + else if(bus == "canivore") talonFxsFDService.setpointType = mode + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt index 8a5c04b..4647b58 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotionMagicTypeCommand.kt @@ -4,9 +4,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.MM_Type -import org.strykeforce.thirdcoast.device.TalonFxFDService -import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.* class P6SelectMotionMagicTypeCommand( parent: Command?, @@ -18,21 +16,46 @@ class P6SelectMotionMagicTypeCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.active_MM_type.ordinal - else if(bus == "canivore") return talonFxFDService.active_MM_type.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.active_MM_type.ordinal + else if(bus == "canivore") return talonFxFDService.active_MM_type.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.active_MM_type.ordinal + else if(bus == "canivore") return talonFxsFDService.active_MM_type.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val type = values[index] - if(bus == "rio") talonFxService.active_MM_type = type - else if(bus == "canivore") talonFxFDService.active_MM_type = type - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") talonFxService.active_MM_type = type + else if(bus == "canivore") talonFxFDService.active_MM_type = type + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") talonFxsService.active_MM_type = type + else if(bus == "canivore") talonFxsFDService.active_MM_type = type + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorArrangementCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorArrangementCommand.kt new file mode 100644 index 0000000..94e611c --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorArrangementCommand.kt @@ -0,0 +1,56 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.MotorArrangementValue +import com.ctre.phoenix6.signals.MotorArrangementValue.* +import net.consensys.cava.toml.TomlTable +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService +import org.strykeforce.thirdcoast.device.TalonFxsService +import org.koin.core.component.inject + +private val MOTOR_TYPE = listOf( + Disabled, + Minion_JST, + Brushed_DC, + NEO_JST, + NEO550_JST, + VORTEX_JST +) + +private val LABELS = listOf( + "Disabled", + "Minion JST", + "Brushed DC", + "NEO JST", + "NEO 550 JST", + "Vortex JST" +) +class P6SelectMotorArrangementCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, MOTOR_TYPE, LABELS) { + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") + + override val activeIndex: Int + get() { + if(bus=="rio") return talonFxsService.activeConfiguration.Commutation.MotorArrangement.ordinal + else return talonFxsFDService.activeConfiguration.Commutation.MotorArrangement.ordinal + } + + override fun setActive(index: Int) { + val type = values[index] + if(bus=="rio") { + talonFxsService.activeConfiguration.Commutation.MotorArrangement = type + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration.Commutation) } + } else { + talonFxsFDService.activeConfiguration.Commutation.MotorArrangement = type + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration.Commutation) } + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt index 51904da..99363a5 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectMotorInvertCommand.kt @@ -6,8 +6,10 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService class P6SelectMotorInvertCommand( parent: Command?, @@ -19,26 +21,55 @@ class P6SelectMotorInvertCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeConfiguration.MotorOutput.Inverted.ordinal - else if(bus == "canivore") return talonFxFDService.activeConfiguration.MotorOutput.Inverted.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeConfiguration.MotorOutput.Inverted.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.MotorOutput.Inverted.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.activeConfiguration.MotorOutput.Inverted.ordinal + else if(bus == "canivore") return talonFxsFDService.activeConfiguration.MotorOutput.Inverted.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val invert = values[index] - if(bus == "rio") { - talonFxService.activeConfiguration.MotorOutput.Inverted = invert - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration.MotorOutput) } - } else if(bus == "canivore") { - talonFxFDService.activeConfiguration.MotorOutput.Inverted = invert - talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration.MotorOutput) } - } else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") { + talonFxService.activeConfiguration.MotorOutput.Inverted = invert + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration.MotorOutput) } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.MotorOutput.Inverted = invert + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration.MotorOutput) } + } else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") { + talonFxsService.activeConfiguration.MotorOutput.Inverted = invert + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration.MotorOutput) } + } else if(bus == "canivore") { + talonFxsFDService.activeConfiguration.MotorOutput.Inverted = invert + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration.MotorOutput) } + } else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt index 2885646..304155d 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralModeCommand.kt @@ -6,8 +6,10 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService private val MODE = listOf( @@ -28,25 +30,54 @@ class P6SelectNeutralModeCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeConfiguration.MotorOutput.NeutralMode.ordinal - else if(bus == "canivore") return talonFxFDService.activeConfiguration.MotorOutput.NeutralMode.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeConfiguration.MotorOutput.NeutralMode.ordinal + else if(bus == "canivore") return talonFxFDService.activeConfiguration.MotorOutput.NeutralMode.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.activeConfiguration.MotorOutput.NeutralMode.ordinal + else if(bus == "canivore") return talonFxsFDService.activeConfiguration.MotorOutput.NeutralMode.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val neutral = values[index] - if(bus == "rio") { - talonFxService.activeConfiguration.MotorOutput.NeutralMode = neutral - talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration) } - } else if(bus == "canivore") { - talonFxFDService.activeConfiguration.MotorOutput.NeutralMode = neutral - talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration.MotorOutput) } - } else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") { + talonFxService.activeConfiguration.MotorOutput.NeutralMode = neutral + talonFxService.active.forEach { it.configurator.apply(talonFxService.activeConfiguration.MotorOutput) } + } else if(bus == "canivore") { + talonFxFDService.activeConfiguration.MotorOutput.NeutralMode = neutral + talonFxFDService.active.forEach { it.configurator.apply(talonFxFDService.activeConfiguration.MotorOutput) } + } else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") { + talonFxsService.activeConfiguration.MotorOutput.NeutralMode = neutral + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration.MotorOutput) } + } else if(bus == "canivore") { + talonFxsFDService.activeConfiguration.MotorOutput.NeutralMode = neutral + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration.MotorOutput) } + } else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt index 3dbc6cd..d9a3eaf 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectNeutralOutputCommand.kt @@ -5,8 +5,10 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService class P6SelectNeutralOutputCommand( parent: Command?, @@ -18,21 +20,46 @@ class P6SelectNeutralOutputCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeNeutralOut.ordinal - else if(bus == "canivore") return talonFxFDService.activeNeutralOut.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeNeutralOut.ordinal + else if(bus == "canivore") return talonFxFDService.activeNeutralOut.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.activeNeutralOut.ordinal + else if(bus == "canivore") return talonFxsFDService.activeNeutralOut.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val neutral = values[index] - if(bus == "rio") talonFxService.activeNeutralOut = neutral - else if(bus == "canivore") talonFxFDService.activeNeutralOut = neutral - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") talonFxService.activeNeutralOut = neutral + else if(bus == "canivore") talonFxFDService.activeNeutralOut = neutral + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") talonFxsService.activeNeutralOut = neutral + else if(bus == "canivore") talonFxsFDService.activeNeutralOut = neutral + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSensorPhaseCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSensorPhaseCommand.kt new file mode 100644 index 0000000..1b942d2 --- /dev/null +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSensorPhaseCommand.kt @@ -0,0 +1,48 @@ +package org.strykeforce.thirdcoast.talon.phoenix6 + +import com.ctre.phoenix6.signals.SensorPhaseValue +import com.ctre.phoenix6.signals.SensorPhaseValue.* +import net.consensys.cava.toml.TomlTable +import org.strykeforce.thirdcoast.command.AbstractSelectCommand +import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService +import org.strykeforce.thirdcoast.device.TalonFxsService +import org.koin.core.component.inject + +private val PHASE = listOf( + Aligned, + Opposed +) + +private val LABELS = listOf( + "Aligned", + "Opposed" +) +class P6SelectSensorPhaseCommand( + parent: Command?, + key: String, + toml: TomlTable +): AbstractSelectCommand(parent, key, toml, PHASE, LABELS) { + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() + + val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") + + override val activeIndex: Int + get() { + if(bus=="rio") return talonFxsService.activeConfiguration.ExternalFeedback.SensorPhase.ordinal + else return talonFxsFDService.activeConfiguration.ExternalFeedback.SensorPhase.ordinal + } + + override fun setActive(index: Int) { + val phase = values[index] + if(bus=="rio") { + talonFxsService.activeConfiguration.ExternalFeedback.SensorPhase = phase + talonFxsService.active.forEach { it.configurator.apply(talonFxsService.activeConfiguration.ExternalFeedback) } + } else { + talonFxsFDService.activeConfiguration.ExternalFeedback.SensorPhase = phase + talonFxsFDService.active.forEach { it.configurator.apply(talonFxsFDService.activeConfiguration.ExternalFeedback) } + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt index 141171f..7c6bafc 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectSlotCommand.kt @@ -4,8 +4,10 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService private val SLOTS = listOf(0,1,2) @@ -17,21 +19,44 @@ class P6SelectSlotCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeSlotIndex - else if(bus == "canivore") return talonFxFDService.activeSlotIndex - else throw IllegalArgumentException() - + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeSlotIndex + else if(bus == "canivore") return talonFxFDService.activeSlotIndex + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") return talonFxsService.activeSlotIndex + else if(bus == "canivore") return talonFxsFDService.activeSlotIndex + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } } override fun setActive(index: Int) { - if(bus == "rio") talonFxService.activeSlotIndex = index - else if(bus == "canivore") talonFxFDService.activeSlotIndex = index - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") talonFxService.activeSlotIndex = index + else if(bus == "canivore") talonFxFDService.activeSlotIndex = index + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus == "rio") talonFxsService.activeSlotIndex = index + else if(bus == "canivore") talonFxsFDService.activeSlotIndex = index + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt index 2b2e0fb..3c796e2 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectTalonsCommand.kt @@ -1,14 +1,17 @@ package org.strykeforce.thirdcoast.talon.phoenix6 import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.hardware.TalonFXS import mu.KotlinLogging import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.prompt +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService import org.strykeforce.thirdcoast.info import org.strykeforce.thirdcoast.readIntList import org.strykeforce.thirdcoast.warn @@ -23,14 +26,27 @@ class P6SelectTalonsCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val menu: String get() = formatMenu( - if(bus == "rio") talonFxService.active.map(TalonFX::getDeviceID).joinToString() - else talonFxFDService.active.map(TalonFX::getDeviceID).joinToString() + when(device) { + "fx" -> { + if(bus == "rio") talonFxService.active.map(TalonFX::getDeviceID).joinToString() + else talonFxFDService.active.map(TalonFX::getDeviceID).joinToString() + } + "fxs" -> { + if(bus=="rio") talonFxsService.active.map(TalonFXS::getDeviceID).joinToString() + else talonFxsFDService.active.map(TalonFXS::getDeviceID).joinToString() + } + else -> throw IllegalArgumentException() + } + ) @@ -39,16 +55,32 @@ class P6SelectTalonsCommand( try { var ids: List var new: Set + when(device) { + "fx" -> { + if(bus == "rio") { + ids = reader.readIntList(this.prompt("ids"), talonFxService.active.map(TalonFX::getDeviceID)) + logger.info("IDS: ${ids}") + new = talonFxService.activate(ids) + } else if(bus == "canivore") { + ids = reader.readIntList(this.prompt("ids"), talonFxFDService.active.map(TalonFX::getDeviceID)) + logger.info("IDS: ${ids}") + new = talonFxFDService.activate(ids) + } else throw IllegalArgumentException() + } + "fxs" -> { + if(bus=="rio") { + ids = reader.readIntList(this.prompt("ids"), talonFxsService.active.map(TalonFXS::getDeviceID)) + logger.info("IDS: ${ids}") + new = talonFxsService.activate(ids) + } else if(bus=="canivore") { + ids = reader.readIntList(this.prompt("ids"), talonFxsFDService.active.map(TalonFXS::getDeviceID)) + logger.info("IDS: ${ids}") + new = talonFxsFDService.activate(ids) + } else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } - if(bus == "rio") { - ids = reader.readIntList(this.prompt("ids"), talonFxService.active.map(TalonFX::getDeviceID)) - logger.info("IDS: ${ids}") - new = talonFxService.activate(ids) - } else if(bus == "canivore") { - ids = reader.readIntList(this.prompt("ids"), talonFxFDService.active.map(TalonFX::getDeviceID)) - logger.info("IDS: ${ids}") - new = talonFxFDService.activate(ids) - } else throw IllegalArgumentException() // ids = reader.readIntList(this.prompt("ids"), talonFxService.active.map(TalonFX::getDeviceID)) // diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt index f742c6f..f6b8336 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6SelectUnitCommand.kt @@ -4,9 +4,7 @@ import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractSelectCommand import org.strykeforce.thirdcoast.command.Command -import org.strykeforce.thirdcoast.device.TalonFxFDService -import org.strykeforce.thirdcoast.device.TalonFxService -import org.strykeforce.thirdcoast.device.Units +import org.strykeforce.thirdcoast.device.* class P6SelectUnitCommand( parent: Command?, @@ -18,22 +16,47 @@ class P6SelectUnitCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override val activeIndex: Int get() { - if(bus == "rio") return talonFxService.activeUnits.ordinal - else if(bus == "canivore") return talonFxFDService.activeUnits.ordinal - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") return talonFxService.activeUnits.ordinal + else if(bus == "canivore") return talonFxFDService.activeUnits.ordinal + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus =="rio") return talonFxsService.activeUnits.ordinal + else if(bus=="canivore") return talonFxsFDService.activeUnits.ordinal + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } override fun setActive(index: Int) { val units = values[index] - if(bus == "rio") talonFxService.activeUnits = units - else if(bus == "canivore") talonFxFDService.activeUnits = units - else throw IllegalArgumentException() + when(device) { + "fx" -> { + if(bus == "rio") talonFxService.activeUnits = units + else if(bus == "canivore") talonFxFDService.activeUnits = units + else throw IllegalArgumentException() + } + "fxs" -> { + if(bus=="rio") talonFxsService.activeUnits = units + else if(bus=="canivore") talonFxsFDService.activeUnits = units + else throw IllegalArgumentException() + } + else -> throw IllegalArgumentException() + } + } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt index 8709b82..fa3067f 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/P6TalonStatusCommand.kt @@ -1,12 +1,15 @@ package org.strykeforce.thirdcoast.talon.phoenix6 import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.configs.TalonFXSConfiguration import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService class P6TalonStatusCommand( parent: Command?, @@ -16,25 +19,45 @@ class P6TalonStatusCommand( private val talonFxService: TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService: TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") override fun execute(): Command { val writer = terminal.writer() - if(bus == "rio") { - talonFxService.active.forEach { - var config = TalonFXConfiguration() - it.configurator.refresh(config) - writer.println(config.toString()) - } - }else if(bus == "canivore") { - talonFxFDService.active.forEach { - var config = TalonFXConfiguration() - it.configurator.refresh(config) - writer.println(config.toString()) - } - }else throw IllegalArgumentException() + if(device == "fx") { + if(bus == "rio") { + talonFxService.active.forEach { + var config = TalonFXConfiguration() + it.configurator.refresh(config) + writer.println(config.toString()) + } + }else if(bus == "canivore") { + talonFxFDService.active.forEach { + var config = TalonFXConfiguration() + it.configurator.refresh(config) + writer.println(config.toString()) + } + }else throw IllegalArgumentException() + } else if(device == "fxs") { + if(bus == "rio") { + talonFxsService.active.forEach { + var config = TalonFXSConfiguration() + it.configurator.refresh(config) + writer.println(config.toString()) + } + }else if(bus == "canivore") { + talonFxsFDService.active.forEach { + var config = TalonFXSConfiguration() + it.configurator.refresh(config) + writer.println(config.toString()) + } + }else throw IllegalArgumentException() + } + return super.execute() } } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt index 3e61dfc..0b5af41 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6Parameter.kt @@ -38,8 +38,8 @@ class Phoenix6Parameter(command: Command, toml: TomlTable, val enum: P6Enum): Ab STATOR_LIM_EN, STATOR_LIM, SUPP_LIM_EN, + SUPP_LOWER_LIM, SUPP_LIM, - SUPP_TRIP_THRES, SUPP_TRIP_TIME, CLOSED_LOOP_RAMP_DC, @@ -91,16 +91,21 @@ class Phoenix6Parameter(command: Command, toml: TomlTable, val enum: P6Enum): Ab OVERRIDE_NEUTRAL, LIM_FWD_MOT, LIM_REV_MOT, - GRAPHER_FRAME + GRAPHER_FRAME, + ABS_SENSOR_DISCONTINUITY, + ABS_SENSOR_OFFSET, + QUAD_EDGE_PER_ROT, + VELOCITY_FILTER_TIME_CONST } companion object { private val tomlTable by lazy { parseResource("/phoenix6.toml") } - fun create(command: Command, param: String): Phoenix6Parameter { - logger.info { "Creating P6 Param: ${param}: ${P6Enum.valueOf(param)}" } + fun create(command: Command, param: String, bus: String, device: String): Phoenix6Parameter { + logger.info { "Creating P6 Param: ${param}: ${P6Enum.valueOf(param)}, ${bus}, ${device}" } val toml = tomlTable.getTable(param) ?: throw java.lang.IllegalArgumentException("missing param: $param") + logger.info { "Finished creating: ${param}" } return Phoenix6Parameter(command, toml, P6Enum.valueOf(param)) } } diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt index 54be013..9371843 100644 --- a/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt +++ b/src/main/kotlin/org/strykeforce/thirdcoast/talon/phoenix6/Phoenix6ParameterCommand.kt @@ -1,15 +1,20 @@ package org.strykeforce.thirdcoast.talon.phoenix6 import com.ctre.phoenix6.configs.SlotConfigs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.configs.TalonFXSConfiguration import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.hardware.TalonFXS import mu.KotlinLogging import net.consensys.cava.toml.TomlTable import org.koin.core.component.inject import org.strykeforce.thirdcoast.command.AbstractCommand import org.strykeforce.thirdcoast.command.Command import org.strykeforce.thirdcoast.command.DOUBLE_FORMAT_4 +import org.strykeforce.thirdcoast.device.TalonFXsFDService import org.strykeforce.thirdcoast.device.TalonFxFDService import org.strykeforce.thirdcoast.device.TalonFxService +import org.strykeforce.thirdcoast.device.TalonFxsService import org.strykeforce.thirdcoast.talon.phoenix6.Phoenix6Parameter.P6Enum.* import org.strykeforce.thirdcoast.warn @@ -25,731 +30,1869 @@ class Phoenix6ParameterCommand( private val talonFxService : TalonFxService by inject() private val talonFxFDService: TalonFxFDService by inject() + private val talonFxsService : TalonFxsService by inject() + private val talonFxsFDService: TalonFXsFDService by inject() val bus = toml.getString(Command.BUS_KEY) ?: throw Exception("$key: ${Command.BUS_KEY} missing") - + val device = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") //val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing") private val timeout = talonFxService.timeout - private val param = Phoenix6Parameter.create(this, toml.getString("param") ?: "UNKNOWN") + private val param = Phoenix6Parameter.create(this, toml.getString("param") ?: "UNKNOWN", bus, device) override val menu: String get() { - val config = if(bus == "rio") talonFxService.activeConfiguration else talonFxFDService.activeConfiguration - val activeSlot = if(bus == "rio") talonFxService.activeSlotIndex else talonFxFDService.activeSlotIndex - logger.info { "Current Param: ${param.name}, ${param.prompt}" } + var fxConfig: TalonFXConfiguration = TalonFXConfiguration() + var activeSlot: Int = 0 + var fxsConfig: TalonFXSConfiguration = TalonFXSConfiguration() + if(device == "fx") { + fxConfig = if(bus == "rio") talonFxService.activeConfiguration else talonFxFDService.activeConfiguration + activeSlot = if(bus == "rio") talonFxService.activeSlotIndex else talonFxFDService.activeSlotIndex + logger.info { "Current Param: ${param.name}, ${param.prompt}" } + } else if (device == "fxs") { + fxsConfig = if(bus == "rio") talonFxsService.activeConfiguration else talonFxsFDService.activeConfiguration + activeSlot = if(bus == "rio") talonFxsService.activeSlotIndex else talonFxsFDService.activeSlotIndex + logger.info { "Current Param: ${param.name}, ${param.prompt}" } + } else throw IllegalArgumentException("Invalid device type") return when(param.enum) { - ROTOR_OFFSET -> formatMenu(config.Feedback.FeedbackRotorOffset, DOUBLE_FORMAT_4) - SENSOR_TO_MECH_RATIO -> formatMenu(config.Feedback.SensorToMechanismRatio, DOUBLE_FORMAT_4) - ROTOR_TO_SENSOR_RATIO -> formatMenu(config.Feedback.RotorToSensorRatio, DOUBLE_FORMAT_4) - REMOTE_SENSOR_ID -> formatMenu(config.Feedback.FeedbackRemoteSensorID) - - SLOT_KP -> formatMenu( - when(activeSlot) { - 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kP else talonFxFDService.activeConfiguration.Slot0.kP - 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kP else talonFxFDService.activeConfiguration.Slot1.kP - 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kP else talonFxFDService.activeConfiguration.Slot2.kP - else -> SlotConfigs().kP + ROTOR_OFFSET -> when(device) { + "fx" -> formatMenu(fxConfig.Feedback.FeedbackRotorOffset, DOUBLE_FORMAT_4) + else -> throw IllegalArgumentException() + } + SENSOR_TO_MECH_RATIO -> when(device) { + "fx" -> formatMenu(fxConfig.Feedback.SensorToMechanismRatio, DOUBLE_FORMAT_4) + "fxs" -> formatMenu(fxsConfig.ExternalFeedback.SensorToMechanismRatio, DOUBLE_FORMAT_4) + else -> throw IllegalArgumentException() + } + ROTOR_TO_SENSOR_RATIO -> when(device) { + "fx" -> formatMenu(fxConfig.Feedback.RotorToSensorRatio, DOUBLE_FORMAT_4) + "fxs" -> formatMenu(fxsConfig.ExternalFeedback.RotorToSensorRatio, DOUBLE_FORMAT_4) + else -> throw IllegalArgumentException() + } + REMOTE_SENSOR_ID -> when(device) { + "fx" -> formatMenu(fxConfig.Feedback.FeedbackRemoteSensorID) + "fxs" -> formatMenu(fxsConfig.ExternalFeedback.FeedbackRemoteSensorID) + else -> throw IllegalArgumentException() + } + SLOT_KP -> when(device) { + "fx" -> formatMenu( + when (activeSlot) { + 0 -> if (bus == "rio") talonFxService.activeConfiguration.Slot0.kP else talonFxFDService.activeConfiguration.Slot0.kP + 1 -> if (bus == "rio") talonFxService.activeConfiguration.Slot1.kP else talonFxFDService.activeConfiguration.Slot1.kP + 2 -> if (bus == "rio") talonFxService.activeConfiguration.Slot2.kP else talonFxFDService.activeConfiguration.Slot2.kP + else -> SlotConfigs().kP + } + ) + + "fxs" -> formatMenu( + when (activeSlot) { + 0 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot0.kP else talonFxsFDService.activeConfiguration.Slot0.kP + 1 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot1.kP else talonFxsFDService.activeConfiguration.Slot1.kP + 2 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot2.kP else talonFxsFDService.activeConfiguration.Slot2.kP + else -> SlotConfigs().kP + } + ) + else -> throw IllegalArgumentException() + } + SLOT_KI -> when(device){ + "fx" -> formatMenu( + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kI else talonFxFDService.activeConfiguration.Slot0.kI + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kI else talonFxFDService.activeConfiguration.Slot1.kI + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kI else talonFxFDService.activeConfiguration.Slot2.kI + else -> SlotConfigs().kI + } + ) + "fxs" -> formatMenu( + when (activeSlot) { + 1 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot1.kI else talonFxsFDService.activeConfiguration.Slot1.kI + 0 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot0.kI else talonFxsFDService.activeConfiguration.Slot0.kI + 2 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot2.kI else talonFxsFDService.activeConfiguration.Slot2.kI + else -> SlotConfigs().kI + } + ) + else -> throw IllegalArgumentException() + } + SLOT_KD -> when(device){ + "fx" -> formatMenu( + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kD else talonFxFDService.activeConfiguration.Slot0.kD + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kD else talonFxFDService.activeConfiguration.Slot1.kD + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kD else talonFxFDService.activeConfiguration.Slot2.kD + else -> SlotConfigs().kD + } + ) + "fxs" -> formatMenu( + when (activeSlot) { + 0 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot0.kD else talonFxsFDService.activeConfiguration.Slot0.kD + 1 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot1.kD else talonFxsFDService.activeConfiguration.Slot1.kD + 2 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot2.kD else talonFxsFDService.activeConfiguration.Slot2.kD + else -> SlotConfigs().kD + } + ) + else -> throw IllegalArgumentException() + } + SLOT_KS -> when(device){ + "fx" -> formatMenu( + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kS else talonFxFDService.activeConfiguration.Slot0.kS + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kS else talonFxFDService.activeConfiguration.Slot1.kS + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kS else talonFxFDService.activeConfiguration.Slot2.kS + else -> SlotConfigs().kS + } + ) + "fxs" -> formatMenu( + when (activeSlot) { + 0 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot0.kS else talonFxsFDService.activeConfiguration.Slot0.kS + 1 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot1.kS else talonFxsFDService.activeConfiguration.Slot1.kS + 2 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot2.kS else talonFxsFDService.activeConfiguration.Slot2.kS + else -> SlotConfigs().kS + } + ) + else -> throw IllegalArgumentException() + } + SLOT_KV -> when(device){ + "fx" -> formatMenu( + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kV else talonFxFDService.activeConfiguration.Slot0.kV + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kV else talonFxFDService.activeConfiguration.Slot1.kV + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kV else talonFxFDService.activeConfiguration.Slot2.kV + else -> SlotConfigs().kV + } + ) + "fxs" -> formatMenu( + when (activeSlot) { + 0 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot0.kV else talonFxsFDService.activeConfiguration.Slot0.kV + 1 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot1.kV else talonFxsFDService.activeConfiguration.Slot1.kV + 2 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot2.kV else talonFxsFDService.activeConfiguration.Slot2.kV + else -> SlotConfigs().kV + } + ) + else -> throw IllegalArgumentException() + } + SLOT_KA -> when(device){ + "fx" -> formatMenu( + when(activeSlot){ + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kA else talonFxFDService.activeConfiguration.Slot0.kA + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kA else talonFxFDService.activeConfiguration.Slot1.kA + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kA else talonFxFDService.activeConfiguration.Slot2.kA + else -> SlotConfigs().kA + } + ) + "fxs" -> formatMenu( + when (activeSlot) { + 0 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot0.kA else talonFxsFDService.activeConfiguration.Slot0.kA + 1 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot1.kA else talonFxsFDService.activeConfiguration.Slot1.kA + 2 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot2.kA else talonFxsFDService.activeConfiguration.Slot2.kA + else -> SlotConfigs().kA + } + ) + else -> throw IllegalArgumentException() + } + SLOT_KG -> when(device){ + "fx" -> formatMenu( + when(activeSlot) { + 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kG else talonFxFDService.activeConfiguration.Slot0.kG + 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kG else talonFxFDService.activeConfiguration.Slot1.kG + 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kG else talonFxFDService.activeConfiguration.Slot2.kG + else -> SlotConfigs().kG + } + ) + "fxs" -> formatMenu( + when (activeSlot) { + 0 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot0.kG else talonFxsFDService.activeConfiguration.Slot0.kG + 1 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot1.kG else talonFxsFDService.activeConfiguration.Slot1.kG + 2 -> if (bus == "rio") talonFxsService.activeConfiguration.Slot2.kG else talonFxsFDService.activeConfiguration.Slot2.kG + else -> SlotConfigs().kG + } + ) + else -> throw IllegalArgumentException() + } + + MM_CRUISE_VEL -> when(device){ + "fx" -> formatMenu(fxConfig.MotionMagic.MotionMagicCruiseVelocity) + "fxs" -> formatMenu(fxsConfig.MotionMagic.MotionMagicCruiseVelocity) + else -> throw IllegalArgumentException() + } + MM_ACCEL -> when(device){ + "fx" -> formatMenu(fxConfig.MotionMagic.MotionMagicAcceleration) + "fxs" -> formatMenu(fxsConfig.MotionMagic.MotionMagicAcceleration) + else -> throw IllegalArgumentException() + } + MM_JERK -> when(device){ + "fx" -> formatMenu(fxConfig.MotionMagic.MotionMagicJerk) + "fxs" -> formatMenu(fxsConfig.MotionMagic.MotionMagicJerk) + else -> throw IllegalArgumentException() + } + MM_EXPO_KA -> when(device){ + "fx" -> formatMenu(fxConfig.MotionMagic.MotionMagicExpo_kA) + "fxs" -> formatMenu(fxsConfig.MotionMagic.MotionMagicExpo_kA) + else -> throw IllegalArgumentException() + } + MM_EXPO_KV -> when(device){ + "fx" -> formatMenu(fxConfig.MotionMagic.MotionMagicExpo_kV) + "fxs" -> formatMenu(fxsConfig.MotionMagic.MotionMagicExpo_kV) + else -> throw IllegalArgumentException() + } + + PEAK_DIFF_DC -> when(device){ + "fx" -> formatMenu(fxConfig.DifferentialConstants.PeakDifferentialDutyCycle) + "fxs" -> formatMenu(fxsConfig.DifferentialConstants.PeakDifferentialDutyCycle) + else -> throw IllegalArgumentException() + } + PEAK_DIFF_VOLT -> when(device){ + "fx" -> formatMenu(fxConfig.DifferentialConstants.PeakDifferentialVoltage) + "fxs" -> formatMenu(fxsConfig.DifferentialConstants.PeakDifferentialVoltage) + else -> throw IllegalArgumentException() + } + PEAK_DIFF_TORQUE -> when(device){ + "fx" -> formatMenu(fxConfig.DifferentialConstants.PeakDifferentialTorqueCurrent) + "fxs" -> formatMenu(fxsConfig.DifferentialConstants.PeakDifferentialTorqueCurrent) + else -> throw IllegalArgumentException() + } + DIFF_SENSOR_REMOTE_ID -> when(device){ + "fx" -> formatMenu(fxConfig.DifferentialSensors.DifferentialRemoteSensorID) + "fxs" -> formatMenu(fxsConfig.DifferentialSensors.DifferentialRemoteSensorID) + else -> throw IllegalArgumentException() + } + DIFF_FX_ID -> when(device){ + "fx" -> formatMenu(fxConfig.DifferentialSensors.DifferentialTalonFXSensorID) + "fxs" -> formatMenu(fxsConfig.DifferentialSensors.DifferentialTalonFXSensorID) + else -> throw IllegalArgumentException() + } + + STATOR_LIM_EN -> when(device){ + "fx" -> formatMenu(fxConfig.CurrentLimits.StatorCurrentLimitEnable) + "fxs" -> formatMenu(fxsConfig.CurrentLimits.StatorCurrentLimitEnable) + else -> throw IllegalArgumentException() + } + STATOR_LIM -> when(device){ + "fx" -> formatMenu(fxConfig.CurrentLimits.StatorCurrentLimit) + "fxs" -> formatMenu(fxsConfig.CurrentLimits.StatorCurrentLimit) + else -> throw IllegalArgumentException() + } + SUPP_LIM_EN -> when(device){ + "fx" -> formatMenu(fxConfig.CurrentLimits.SupplyCurrentLimitEnable) + "fxs" -> formatMenu(fxsConfig.CurrentLimits.SupplyCurrentLimitEnable) + else -> throw IllegalArgumentException() + } + SUPP_LOWER_LIM -> when(device){ + "fx" -> formatMenu(fxConfig.CurrentLimits.SupplyCurrentLowerLimit) + "fxs" -> formatMenu(fxsConfig.CurrentLimits.SupplyCurrentLowerLimit) + else -> throw IllegalArgumentException() + } + SUPP_LIM -> when(device){ + "fx" -> formatMenu(fxConfig.CurrentLimits.SupplyCurrentLimit) + "fxs" -> formatMenu(fxsConfig.CurrentLimits.SupplyCurrentLimit) + else -> throw IllegalArgumentException() + } + SUPP_TRIP_TIME -> when(device){ + "fx" -> formatMenu(fxConfig.CurrentLimits.SupplyCurrentLowerTime) + "fxs" -> formatMenu(fxsConfig.CurrentLimits.SupplyCurrentLowerTime) + else -> throw IllegalArgumentException() + } + + CLOSED_LOOP_RAMP_DC -> when(device){ + "fx" -> formatMenu(fxConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod) + "fxs" -> formatMenu(fxsConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod) + else -> throw IllegalArgumentException() + } + PEAK_FWD_DC -> when(device){ + "fx" -> formatMenu(fxConfig.MotorOutput.PeakForwardDutyCycle) + "fxs" -> formatMenu(fxsConfig.MotorOutput.PeakForwardDutyCycle) + else -> throw IllegalArgumentException() + } + PEAK_REV_DC -> when(device){ + "fx" -> formatMenu(fxConfig.MotorOutput.PeakReverseDutyCycle) + "fxs" -> formatMenu(fxsConfig.MotorOutput.PeakReverseDutyCycle) + else -> throw IllegalArgumentException() + } + NEUTRAL_DEADBAND_DC -> when(device){ + "fx" -> formatMenu(fxConfig.MotorOutput.DutyCycleNeutralDeadband) + "fxs" -> formatMenu(fxsConfig.MotorOutput.DutyCycleNeutralDeadband) + else -> throw IllegalArgumentException() + } + OPEN_LOOP_RAMP_DC -> when(device){ + "fx" -> formatMenu(fxConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod) + "fxs" -> formatMenu(fxsConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod) + else -> throw IllegalArgumentException() + } + PEAK_FWD_V -> when(device){ + "fx" -> formatMenu(fxConfig.Voltage.PeakForwardVoltage) + "fxs" -> formatMenu(fxsConfig.Voltage.PeakForwardVoltage) + else -> throw IllegalArgumentException() + } + PEAK_REV_V -> when(device){ + "fx" -> formatMenu(fxConfig.Voltage.PeakReverseVoltage) + "fxs" -> formatMenu(fxsConfig.Voltage.PeakReverseVoltage) + else -> throw IllegalArgumentException() + } + SUPPLY_V_TIME_CONST -> when(device){ + "fx" -> formatMenu(fxConfig.Voltage.SupplyVoltageTimeConstant, DOUBLE_FORMAT_4) + "fxs" -> formatMenu(fxsConfig.Voltage.SupplyVoltageTimeConstant, DOUBLE_FORMAT_4) + else -> throw IllegalArgumentException() + } + OPEN_LOOP_RAMP_V -> when(device){ + "fx" -> formatMenu(fxConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod) + "fxs" -> formatMenu(fxsConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod) + else -> throw IllegalArgumentException() + } + CLOSED_LOOP_RAMP_V -> when(device){ + "fx" -> formatMenu(fxConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod) + "fxs" -> formatMenu(fxsConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod) + else -> throw IllegalArgumentException() + } + PEAK_FWD_I -> when(device){ + "fx" -> formatMenu(fxConfig.TorqueCurrent.PeakForwardTorqueCurrent) + else -> throw IllegalArgumentException() + } + PEAK_REV_I -> when(device){ + "fx" -> formatMenu(fxConfig.TorqueCurrent.PeakReverseTorqueCurrent) + else -> throw IllegalArgumentException() + } + TORQUE_NEUTRAL_DEADBAND -> when(device){ + "fx" -> formatMenu(fxConfig.TorqueCurrent.TorqueNeutralDeadband) + else -> throw IllegalArgumentException() + } + OPEN_LOOP_RAMP_I -> when(device){ + "fx" -> formatMenu(fxConfig.OpenLoopRamps.TorqueOpenLoopRampPeriod) + "fxs" -> formatMenu(fxsConfig.OpenLoopRamps.TorqueOpenLoopRampPeriod) + else -> throw IllegalArgumentException() + } + CLOSED_LOOP_RAMP_I -> when(device){ + "fx" -> formatMenu(fxConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod) + "fxs" -> formatMenu(fxsConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod) + else -> throw IllegalArgumentException() + } + + FWD_SOFT_EN -> when(device){ + "fx" -> formatMenu(fxConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable) + "fxs" -> formatMenu(fxsConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable) + else -> throw IllegalArgumentException() + } + FWD_SOFT_THRES -> when(device){ + "fx" -> formatMenu(fxConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold) + "fxs" -> formatMenu(fxsConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold) + else -> throw IllegalArgumentException() + } + REV_SOFT_EN -> when(device){ + "fx" -> formatMenu(fxConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable) + "fxs" -> formatMenu(fxsConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable) + else -> throw IllegalArgumentException() + } + REV_SOFT_THRES -> when(device){ + "fx" -> formatMenu(fxConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold) + "fxs" -> formatMenu(fxsConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold) + else -> throw IllegalArgumentException() + } + + FWD_HARD_EN -> when(device){ + "fx" -> formatMenu(fxConfig.HardwareLimitSwitch.ForwardLimitEnable) + "fxs" -> formatMenu(fxsConfig.HardwareLimitSwitch.ForwardLimitEnable) + else -> throw IllegalArgumentException() + } + FWD_REMOTE_ID -> when(device){ + "fx" -> formatMenu(fxConfig.HardwareLimitSwitch.ForwardLimitRemoteSensorID) + "fxs" -> formatMenu(fxsConfig.HardwareLimitSwitch.ForwardLimitRemoteSensorID) + else -> throw IllegalArgumentException() + } + FWD_AUTOSET_POS -> when(device){ + "fx" -> formatMenu(fxConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable) + "fxs" -> formatMenu(fxsConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable) + else -> throw IllegalArgumentException() + } + FWD_AUTOSET_POS_VALUE -> when(device){ + "fx" -> formatMenu(fxConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionValue) + "fxs" -> formatMenu(fxsConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionValue) + else -> throw IllegalArgumentException() + } + REV_HARD_EN -> when(device){ + "fx" -> formatMenu(fxConfig.HardwareLimitSwitch.ReverseLimitEnable) + "fxs" -> formatMenu(fxsConfig.HardwareLimitSwitch.ReverseLimitEnable) + else -> throw IllegalArgumentException() + } + REV_REMOTE_ID -> when(device){ + "fx" -> formatMenu(fxConfig.HardwareLimitSwitch.ReverseLimitRemoteSensorID) + "fxs" -> formatMenu(fxsConfig.HardwareLimitSwitch.ReverseLimitRemoteSensorID) + else -> throw IllegalArgumentException() + } + REV_AUTOSET_POS -> when(device){ + "fx" -> formatMenu(fxConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable) + "fxs" -> formatMenu(fxsConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable) + else -> throw IllegalArgumentException() + } + REV_AUTOSET_POS_VALUE -> when(device){ + "fx" -> formatMenu(fxConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionValue) + "fxs" -> formatMenu(fxsConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionValue) + else -> throw IllegalArgumentException() + } + + ALLOW_MUSIC_DIS -> when(device){ + "fx" -> formatMenu(fxConfig.Audio.AllowMusicDurDisable) + "fxs" -> formatMenu(fxsConfig.Audio.AllowMusicDurDisable) + else -> throw IllegalArgumentException() + } + BEEP_ON_BOOT -> when(device){ + "fx" -> formatMenu(fxConfig.Audio.BeepOnBoot) + "fxs" -> formatMenu(fxsConfig.Audio.BeepOnBoot) + else -> throw IllegalArgumentException() + } + BEEP_ON_CONFIG -> when(device){ + "fx" -> formatMenu(fxConfig.Audio.BeepOnConfig) + "fxs" -> formatMenu(fxsConfig.Audio.BeepOnConfig) + else -> throw IllegalArgumentException() + } + POSITION -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0 + else talonFxFDService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0, DOUBLE_FORMAT_4 + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0 + else talonFxsFDService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0, DOUBLE_FORMAT_4 + ) + else -> throw IllegalArgumentException() + } + VELOCITY -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeVelocity + else talonFxFDService.activeVelocity + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0 + else talonFxsFDService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0, DOUBLE_FORMAT_4 + ) + else -> throw IllegalArgumentException() + } + ACCELERATION -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeAcceleration + else talonFxFDService.activeAcceleration + ) + "fxs" -> formatMenu( + if(bus =="rio") talonFxsService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0 + else talonFxsFDService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0, DOUBLE_FORMAT_4 + ) + else -> throw IllegalArgumentException() + } + JERK -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeJerk + else talonFxFDService.activeJerk + ) + "fxs" -> formatMenu( + if(bus =="rio") talonFxsService.activeJerk + else talonFxsFDService.activeJerk + ) + else -> throw IllegalArgumentException() + } + TORQUE_CURRENT_DEADBAND -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeTorqueCurrentDeadband + else talonFxFDService.activeTorqueCurrentDeadband + ) + "fxs" -> formatMenu( + if(bus =="rio") talonFxsService.activeTorqueCurrentDeadband + else talonFxsFDService.activeTorqueCurrentDeadband + ) + else -> throw IllegalArgumentException() + } + TORQUE_CURRENT_MAX -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeTorqueCurrentMaxOut + else talonFxFDService.activeTorqueCurrentMaxOut + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeTorqueCurrentMaxOut + else talonFxsFDService.activeTorqueCurrentMaxOut + ) + else -> throw IllegalArgumentException() + } + FEED_FORWARD -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeFeedForward + else talonFxFDService.activeFeedForward + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeFeedForward + else talonFxsFDService.activeFeedForward + ) + else -> throw IllegalArgumentException() + } + OPPOSE_MAIN -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeOpposeMain + else talonFxFDService.activeOpposeMain + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeOpposeMain + else talonFxsFDService.activeOpposeMain + ) + else -> throw IllegalArgumentException() + } + DIFFERENTIAL_SLOT -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeDifferentialSlot + else talonFxFDService.activeDifferentialSlot + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeDifferentialSlot + else talonFxsFDService.activeDifferentialSlot + ) + else -> throw IllegalArgumentException() + } + DIFFERENTIAL_TARGET -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeDifferentialTarget + else talonFxFDService.activeDifferentialTarget + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeDifferentialTarget + else talonFxsFDService.activeDifferentialTarget + ) + else -> throw IllegalArgumentException() + } + FOC -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeFOC + else talonFxFDService.activeFOC + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeFOC + else talonFxsFDService.activeFOC + ) + else -> throw IllegalArgumentException() + } + OVERRIDE_NEUTRAL -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.activeOverrideNeutral + else talonFxFDService.activeOverrideNeutral + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeOverrideNeutral + else talonFxsFDService.activeOverrideNeutral + ) + else -> throw IllegalArgumentException() + } + LIM_FWD_MOT -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.limFwdMotion + else talonFxFDService.limFwdMotion + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.limFwdMotion + else talonFxsFDService.limFwdMotion + ) + else -> throw IllegalArgumentException() + } + LIM_REV_MOT -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.limRevMotion + else talonFxFDService.limRevMotion + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.limRevMotion + else talonFxsFDService.limRevMotion + ) + else -> throw IllegalArgumentException() + } + GRAPHER_FRAME -> when(device){ + "fx" -> formatMenu( + if(bus == "rio") talonFxService.grapherStatusFrameHz + else talonFxFDService.grapherStatusFrameHz + ) + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.grapherStatusFrameHz + else talonFxsFDService.grapherStatusFrameHz + ) + else -> throw IllegalArgumentException() + } + + ABS_SENSOR_DISCONTINUITY -> when(device) { + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeConfiguration.ExternalFeedback.AbsoluteSensorDiscontinuityPoint + else talonFxsFDService.activeConfiguration.ExternalFeedback.AbsoluteSensorDiscontinuityPoint + ) + else -> throw IllegalArgumentException() + } + ABS_SENSOR_OFFSET -> when(device) { + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeConfiguration.ExternalFeedback.AbsoluteSensorOffset + else talonFxsFDService.activeConfiguration.ExternalFeedback.AbsoluteSensorOffset + ) + else -> throw IllegalArgumentException() + } + QUAD_EDGE_PER_ROT -> when(device) { + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeConfiguration.ExternalFeedback.QuadratureEdgesPerRotation + else talonFxsFDService.activeConfiguration.ExternalFeedback.QuadratureEdgesPerRotation + ) + else -> throw IllegalArgumentException() + } + VELOCITY_FILTER_TIME_CONST -> when(device) { + "fxs" -> formatMenu( + if(bus=="rio") talonFxsService.activeConfiguration.ExternalFeedback.VelocityFilterTimeConstant + else talonFxsFDService.activeConfiguration.ExternalFeedback.VelocityFilterTimeConstant + ) + else -> throw IllegalArgumentException() + } + + else -> TODO("${param.enum} not implemented") + } + } + + override fun execute(): Command { + val timeout = talonFxService.timeout + + var fxConfig: TalonFXConfiguration = TalonFXConfiguration() + var activeSlotIndex: Int = 0 + var fxsConfig: TalonFXSConfiguration = TalonFXSConfiguration() + if(device == "fx") { + fxConfig = if(bus == "rio") talonFxService.activeConfiguration else talonFxFDService.activeConfiguration + activeSlotIndex = if(bus == "rio") talonFxService.activeSlotIndex else talonFxFDService.activeSlotIndex + logger.info { "Current Param: ${param.name}, ${param.prompt}" } + } else if (device == "fxs") { + fxsConfig = if(bus == "rio") talonFxsService.activeConfiguration else talonFxsFDService.activeConfiguration + activeSlotIndex = if(bus == "rio") talonFxsService.activeSlotIndex else talonFxsFDService.activeSlotIndex + logger.info { "Current Param: ${param.name}, ${param.prompt}" } + } else throw IllegalArgumentException("Invalid device type") + + when(param.enum) { + ROTOR_OFFSET -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.Feedback.FeedbackRotorOffset) { + talonFx, value -> + fxConfig.Feedback.FeedbackRotorOffset = value + talonFx.configurator.apply(fxConfig.Feedback) + } + else -> throw IllegalArgumentException() + } + SENSOR_TO_MECH_RATIO -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.Feedback.SensorToMechanismRatio) { + talonFx, value -> + fxConfig.Feedback.SensorToMechanismRatio = value + talonFx.configurator.apply(fxConfig.Feedback) + } + "fxs" -> configFXSDoubleParam(fxsConfig.ExternalFeedback.SensorToMechanismRatio) { talonFxs, value -> + fxsConfig.ExternalFeedback.SensorToMechanismRatio = value + talonFxs.configurator.apply(fxsConfig.ExternalFeedback) + } + else -> throw IllegalArgumentException() + } + ROTOR_TO_SENSOR_RATIO -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.Feedback.RotorToSensorRatio) { + talonFx, value -> + fxConfig.Feedback.RotorToSensorRatio = value + talonFx.configurator.apply(fxConfig.Feedback) + } + "fxs" -> configFXSDoubleParam(fxsConfig.ExternalFeedback.RotorToSensorRatio) { talonFxs, value -> + fxsConfig.ExternalFeedback.RotorToSensorRatio = value + talonFxs.configurator.apply(fxsConfig.ExternalFeedback) + } + else -> throw IllegalArgumentException() + } + REMOTE_SENSOR_ID -> when(device){ + "fx" -> configFXIntParam(fxConfig.Feedback.FeedbackRemoteSensorID) { + talonFx, value -> + fxConfig.Feedback.FeedbackRemoteSensorID = value + talonFx.configurator.apply(fxConfig.Feedback) + } + "fxs" -> configFXSIntParam(fxsConfig.ExternalFeedback.FeedbackRemoteSensorID) { talonFxs, value -> + fxsConfig.ExternalFeedback.FeedbackRemoteSensorID = value + talonFxs.configurator.apply(fxsConfig.ExternalFeedback) + } + else -> throw IllegalArgumentException() + } + + SLOT_KP -> when(device){ + "fx" -> { + when (activeSlotIndex) { + 0 -> configFXDoubleParam(fxConfig.Slot0.kP) { talonFx, value -> + fxConfig.Slot0.kP = value + talonFx.configurator.apply(fxConfig.Slot0) + } + 1 -> configFXDoubleParam(fxConfig.Slot1.kP) { talonFx, value -> + fxConfig.Slot1.kP = value + talonFx.configurator.apply(fxConfig.Slot1) + } + 2 -> configFXDoubleParam(fxConfig.Slot2.kP) { talonFx, value -> + fxConfig.Slot2.kP = value + talonFx.configurator.apply(fxConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") } - ) - SLOT_KI -> formatMenu( - when(activeSlot) { - 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kI else talonFxFDService.activeConfiguration.Slot0.kI - 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kI else talonFxFDService.activeConfiguration.Slot1.kI - 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kI else talonFxFDService.activeConfiguration.Slot2.kI - else -> SlotConfigs().kI + } + "fxs" -> { + when(activeSlotIndex) { + 0 -> configFXSDoubleParam(fxsConfig.Slot0.kP) { talonFXS, value -> + fxsConfig.Slot0.kP = value + talonFXS.configurator.apply(fxsConfig.Slot0) + } + 1 -> configFXSDoubleParam(fxsConfig.Slot1.kP) { talonFXS, value -> + fxsConfig.Slot1.kP = value + talonFXS.configurator.apply(fxsConfig.Slot1) + } + 2 -> configFXSDoubleParam(fxsConfig.Slot2.kP) { talonFXS, value -> + fxsConfig.Slot2.kP = value + talonFXS.configurator.apply(fxsConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") } - ) - SLOT_KD -> formatMenu( - when(activeSlot) { - 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kD else talonFxFDService.activeConfiguration.Slot0.kD - 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kD else talonFxFDService.activeConfiguration.Slot1.kD - 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kD else talonFxFDService.activeConfiguration.Slot2.kD - else -> SlotConfigs().kD + } + else -> throw IllegalArgumentException() + } + SLOT_KI -> when(device) { + "fx" -> { + when (activeSlotIndex) { + 0 -> configFXDoubleParam(fxConfig.Slot0.kI) { talonFx, value -> + fxConfig.Slot0.kI = value + talonFx.configurator.apply(fxConfig.Slot0) + } + 1 -> configFXDoubleParam(fxConfig.Slot1.kI) { talonFx, value -> + fxConfig.Slot1.kI = value + talonFx.configurator.apply(fxConfig.Slot1) + } + 2 -> configFXDoubleParam(fxConfig.Slot0.kI) { talonFx, value -> + fxConfig.Slot2.kI = value + talonFx.configurator.apply(fxConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") } - ) - SLOT_KS -> formatMenu( - when(activeSlot) { - 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kS else talonFxFDService.activeConfiguration.Slot0.kS - 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kS else talonFxFDService.activeConfiguration.Slot1.kS - 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kS else talonFxFDService.activeConfiguration.Slot2.kS - else -> SlotConfigs().kS + } + "fxs" -> { + when(activeSlotIndex) { + 0 -> configFXSDoubleParam(fxsConfig.Slot0.kI) { talonFXS, value -> + fxsConfig.Slot0.kI = value + talonFXS.configurator.apply(fxsConfig.Slot0) + } + 1 -> configFXSDoubleParam(fxsConfig.Slot1.kI) { talonFXS, value -> + fxsConfig.Slot1.kI = value + talonFXS.configurator.apply(fxsConfig.Slot1) + } + 2 -> configFXSDoubleParam(fxsConfig.Slot2.kI) { talonFXS, value -> + fxsConfig.Slot2.kI = value + talonFXS.configurator.apply(fxsConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") } - ) - SLOT_KV -> formatMenu( - when(activeSlot) { - 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kV else talonFxFDService.activeConfiguration.Slot0.kV - 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kV else talonFxFDService.activeConfiguration.Slot1.kV - 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kV else talonFxFDService.activeConfiguration.Slot2.kV - else -> SlotConfigs().kV + } + else -> throw IllegalArgumentException() + } + SLOT_KD -> when(device) { + "fx" -> { + when (activeSlotIndex) { + 0 -> configFXDoubleParam(fxConfig.Slot0.kD) { talonFx, value -> + fxConfig.Slot0.kD = value + talonFx.configurator.apply(fxConfig.Slot0) + } + 1 -> configFXDoubleParam(fxConfig.Slot1.kD) { talonFx, value -> + fxConfig.Slot1.kD = value + talonFx.configurator.apply(fxConfig.Slot1) + } + 2 -> configFXDoubleParam(fxConfig.Slot2.kD) { talonFx, value -> + fxConfig.Slot2.kD = value + talonFx.configurator.apply(fxConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") } - ) - SLOT_KA -> formatMenu( - when(activeSlot){ - 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kA else talonFxFDService.activeConfiguration.Slot0.kA - 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kA else talonFxFDService.activeConfiguration.Slot1.kA - 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kA else talonFxFDService.activeConfiguration.Slot2.kA - else -> SlotConfigs().kA + } + "fxs" -> { + when(activeSlotIndex) { + 0 -> configFXSDoubleParam(fxsConfig.Slot0.kD) { talonFXS, value -> + fxsConfig.Slot0.kD = value + talonFXS.configurator.apply(fxsConfig.Slot0) + } + 1 -> configFXSDoubleParam(fxsConfig.Slot1.kD) { talonFXS, value -> + fxsConfig.Slot1.kD = value + talonFXS.configurator.apply(fxsConfig.Slot1) + } + 2 -> configFXSDoubleParam(fxsConfig.Slot2.kD) { talonFXS, value -> + fxsConfig.Slot2.kD = value + talonFXS.configurator.apply(fxsConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") } - ) - SLOT_KG -> formatMenu( - when(activeSlot) { - 0 -> if(bus == "rio") talonFxService.activeConfiguration.Slot0.kG else talonFxFDService.activeConfiguration.Slot0.kG - 1 -> if(bus == "rio") talonFxService.activeConfiguration.Slot1.kG else talonFxFDService.activeConfiguration.Slot1.kG - 2 -> if(bus == "rio") talonFxService.activeConfiguration.Slot2.kG else talonFxFDService.activeConfiguration.Slot2.kG - else -> SlotConfigs().kG + } + else -> throw IllegalArgumentException() + } + SLOT_KS -> when(device) { + "fx" -> { + when (activeSlotIndex) { + 0 -> configFXDoubleParam(fxConfig.Slot0.kS) { talonFx, value -> + fxConfig.Slot0.kS = value + talonFx.configurator.apply(fxConfig.Slot0) + } + 1 -> configFXDoubleParam(fxConfig.Slot1.kS) { talonFx, value -> + fxConfig.Slot1.kS = value + talonFx.configurator.apply(fxConfig.Slot1) + } + 2 -> configFXDoubleParam(fxConfig.Slot2.kS) { talonFx, value -> + fxConfig.Slot2.kS = value + talonFx.configurator.apply(fxConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") } - ) - - MM_CRUISE_VEL -> formatMenu(config.MotionMagic.MotionMagicCruiseVelocity) - MM_ACCEL -> formatMenu(config.MotionMagic.MotionMagicAcceleration) - MM_JERK -> formatMenu(config.MotionMagic.MotionMagicJerk) - MM_EXPO_KA -> formatMenu(config.MotionMagic.MotionMagicExpo_kA) - MM_EXPO_KV -> formatMenu(config.MotionMagic.MotionMagicExpo_kV) - - PEAK_DIFF_DC -> formatMenu(config.DifferentialConstants.PeakDifferentialDutyCycle) - PEAK_DIFF_VOLT -> formatMenu(config.DifferentialConstants.PeakDifferentialVoltage) - PEAK_DIFF_TORQUE -> formatMenu(config.DifferentialConstants.PeakDifferentialTorqueCurrent) - DIFF_SENSOR_REMOTE_ID -> formatMenu(config.DifferentialSensors.DifferentialRemoteSensorID) - DIFF_FX_ID -> formatMenu(config.DifferentialSensors.DifferentialTalonFXSensorID) - - STATOR_LIM_EN -> formatMenu(config.CurrentLimits.StatorCurrentLimitEnable) - STATOR_LIM -> formatMenu(config.CurrentLimits.StatorCurrentLimit) - SUPP_LIM_EN -> formatMenu(config.CurrentLimits.SupplyCurrentLimitEnable) - SUPP_LIM -> formatMenu(config.CurrentLimits.SupplyCurrentLimit) - SUPP_TRIP_THRES -> formatMenu(config.CurrentLimits.SupplyCurrentThreshold) - SUPP_TRIP_TIME -> formatMenu(config.CurrentLimits.SupplyTimeThreshold) - - CLOSED_LOOP_RAMP_DC -> formatMenu(config.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod) - PEAK_FWD_DC -> formatMenu(config.MotorOutput.PeakForwardDutyCycle) - PEAK_REV_DC -> formatMenu(config.MotorOutput.PeakReverseDutyCycle) - NEUTRAL_DEADBAND_DC -> formatMenu(config.MotorOutput.DutyCycleNeutralDeadband) - OPEN_LOOP_RAMP_DC -> formatMenu(config.OpenLoopRamps.DutyCycleOpenLoopRampPeriod) - PEAK_FWD_V -> formatMenu(config.Voltage.PeakForwardVoltage) - PEAK_REV_V -> formatMenu(config.Voltage.PeakReverseVoltage) - SUPPLY_V_TIME_CONST -> formatMenu(config.Voltage.SupplyVoltageTimeConstant, DOUBLE_FORMAT_4) - OPEN_LOOP_RAMP_V -> formatMenu(config.OpenLoopRamps.VoltageOpenLoopRampPeriod) - CLOSED_LOOP_RAMP_V -> formatMenu(config.ClosedLoopRamps.VoltageClosedLoopRampPeriod) - PEAK_FWD_I -> formatMenu(config.TorqueCurrent.PeakForwardTorqueCurrent) - PEAK_REV_I -> formatMenu(config.TorqueCurrent.PeakReverseTorqueCurrent) - TORQUE_NEUTRAL_DEADBAND -> formatMenu(config.TorqueCurrent.TorqueNeutralDeadband) - OPEN_LOOP_RAMP_I -> formatMenu(config.OpenLoopRamps.TorqueOpenLoopRampPeriod) - CLOSED_LOOP_RAMP_I -> formatMenu(config.ClosedLoopRamps.TorqueClosedLoopRampPeriod) - - FWD_SOFT_EN -> formatMenu(config.SoftwareLimitSwitch.ForwardSoftLimitEnable) - FWD_SOFT_THRES -> formatMenu(config.SoftwareLimitSwitch.ForwardSoftLimitThreshold) - REV_SOFT_EN -> formatMenu(config.SoftwareLimitSwitch.ReverseSoftLimitEnable) - REV_SOFT_THRES -> formatMenu(config.SoftwareLimitSwitch.ReverseSoftLimitThreshold) - - FWD_HARD_EN -> formatMenu(config.HardwareLimitSwitch.ForwardLimitEnable) - FWD_REMOTE_ID -> formatMenu(config.HardwareLimitSwitch.ForwardLimitRemoteSensorID) - FWD_AUTOSET_POS -> formatMenu(config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable) - FWD_AUTOSET_POS_VALUE -> formatMenu(config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue) - REV_HARD_EN -> formatMenu(config.HardwareLimitSwitch.ReverseLimitEnable) - REV_REMOTE_ID -> formatMenu(config.HardwareLimitSwitch.ReverseLimitRemoteSensorID) - REV_AUTOSET_POS -> formatMenu(config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable) - REV_AUTOSET_POS_VALUE -> formatMenu(config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue) - - ALLOW_MUSIC_DIS -> formatMenu(config.Audio.AllowMusicDurDisable) - BEEP_ON_BOOT -> formatMenu(config.Audio.BeepOnBoot) - BEEP_ON_CONFIG -> formatMenu(config.Audio.BeepOnConfig) - POSITION -> formatMenu( - if(bus == "rio") talonFxService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0 - else talonFxFDService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0, DOUBLE_FORMAT_4 - ) - VELOCITY -> formatMenu( - if(bus == "rio") talonFxService.activeVelocity - else talonFxFDService.activeVelocity - ) - ACCELERATION -> formatMenu( - if(bus == "rio") talonFxService.activeAcceleration - else talonFxFDService.activeAcceleration - ) - JERK -> formatMenu( - if(bus == "rio") talonFxService.activeJerk - else talonFxFDService.activeJerk - ) - TORQUE_CURRENT_DEADBAND -> formatMenu( - if(bus == "rio") talonFxService.activeTorqueCurrentDeadband - else talonFxFDService.activeTorqueCurrentDeadband - ) - TORQUE_CURRENT_MAX -> formatMenu( - if(bus == "rio") talonFxService.activeTorqueCurrentMaxOut - else talonFxFDService.activeTorqueCurrentMaxOut - ) - FEED_FORWARD -> formatMenu( - if(bus == "rio") talonFxService.activeFeedForward - else talonFxFDService.activeFeedForward - ) - OPPOSE_MAIN -> formatMenu( - if(bus == "rio") talonFxService.activeOpposeMain - else talonFxFDService.activeOpposeMain - - ) - DIFFERENTIAL_SLOT -> formatMenu( - if(bus == "rio") talonFxService.activeDifferentialSlot - else talonFxFDService.activeDifferentialSlot - - ) - DIFFERENTIAL_TARGET -> formatMenu( - if(bus == "rio") talonFxService.activeDifferentialTarget - else talonFxFDService.activeDifferentialTarget - - ) - FOC -> formatMenu( - if(bus == "rio") talonFxService.activeFOC - else talonFxFDService.activeFOC - - ) - OVERRIDE_NEUTRAL -> formatMenu( - if(bus == "rio") talonFxService.activeOverrideNeutral - else talonFxFDService.activeOverrideNeutral - - ) - LIM_FWD_MOT -> formatMenu( - if(bus == "rio") talonFxService.limFwdMotion - else talonFxFDService.limFwdMotion - - ) - LIM_REV_MOT -> formatMenu( - if(bus == "rio") talonFxService.limRevMotion - else talonFxFDService.limRevMotion - - ) - GRAPHER_FRAME -> formatMenu( - if(bus == "rio") talonFxService.grapherStatusFrameHz - else talonFxFDService.grapherStatusFrameHz + } + "fxs" -> { + when(activeSlotIndex) { + 0 -> configFXSDoubleParam(fxsConfig.Slot0.kS) { talonFXS, value -> + fxsConfig.Slot0.kS = value + talonFXS.configurator.apply(fxsConfig.Slot0) + } + 1 -> configFXSDoubleParam(fxsConfig.Slot1.kS) { talonFXS, value -> + fxsConfig.Slot1.kS = value + talonFXS.configurator.apply(fxsConfig.Slot1) + } + 2 -> configFXSDoubleParam(fxsConfig.Slot2.kS) { talonFXS, value -> + fxsConfig.Slot2.kS = value + talonFXS.configurator.apply(fxsConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + else -> throw IllegalArgumentException() + } + SLOT_KV -> when(device) { + "fx" -> { + when (activeSlotIndex) { + 0 -> configFXDoubleParam(fxConfig.Slot0.kV) { talonFx, value -> + fxConfig.Slot0.kV = value + talonFx.configurator.apply(fxConfig.Slot0) + } + 1 -> configFXDoubleParam(fxConfig.Slot1.kV) { talonFx, value -> + fxConfig.Slot1.kV = value + talonFx.configurator.apply(fxConfig.Slot1) + } + 2 -> configFXDoubleParam(fxConfig.Slot2.kV) { talonFx, value -> + fxConfig.Slot2.kV = value + talonFx.configurator.apply(fxConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + "fxs" -> { + when(activeSlotIndex) { + 0 -> configFXSDoubleParam(fxsConfig.Slot0.kV) { talonFXS, value -> + fxsConfig.Slot0.kV = value + talonFXS.configurator.apply(fxsConfig.Slot0) + } + 1 -> configFXSDoubleParam(fxsConfig.Slot1.kV) { talonFXS, value -> + fxsConfig.Slot1.kV = value + talonFXS.configurator.apply(fxsConfig.Slot1) + } + 2 -> configFXSDoubleParam(fxsConfig.Slot2.kV) { talonFXS, value -> + fxsConfig.Slot2.kV = value + talonFXS.configurator.apply(fxsConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + else -> throw IllegalArgumentException() + } + SLOT_KA -> when(device) { + "fx" -> { + when (activeSlotIndex) { + 0 -> configFXDoubleParam(fxConfig.Slot0.kA) { talonFx, value -> + fxConfig.Slot0.kA = value + talonFx.configurator.apply(fxConfig.Slot0) + } + 1 -> configFXDoubleParam(fxConfig.Slot1.kA) { talonFx, value -> + fxConfig.Slot1.kA = value + talonFx.configurator.apply(fxConfig.Slot1) + } + 2 -> configFXDoubleParam(fxConfig.Slot2.kA) { talonFx, value -> + fxConfig.Slot2.kA = value + talonFx.configurator.apply(fxConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + "fxs" -> { + when(activeSlotIndex) { + 0 -> configFXSDoubleParam(fxsConfig.Slot0.kA) { talonFXS, value -> + fxsConfig.Slot0.kA = value + talonFXS.configurator.apply(fxsConfig.Slot0) + } + 1 -> configFXSDoubleParam(fxsConfig.Slot1.kA) { talonFXS, value -> + fxsConfig.Slot1.kA = value + talonFXS.configurator.apply(fxsConfig.Slot1) + } + 2 -> configFXSDoubleParam(fxsConfig.Slot2.kA) { talonFXS, value -> + fxsConfig.Slot2.kA = value + talonFXS.configurator.apply(fxsConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + else -> throw IllegalArgumentException() + } + SLOT_KG -> when(device) { + "fx" -> { + when (activeSlotIndex) { + 0 -> configFXDoubleParam(fxConfig.Slot0.kG) { talonFx, value -> + fxConfig.Slot0.kG = value + talonFx.configurator.apply(fxConfig.Slot0) + } + 1 -> configFXDoubleParam(fxConfig.Slot1.kG) { talonFx, value -> + fxConfig.Slot1.kG = value + talonFx.configurator.apply(fxConfig.Slot1) + } + 2 -> configFXDoubleParam(fxConfig.Slot2.kG) { talonFx, value -> + fxConfig.Slot2.kG = value + talonFx.configurator.apply(fxConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + "fxs" -> { + when(activeSlotIndex) { + 0 -> configFXSDoubleParam(fxsConfig.Slot0.kG) { talonFXS, value -> + fxsConfig.Slot0.kG = value + talonFXS.configurator.apply(fxsConfig.Slot0) + } + 1 -> configFXSDoubleParam(fxsConfig.Slot1.kG) { talonFXS, value -> + fxsConfig.Slot1.kG = value + talonFXS.configurator.apply(fxsConfig.Slot1) + } + 2 -> configFXSDoubleParam(fxsConfig.Slot2.kG) { talonFXS, value -> + fxsConfig.Slot2.kG = value + talonFXS.configurator.apply(fxsConfig.Slot2) + } + else -> TODO("$activeSlotIndex is not a valid slot") + } + } + else -> throw IllegalArgumentException() + } - ) + MM_ACCEL -> when(device) { + "fx" -> configFXDoubleParam(fxConfig.MotionMagic.MotionMagicAcceleration) { + talonFx, value -> + fxConfig.MotionMagic.MotionMagicAcceleration = value + talonFx.configurator.apply(fxConfig.MotionMagic) + } + "fxs" -> configFXSDoubleParam(fxsConfig.MotionMagic.MotionMagicAcceleration) { talonFXS, value -> + fxsConfig.MotionMagic.MotionMagicAcceleration = value + talonFXS.configurator.apply(fxsConfig.MotionMagic) + } + else -> throw IllegalArgumentException() + } + MM_CRUISE_VEL -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.MotionMagic.MotionMagicCruiseVelocity) { + talonFx, value -> + fxConfig.MotionMagic.MotionMagicCruiseVelocity = value + talonFx.configurator.apply(fxConfig.MotionMagic) + } + "fxs" -> configFXSDoubleParam(fxsConfig.MotionMagic.MotionMagicCruiseVelocity) { talonFXS, value -> + fxsConfig.MotionMagic.MotionMagicCruiseVelocity = value + talonFXS.configurator.apply(fxsConfig.MotionMagic) + } + else -> throw IllegalArgumentException() + } + MM_JERK -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.MotionMagic.MotionMagicJerk) { + talonFx, value -> + fxConfig.MotionMagic.MotionMagicJerk = value + talonFx.configurator.apply(fxConfig.MotionMagic) + } + "fxs" -> configFXSDoubleParam(fxsConfig.MotionMagic.MotionMagicJerk) { talonFXS, value -> + fxsConfig.MotionMagic.MotionMagicJerk = value + talonFXS.configurator.apply(fxsConfig.MotionMagic) + } + else -> throw IllegalArgumentException() + } + MM_EXPO_KA -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.MotionMagic.MotionMagicExpo_kA) { + talonFx, value -> + fxConfig.MotionMagic.MotionMagicExpo_kA = value + talonFx.configurator.apply(fxConfig.MotionMagic) + } + "fxs" -> configFXSDoubleParam(fxsConfig.MotionMagic.MotionMagicExpo_kA) { talonFXS, value -> + fxsConfig.MotionMagic.MotionMagicExpo_kA = value + talonFXS.configurator.apply(fxsConfig.MotionMagic) + } + else -> throw IllegalArgumentException() + } + MM_EXPO_KV -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.MotionMagic.MotionMagicExpo_kV) { + talonFx, value -> + fxConfig.MotionMagic.MotionMagicExpo_kV = value + talonFx.configurator.apply(fxConfig.MotionMagic) + } + "fxs" -> configFXSDoubleParam(fxsConfig.MotionMagic.MotionMagicExpo_kV) { talonFXS, value -> + fxsConfig.MotionMagic.MotionMagicExpo_kV = value + talonFXS.configurator.apply(fxsConfig.MotionMagic) + } + else -> throw IllegalArgumentException() + } - else -> TODO("${param.enum} not implemented") + PEAK_DIFF_DC -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.DifferentialConstants.PeakDifferentialDutyCycle) { + talonFx, value -> + fxConfig.DifferentialConstants.PeakDifferentialDutyCycle = value + talonFx.configurator.apply(fxConfig.DifferentialConstants) + } + "fxs" -> configFXSDoubleParam(fxsConfig.DifferentialConstants.PeakDifferentialDutyCycle) { talonFXS, value -> + fxsConfig.DifferentialConstants.PeakDifferentialDutyCycle = value + talonFXS.configurator.apply(fxsConfig.DifferentialConstants) + } + else -> throw IllegalArgumentException() + } + PEAK_DIFF_TORQUE -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.DifferentialConstants.PeakDifferentialTorqueCurrent) { + talonFx, value -> + fxConfig.DifferentialConstants.PeakDifferentialTorqueCurrent = value + talonFx.configurator.apply(fxConfig.DifferentialConstants) + } + "fxs" -> configFXSDoubleParam(fxsConfig.DifferentialConstants.PeakDifferentialTorqueCurrent) { talonFXS, value -> + fxsConfig.DifferentialConstants.PeakDifferentialTorqueCurrent = value + talonFXS.configurator.apply(fxsConfig.DifferentialConstants) + } + else -> throw IllegalArgumentException() + } + PEAK_DIFF_VOLT -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.DifferentialConstants.PeakDifferentialVoltage) { + talonFx, value -> + fxConfig.DifferentialConstants.PeakDifferentialVoltage = value + talonFx.configurator.apply(fxConfig.DifferentialConstants) + } + "fxs" -> configFXSDoubleParam(fxsConfig.DifferentialConstants.PeakDifferentialVoltage) { talonFXS, value -> + fxsConfig.DifferentialConstants.PeakDifferentialVoltage = value + talonFXS.configurator.apply(fxsConfig.DifferentialConstants) + } + else -> throw IllegalArgumentException() + } + DIFF_SENSOR_REMOTE_ID -> when(device){ + "fx" -> configFXIntParam(fxConfig.DifferentialSensors.DifferentialRemoteSensorID) { + talonFx, value -> + fxConfig.DifferentialSensors.DifferentialRemoteSensorID = value + talonFx.configurator.apply(fxConfig.DifferentialSensors) + } + "fxs" -> configFXSIntParam(fxsConfig.DifferentialSensors.DifferentialRemoteSensorID) { talonFXS, value -> + fxsConfig.DifferentialSensors.DifferentialRemoteSensorID = value + talonFXS.configurator.apply(fxsConfig.DifferentialSensors) + } + else -> throw IllegalArgumentException() + } + DIFF_FX_ID -> when(device){ + "fx" -> configFXIntParam(fxConfig.DifferentialSensors.DifferentialTalonFXSensorID) { + talonFx, value -> + fxConfig.DifferentialSensors.DifferentialTalonFXSensorID = value + talonFx.configurator.apply(fxConfig.DifferentialSensors) + } + "fxs" -> configFXSIntParam(fxsConfig.DifferentialSensors.DifferentialTalonFXSensorID) { talonFXS, value -> + fxsConfig.DifferentialSensors.DifferentialTalonFXSensorID = value + talonFXS.configurator.apply(fxsConfig.DifferentialSensors) + } + else -> throw IllegalArgumentException() } - } - override fun execute(): Command { - val config = if(bus == "rio") talonFxService.activeConfiguration else talonFxFDService.activeConfiguration - val activeSlotIndex = if(bus == "rio") talonFxService.activeSlotIndex else talonFxFDService.activeSlotIndex - val timeout = talonFxService.timeout + STATOR_LIM_EN -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.CurrentLimits.StatorCurrentLimitEnable){ + talonFx, value -> + fxConfig.CurrentLimits.StatorCurrentLimitEnable = value + talonFx.configurator.apply(fxConfig.CurrentLimits) + } + "fxs" -> configFXSBooleanParam(fxsConfig.CurrentLimits.StatorCurrentLimitEnable) { talonFXS, value -> + fxsConfig.CurrentLimits.StatorCurrentLimitEnable = value + talonFXS.configurator.apply(fxsConfig.CurrentLimits) + } + else -> throw IllegalArgumentException() + } + STATOR_LIM -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.CurrentLimits.StatorCurrentLimit) { + talonFx, value -> + fxConfig.CurrentLimits.StatorCurrentLimit = value + talonFx.configurator.apply(fxConfig.CurrentLimits) + } + "fxs" -> configFXSDoubleParam(fxsConfig.CurrentLimits.StatorCurrentLimit) { talonFXS, value -> + fxsConfig.CurrentLimits.StatorCurrentLimit = value + talonFXS.configurator.apply(fxsConfig.CurrentLimits) + } + else -> throw IllegalArgumentException() + } + SUPP_LIM_EN -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.CurrentLimits.SupplyCurrentLimitEnable) { + talonFx, value -> + fxConfig.CurrentLimits.SupplyCurrentLimitEnable = value + talonFx.configurator.apply(fxConfig.CurrentLimits) + } + "fxs" -> configFXSBooleanParam(fxsConfig.CurrentLimits.SupplyCurrentLimitEnable) { talonFXS, value -> + fxsConfig.CurrentLimits.SupplyCurrentLimitEnable = value + talonFXS.configurator.apply(fxsConfig.CurrentLimits) + } + else -> throw IllegalArgumentException() + } + SUPP_LOWER_LIM -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.CurrentLimits.SupplyCurrentLowerLimit) { + talonFx, value -> + fxConfig.CurrentLimits.SupplyCurrentLowerLimit = value + talonFx.configurator.apply(fxConfig.CurrentLimits) + } + "fxs" -> configFXSDoubleParam(fxsConfig.CurrentLimits.SupplyCurrentLowerLimit) { talonFXS, value -> + fxsConfig.CurrentLimits.SupplyCurrentLowerLimit = value + talonFXS.configurator.apply(fxsConfig.CurrentLimits) + } + else -> throw IllegalArgumentException() + } + SUPP_LIM -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.CurrentLimits.SupplyCurrentLimit) { + talonFx, value -> + fxConfig.CurrentLimits.SupplyCurrentLimit = value + talonFx.configurator.apply(fxConfig.CurrentLimits) + } + "fxs" -> configFXSDoubleParam(fxsConfig.CurrentLimits.SupplyCurrentLimit) { talonFXS, value -> + fxsConfig.CurrentLimits.SupplyCurrentLimit = value + talonFXS.configurator.apply(fxsConfig.CurrentLimits) + } + else -> throw IllegalArgumentException() + } + SUPP_TRIP_TIME -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.CurrentLimits.SupplyCurrentLowerTime) { + talonFx, value -> + fxConfig.CurrentLimits.SupplyCurrentLowerTime = value + talonFx.configurator.apply(fxConfig.CurrentLimits) + } + "fxs" -> configFXSDoubleParam(fxsConfig.CurrentLimits.SupplyCurrentLowerTime) { talonFXS, value -> + fxsConfig.CurrentLimits.SupplyCurrentLowerTime = value + talonFXS.configurator.apply(fxsConfig.CurrentLimits) + } + else -> throw IllegalArgumentException() + } - when(param.enum) { - ROTOR_OFFSET -> configDoubleParam(config.Feedback.FeedbackRotorOffset) { - talonFx, value -> - config.Feedback.FeedbackRotorOffset = value - talonFx.configurator.apply(config.Feedback) + CLOSED_LOOP_RAMP_DC -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod) { + talonFx, value -> + fxConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = value + talonFx.configurator.apply(fxConfig.ClosedLoopRamps) + } + "fxs" -> configFXSDoubleParam(fxsConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod) { talonFXS, value -> + fxsConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = value + talonFXS.configurator.apply(fxsConfig.ClosedLoopRamps) + } + else -> throw IllegalArgumentException() } - SENSOR_TO_MECH_RATIO -> configDoubleParam(config.Feedback.SensorToMechanismRatio) { - talonFx, value -> - config.Feedback.SensorToMechanismRatio = value - talonFx.configurator.apply(config.Feedback) + PEAK_FWD_DC -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.MotorOutput.PeakForwardDutyCycle) { + talonFx, value -> + fxConfig.MotorOutput.PeakForwardDutyCycle = value + talonFx.configurator.apply(fxConfig.MotorOutput) + } + "fxs" -> configFXSDoubleParam(fxsConfig.MotorOutput.PeakForwardDutyCycle) { talonFXS, value -> + fxsConfig.MotorOutput.PeakForwardDutyCycle = value + talonFXS.configurator.apply(fxsConfig.MotorOutput) + } + else -> throw IllegalArgumentException() } - ROTOR_TO_SENSOR_RATIO -> configDoubleParam(config.Feedback.RotorToSensorRatio) { - talonFx, value -> - config.Feedback.RotorToSensorRatio = value - talonFx.configurator.apply(config.Feedback) + PEAK_REV_DC -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.MotorOutput.PeakReverseDutyCycle) { + talonFx, value -> + fxConfig.MotorOutput.PeakReverseDutyCycle = value + talonFx.configurator.apply(fxConfig.MotorOutput) + } + "fxs" -> configFXSDoubleParam(fxsConfig.MotorOutput.PeakReverseDutyCycle) { talonFXS, value -> + fxsConfig.MotorOutput.PeakReverseDutyCycle = value + talonFXS.configurator.apply(fxsConfig.MotorOutput) + } + else -> throw IllegalArgumentException() } - REMOTE_SENSOR_ID -> configIntParam(config.Feedback.FeedbackRemoteSensorID) { - talonFx, value -> - config.Feedback.FeedbackRemoteSensorID = value - talonFx.configurator.apply(config.Feedback) + NEUTRAL_DEADBAND_DC -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.MotorOutput.DutyCycleNeutralDeadband) { + talonFx, value -> + fxConfig.MotorOutput.DutyCycleNeutralDeadband = value + talonFx.configurator.apply(fxConfig.MotorOutput) + } + "fxs" -> configFXSDoubleParam(fxsConfig.MotorOutput.DutyCycleNeutralDeadband) { talonFXS, value -> + fxsConfig.MotorOutput.DutyCycleNeutralDeadband = value + talonFXS.configurator.apply(fxsConfig.MotorOutput) + } + else -> throw IllegalArgumentException() + } + OPEN_LOOP_RAMP_DC -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod) { + talonFx, value -> + fxConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = value + talonFx.configurator.apply(fxConfig.OpenLoopRamps) + } + "fxs" -> configFXSDoubleParam(fxsConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod) { talonFXS, value -> + fxsConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = value + talonFXS.configurator.apply(fxsConfig.OpenLoopRamps) + } + else -> throw IllegalArgumentException() + } + PEAK_FWD_V -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.Voltage.PeakForwardVoltage) { + talonFx, value -> + fxConfig.Voltage.PeakForwardVoltage = value + talonFx.configurator.apply(fxConfig.Voltage) + } + "fxs" -> configFXSDoubleParam(fxsConfig.Voltage.PeakForwardVoltage) { talonFXS, value -> + fxsConfig.Voltage.PeakForwardVoltage = value + talonFXS.configurator.apply(fxsConfig.Voltage) + } + else -> throw IllegalArgumentException() + } + PEAK_REV_V -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.Voltage.PeakReverseVoltage) { + talonFx, value -> + fxConfig.Voltage.PeakReverseVoltage = value + talonFx.configurator.apply(fxConfig.Voltage) + } + "fxs" -> configFXSDoubleParam(fxsConfig.Voltage.PeakReverseVoltage) { talonFXS, value -> + fxsConfig.Voltage.PeakReverseVoltage = value + talonFXS.configurator.apply(fxsConfig.Voltage) + } + else -> throw IllegalArgumentException() + } + SUPPLY_V_TIME_CONST -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.Voltage.SupplyVoltageTimeConstant) { + talonFx, value -> + fxConfig.Voltage.SupplyVoltageTimeConstant = value + talonFx.configurator.apply(fxConfig.Voltage) + } + "fxs" -> configFXSDoubleParam(fxsConfig.Voltage.SupplyVoltageTimeConstant) { talonFXS, value -> + fxsConfig.Voltage.SupplyVoltageTimeConstant = value + talonFXS.configurator.apply(fxsConfig.Voltage) + } + else -> throw IllegalArgumentException() + } + OPEN_LOOP_RAMP_V -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod) { + talonFx, value -> + fxConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = value + talonFx.configurator.apply(fxConfig.OpenLoopRamps) + } + "fxs" -> configFXSDoubleParam(fxsConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod) { talonFXS, value -> + fxsConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = value + talonFXS.configurator.apply(fxsConfig.OpenLoopRamps) + } + else -> throw IllegalArgumentException() + } + CLOSED_LOOP_RAMP_V -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod) { + talonFx, value -> + fxConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = value + talonFx.configurator.apply(fxConfig.ClosedLoopRamps) + } + "fxs" -> configFXSDoubleParam(fxsConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod) { talonFXS, value -> + fxsConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = value + talonFXS.configurator.apply(fxsConfig.ClosedLoopRamps) + } + else -> throw IllegalArgumentException() + } + PEAK_FWD_I -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.TorqueCurrent.PeakForwardTorqueCurrent) { + talonFx, value -> + fxConfig.TorqueCurrent.PeakForwardTorqueCurrent = value + talonFx.configurator.apply(fxConfig.TorqueCurrent) + } + else -> throw IllegalArgumentException() + } + PEAK_REV_I -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.TorqueCurrent.PeakReverseTorqueCurrent) { + talonFx, value -> + fxConfig.TorqueCurrent.PeakReverseTorqueCurrent = value + talonFx.configurator.apply(fxConfig.TorqueCurrent) + } + else -> throw IllegalArgumentException() + } + TORQUE_NEUTRAL_DEADBAND -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.TorqueCurrent.TorqueNeutralDeadband) { + talonFx, value -> + fxConfig.TorqueCurrent.TorqueNeutralDeadband = value + talonFx.configurator.apply(fxConfig.TorqueCurrent) + } + else -> throw IllegalArgumentException() + } + OPEN_LOOP_RAMP_I -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.OpenLoopRamps.TorqueOpenLoopRampPeriod) { + talonFx, value -> + fxConfig.OpenLoopRamps.TorqueOpenLoopRampPeriod = value + talonFx.configurator.apply(fxConfig.OpenLoopRamps) + } + "fxs" -> configFXSDoubleParam(fxsConfig.OpenLoopRamps.TorqueOpenLoopRampPeriod) { talonFXS, value -> + fxsConfig.OpenLoopRamps.TorqueOpenLoopRampPeriod = value + talonFXS.configurator.apply(fxsConfig.OpenLoopRamps) + } + else -> throw IllegalArgumentException() + } + CLOSED_LOOP_RAMP_I -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod) { + talonFx, value -> + fxConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = value + talonFx.configurator.apply(fxConfig.ClosedLoopRamps) + } + "fxs" -> configFXSDoubleParam(fxsConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod) { talonFXS, value -> + fxsConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = value + talonFXS.configurator.apply(fxsConfig.ClosedLoopRamps) + } + else -> throw IllegalArgumentException() + } + CONTINUOUS_WRAP -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.ClosedLoopGeneral.ContinuousWrap){ + talonFx, value -> + fxConfig.ClosedLoopGeneral.ContinuousWrap = value + talonFx.configurator.apply(fxConfig.ClosedLoopGeneral) + } + "fxs" -> configFXSBooleanParam(fxsConfig.ClosedLoopGeneral.ContinuousWrap) { talonFXS, value -> + fxsConfig.ClosedLoopGeneral.ContinuousWrap = value + talonFXS.configurator.apply(fxsConfig.ClosedLoopGeneral) + } + else -> throw IllegalArgumentException() } - SLOT_KP -> { - when(activeSlotIndex) { - 0 -> configDoubleParam(config.Slot0.kP) { + FWD_SOFT_EN -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable) { talonFx, value -> - config.Slot0.kP = value - talonFx.configurator.apply(config.Slot0) - } - 1 -> configDoubleParam(config.Slot1.kP) { + fxConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = value + talonFx.configurator.apply(fxConfig.SoftwareLimitSwitch) + } + "fxs" -> configFXSBooleanParam(fxsConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable) { talonFXS, value -> + fxsConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = value + talonFXS.configurator.apply(fxsConfig.SoftwareLimitSwitch) + } + else -> throw IllegalArgumentException() + } + FWD_SOFT_THRES -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold) { talonFx, value -> - config.Slot1.kP = value - talonFx.configurator.apply(config.Slot1) - } - 2 -> configDoubleParam(config.Slot2.kP) { + fxConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = value + talonFx.configurator.apply(fxConfig.SoftwareLimitSwitch) + } + "fxs" -> configFXSDoubleParam(fxsConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold) { talonFXS, value -> + fxsConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = value + talonFXS.configurator.apply(fxsConfig.SoftwareLimitSwitch) + } + else -> throw IllegalArgumentException() + } + REV_SOFT_EN -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable) { talonFx, value -> - config.Slot2.kP = value - talonFx.configurator.apply(config.Slot2) - } - else -> TODO("$activeSlotIndex is not a valid slot") + fxConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = value + talonFx.configurator.apply(fxConfig.SoftwareLimitSwitch) + } + "fxs" -> configFXSBooleanParam(fxsConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable) { talonFXS, value -> + fxsConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = value + talonFXS.configurator.apply(fxsConfig.SoftwareLimitSwitch) } + else -> throw IllegalArgumentException() } - SLOT_KI -> { - when(activeSlotIndex) { - 0 -> configDoubleParam(config.Slot0.kI) { + REV_SOFT_THRES -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold) { talonFx, value -> - config.Slot0.kI = value - talonFx.configurator.apply(config.Slot0) - } - 1 -> configDoubleParam(config.Slot1.kI) { + fxConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = value + talonFx.configurator.apply(fxConfig.SoftwareLimitSwitch) + } + "fxs" -> configFXSDoubleParam(fxsConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold) { talonFXS, value -> + fxsConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = value + talonFXS.configurator.apply(fxsConfig.SoftwareLimitSwitch) + } + else -> throw IllegalArgumentException() + } + + FWD_HARD_EN -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.HardwareLimitSwitch.ForwardLimitEnable) { talonFx, value -> - config.Slot1.kI = value - talonFx.configurator.apply(config.Slot1) - } - 2 -> configDoubleParam(config.Slot0.kI) { + fxConfig.HardwareLimitSwitch.ForwardLimitEnable = value + talonFx.configurator.apply(fxConfig.HardwareLimitSwitch) + } + "fxs" -> configFXSBooleanParam(fxsConfig.HardwareLimitSwitch.ForwardLimitEnable) { talonFXS, value -> + fxsConfig.HardwareLimitSwitch.ForwardLimitEnable = value + talonFXS.configurator.apply(fxsConfig.HardwareLimitSwitch) + } + else -> throw IllegalArgumentException() + } + FWD_REMOTE_ID -> when(device){ + "fx" -> configFXIntParam(fxConfig.HardwareLimitSwitch.ForwardLimitRemoteSensorID) { talonFx, value -> - config.Slot2.kI = value - talonFx.configurator.apply(config.Slot2) - } - else -> TODO("$activeSlotIndex is not a valid slot") + fxConfig.HardwareLimitSwitch.ForwardLimitRemoteSensorID = value + talonFx.configurator.apply(fxConfig.HardwareLimitSwitch) } + "fxs" -> configFXSIntParam(fxsConfig.HardwareLimitSwitch.ForwardLimitRemoteSensorID) { talonFXS, value -> + fxsConfig.HardwareLimitSwitch.ForwardLimitRemoteSensorID = value + talonFXS.configurator.apply(fxsConfig.HardwareLimitSwitch) + } + else -> throw IllegalArgumentException() } - SLOT_KD -> { - when(activeSlotIndex) { - 0 -> configDoubleParam(config.Slot0.kD) { + FWD_AUTOSET_POS -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable) { talonFx, value -> - config.Slot0.kD = value - talonFx.configurator.apply(config.Slot0) - } - 1 -> configDoubleParam(config.Slot1.kD) { + fxConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable = value + talonFx.configurator.apply(fxConfig.HardwareLimitSwitch) + } + "fxs" -> configFXSBooleanParam(fxsConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable) { talonFXS, value -> + fxsConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable = value + talonFXS.configurator.apply(fxsConfig.HardwareLimitSwitch) + } + else -> throw IllegalArgumentException() + } + FWD_AUTOSET_POS_VALUE -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionValue) { talonFx, value -> - config.Slot1.kD = value - talonFx.configurator.apply(config.Slot1) - } - 2 -> configDoubleParam(config.Slot2.kD) { + fxConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = value + talonFx.configurator.apply(fxConfig.HardwareLimitSwitch) + } + "fxs" -> configFXSDoubleParam(fxsConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionValue) { talonFXS, value -> + fxsConfig.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = value + talonFXS.configurator.apply(fxsConfig.HardwareLimitSwitch) + } + else -> throw IllegalArgumentException() + } + REV_HARD_EN -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.HardwareLimitSwitch.ReverseLimitEnable) { talonFx, value -> - config.Slot2.kD = value - talonFx.configurator.apply(config.Slot2) - } - else -> TODO("$activeSlotIndex is not a valid slot") + fxConfig.HardwareLimitSwitch.ReverseLimitEnable = value + talonFx.configurator.apply(fxConfig.HardwareLimitSwitch) + } + "fxs" -> configFXSBooleanParam(fxsConfig.HardwareLimitSwitch.ReverseLimitEnable) { talonFXS, value -> + fxsConfig.HardwareLimitSwitch.ReverseLimitEnable = value + talonFXS.configurator.apply(fxsConfig.HardwareLimitSwitch) } + else -> throw IllegalArgumentException() } - SLOT_KS -> { - when(activeSlotIndex) { - 0 -> configDoubleParam(config.Slot0.kS) { - talonFx, value -> - config.Slot0.kS = value - talonFx.configurator.apply(config.Slot0) - } - 1 -> configDoubleParam(config.Slot1.kS) { - talonFx, value -> - config.Slot1.kS = value - talonFx.configurator.apply(config.Slot1) - } - 2 -> configDoubleParam(config.Slot2.kS) { - talonFx, value -> - config.Slot2.kS = value - talonFx.configurator.apply(config.Slot2) - } - else -> TODO("$activeSlotIndex is not a valid slot") + REV_REMOTE_ID -> when(device){ + "fx" -> configFXIntParam(fxConfig.HardwareLimitSwitch.ReverseLimitRemoteSensorID) { + talonFx, value -> + fxConfig.HardwareLimitSwitch.ReverseLimitRemoteSensorID = value + talonFx.configurator.apply(fxConfig.HardwareLimitSwitch) + } + "fxs" -> configFXSIntParam(fxsConfig.HardwareLimitSwitch.ReverseLimitRemoteSensorID) { talonFXS, value -> + fxsConfig.HardwareLimitSwitch.ReverseLimitRemoteSensorID = value + talonFXS.configurator.apply(fxsConfig.HardwareLimitSwitch) } + else -> throw IllegalArgumentException() } - SLOT_KV -> { - when(activeSlotIndex) { - 0 -> configDoubleParam(config.Slot0.kV) { - talonFx, value -> - config.Slot0.kV = value - talonFx.configurator.apply(config.Slot0) - } - 1 -> configDoubleParam(config.Slot1.kV) { - talonFx, value -> - config.Slot1.kV = value - talonFx.configurator.apply(config.Slot1) - } - 2 -> configDoubleParam(config.Slot2.kV) { - talonFx, value -> - config.Slot2.kV = value - talonFx.configurator.apply(config.Slot2) - } - else -> TODO("$activeSlotIndex is not a valid slot") + REV_AUTOSET_POS -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable) { + talonFx, value -> + fxConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = value + talonFx.configurator.apply(fxConfig.HardwareLimitSwitch) + } + "fxs" -> configFXSBooleanParam(fxsConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable) { talonFXS, value -> + fxsConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = value + talonFXS.configurator.apply(fxsConfig.HardwareLimitSwitch) } + else -> throw IllegalArgumentException() } - SLOT_KA -> { - when(activeSlotIndex) { - 0 -> configDoubleParam(config.Slot0.kA) { - talonFx, value -> - config.Slot0.kA = value - talonFx.configurator.apply(config.Slot0) - } - 1 -> configDoubleParam(config.Slot1.kA) { - talonFx, value -> - config.Slot1.kA = value - talonFx.configurator.apply(config.Slot1) - } - 2 -> configDoubleParam(config.Slot2.kA) { - talonFx, value -> - config.Slot2.kA = value - talonFx.configurator.apply(config.Slot2) - } - else -> TODO("$activeSlotIndex is not a valid slot") + REV_AUTOSET_POS_VALUE -> when(device){ + "fx" -> configFXDoubleParam(fxConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionValue) { + talonFx, value -> + fxConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = value + talonFx.configurator.apply(fxConfig.HardwareLimitSwitch) + } + "fxs" -> configFXSDoubleParam(fxsConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionValue) { talonFXS, value -> + fxsConfig.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = value + talonFXS.configurator.apply(fxsConfig.HardwareLimitSwitch) + } + else -> throw IllegalArgumentException() + } + + ALLOW_MUSIC_DIS -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.Audio.AllowMusicDurDisable) { + talonFx, value -> + fxConfig.Audio.AllowMusicDurDisable = value + talonFx.configurator.apply(fxConfig.Audio) + } + "fxs" -> configFXSBooleanParam(fxsConfig.Audio.AllowMusicDurDisable) { talonFXS, value -> + fxsConfig.Audio.AllowMusicDurDisable = value + talonFXS.configurator.apply(fxsConfig.Audio) } + else -> throw IllegalArgumentException() } - SLOT_KG -> { - when(activeSlotIndex) { - 0 -> configDoubleParam(config.Slot0.kG) { - talonFx, value -> - config.Slot0.kG = value - talonFx.configurator.apply(config.Slot0) + BEEP_ON_BOOT -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.Audio.BeepOnBoot) { + talonFx, value -> + fxConfig.Audio.BeepOnBoot = value + talonFx.configurator.apply(fxConfig.Audio) + } + "fxs" -> configFXSBooleanParam(fxsConfig.Audio.BeepOnBoot) { talonFXS, value -> + fxsConfig.Audio.BeepOnBoot = value + talonFXS.configurator.apply(fxsConfig.Audio) + } + else -> throw IllegalArgumentException() + } + BEEP_ON_CONFIG -> when(device){ + "fx" -> configFXBooleanParam(fxConfig.Audio.BeepOnConfig) { + talonFx, value -> + fxConfig.Audio.BeepOnConfig = value + talonFx.configurator.apply(fxConfig.Audio) + } + "fxs" -> configFXSBooleanParam(fxsConfig.Audio.BeepOnConfig) { talonFXS, value -> + fxsConfig.Audio.BeepOnConfig = value + talonFXS.configurator.apply(fxsConfig.Audio) + } + else -> throw IllegalArgumentException() + } + POSITION -> when(device){ + "fx" -> configFXDoubleParam( + if(bus=="rio") talonFxService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0 + else talonFxFDService.active.firstOrNull()?.position?.valueAsDouble?: 2767.0) { + talonFx, value -> + if(bus=="rio") talonFxService.active.forEach{ + it.setPosition(value, timeout) } - 1 -> configDoubleParam(config.Slot1.kG) { - talonFx, value -> - config.Slot1.kG = value - talonFx.configurator.apply(config.Slot1) + else talonFxFDService.active.forEach { + it.setPosition(value, timeout) } - 2 -> configDoubleParam(config.Slot2.kG) { - talonFx, value -> - config.Slot2.kG = value - talonFx.configurator.apply(config.Slot2) + } + "fxs" -> configFXSDoubleParam( + if(bus=="rio") talonFxsService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0 + else talonFxsFDService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0) { talonFXS, value -> + if(bus=="rio") talonFxsService.active.forEach { + it.setPosition(value, timeout) + } else talonFxsFDService.active.forEach { + it.setPosition(value, timeout) } - else -> TODO("$activeSlotIndex is not a valid slot") } + else -> throw IllegalArgumentException() + } + VELOCITY -> when(device){ + "fx" -> configFXDoubleParam( + if(bus=="rio") talonFxService.activeVelocity + else talonFxFDService.activeVelocity) { talonFx, value -> + if(bus=="rio") talonFxService.activeVelocity = value + else talonFxFDService.activeVelocity = value + } + "fxs" -> configFXSDoubleParam( + if(bus=="rio") talonFxsService.activeVelocity + else talonFxsFDService.activeVelocity) { talonFXS, value -> + if(bus=="rio") talonFxsService.activeVelocity = value + else talonFxsFDService.activeVelocity = value + } + else -> throw IllegalArgumentException() + } + ACCELERATION -> when(device){ + "fx" -> configFXDoubleParam( + if(bus=="rio") talonFxService.activeAcceleration + else talonFxFDService.activeAcceleration){ talonFx, value -> + if(bus=="rio") talonFxService.activeAcceleration = value + else talonFxFDService.activeAcceleration = value + } + "fxs" -> configFXSDoubleParam( + if(bus=="rio") talonFxsService.activeAcceleration + else talonFxsFDService.activeAcceleration) { talonFXS, value -> + if(bus=="rio") talonFxsService.activeAcceleration = value + else talonFxsFDService.activeAcceleration = value + } + else -> throw IllegalArgumentException() + } + JERK -> when(device){ + "fx" -> configFXDoubleParam( + if(bus=="rio") talonFxService.activeJerk + else talonFxFDService.activeJerk){ talonFx, value -> + if(bus=="rio") talonFxService.activeJerk = value + else talonFxFDService.activeJerk = value + } + "fxs" -> configFXSDoubleParam( + if(bus=="rio") talonFxsService.activeJerk + else talonFxsFDService.activeJerk) { talonFXS, value -> + if(bus=="rio") talonFxsService.activeJerk = value + else talonFxsFDService.activeJerk = value + } + else -> throw IllegalArgumentException() + } + TORQUE_CURRENT_DEADBAND -> when(device){ + "fx" -> configFXDoubleParam( + if(bus=="rio") talonFxService.activeTorqueCurrentDeadband + else talonFxFDService.activeTorqueCurrentDeadband){ talonFx, value -> + if(bus=="rio") talonFxService.activeTorqueCurrentDeadband = value + else talonFxFDService.activeTorqueCurrentDeadband = value + } + "fxs" -> configFXSDoubleParam( + if(bus=="rio") talonFxsService.activeTorqueCurrentDeadband + else talonFxsFDService.activeTorqueCurrentDeadband) { talonFXS, value -> + if(bus=="rio") talonFxsService.activeTorqueCurrentDeadband = value + else talonFxsFDService.activeTorqueCurrentDeadband = value + } + else -> throw IllegalArgumentException() + } + TORQUE_CURRENT_MAX -> when(device){ + "fx" -> configFXDoubleParam( + if(bus=="rio") talonFxService.activeTorqueCurrentMaxOut + else talonFxsFDService.activeTorqueCurrentMaxOut){ talonFx, value -> + if(bus=="rio") talonFxService.activeTorqueCurrentMaxOut = value + talonFxFDService.activeTorqueCurrentMaxOut = value + } + "fxs" -> configFXSDoubleParam( + if(bus=="rio") talonFxsService.activeTorqueCurrentMaxOut + else talonFxsFDService.activeTorqueCurrentMaxOut) { talonFXS, value -> + if(bus=="rio") talonFxsService.activeTorqueCurrentMaxOut = value + else talonFxsFDService.activeTorqueCurrentMaxOut = value + } + else -> throw IllegalArgumentException() + } + FEED_FORWARD -> when(device){ + "fx" -> configFXDoubleParam( + if(bus=="rio") talonFxService.activeFeedForward + else talonFxFDService.activeFeedForward){ talonFx, value -> + if(bus=="rio") talonFxService.activeFeedForward = value + talonFxFDService.activeFeedForward = value + } + "fxs" -> configFXSDoubleParam( + if(bus=="rio") talonFxsService.activeFeedForward + else talonFxsFDService.activeFeedForward) { talonFXS, value -> + if(bus=="rio") talonFxsService.activeFeedForward = value + else talonFxsFDService.activeFeedForward = value + } + else -> throw IllegalArgumentException() + } + OPPOSE_MAIN -> when(device){ + "fx" -> configFXBooleanParam( + if(bus=="rio") talonFxService.activeOpposeMain + else talonFxFDService.activeOpposeMain){ talonFx, value -> + if(bus=="rio") talonFxService.activeOpposeMain = value + else talonFxFDService.activeOpposeMain = value + } + "fxs" -> configFXSBooleanParam( + if(bus=="rio") talonFxsService.activeOpposeMain + else talonFxsFDService.activeOpposeMain) { talonFXS, value -> + if(bus=="rio") talonFxsService.activeOpposeMain = value + else talonFxsFDService.activeOpposeMain = value + } + else -> throw IllegalArgumentException() + } + DIFFERENTIAL_SLOT -> when(device){ + "fx" -> configFXIntParam( + if(bus=="rio") talonFxService.activeDifferentialSlot + else talonFxFDService.activeDifferentialSlot){ talonFx, value -> + if(value >= 0 && value <= 2){ + if(bus=="rio") talonFxService.activeDifferentialSlot = value + else talonFxFDService.activeDifferentialSlot = value + } else terminal.warn("${value} is not a valid slot index, must be 0-2") + } + "fxs" -> configFXSIntParam( + if(bus=="rio") talonFxsService.activeDifferentialSlot + else talonFxsFDService.activeDifferentialSlot) { talonFXS, value -> + if(value >= 0 && value <= 2) { + if(bus=="rio") talonFxsService.activeDifferentialSlot = value + else talonFxsFDService.activeDifferentialSlot = value + } else terminal.warn("${value} is not a valid slot index, must be 0-2") + } + else -> throw IllegalArgumentException() + } + DIFFERENTIAL_TARGET -> when(device){ + "fx" -> configFXDoubleParam( + if(bus=="rio") talonFxService.activeDifferentialTarget + else talonFxFDService.activeDifferentialTarget){ talonFx, value -> + if(bus=="rio") talonFxService.activeDifferentialTarget = value + else talonFxFDService.activeDifferentialTarget = value + } + "fxs" -> configFXSDoubleParam( + if(bus=="rio") talonFxsService.activeDifferentialTarget + else talonFxsFDService.activeDifferentialTarget) { talonFXS, value -> + if(bus=="rio") talonFxsService.activeDifferentialTarget = value + else talonFxsFDService.activeDifferentialTarget = value + } + else -> throw IllegalArgumentException() + } + FOC -> when(device){ + "fx" -> configFXBooleanParam( + if(bus=="rio") talonFxService.activeFOC + else talonFxFDService.activeFOC){ talonFx, value -> + if(bus=="rio") talonFxService.activeFOC = value + else talonFxFDService.activeFOC = value + } + "fxs" -> configFXSBooleanParam( + if(bus=="rio") talonFxsService.activeFOC + else talonFxsFDService.activeFOC) { talonFXS, value -> + if(bus=="rio") talonFxsService.activeFOC = value + else talonFxsFDService.activeFOC = value + } + else -> throw IllegalArgumentException() + } + OVERRIDE_NEUTRAL -> when(device){ + "fx" -> configFXBooleanParam( + if(bus=="rio") talonFxService.activeOverrideNeutral + else talonFxFDService.activeOverrideNeutral){ talonFx, value -> + if(bus=="rio") talonFxService.activeOverrideNeutral = value + else talonFxFDService.activeOverrideNeutral = value + } + "fxs" -> configFXSBooleanParam( + if(bus=="rio") talonFxsService.activeOverrideNeutral + else talonFxsFDService.activeOverrideNeutral) { talonFXS, value -> + if(bus=="rio") talonFxsService.activeOverrideNeutral = value + else talonFxsFDService.activeOverrideNeutral = value + } + else -> throw IllegalArgumentException() + } + LIM_FWD_MOT -> when(device){ + "fx" -> configFXBooleanParam( + if(bus=="rio") talonFxService.limFwdMotion + else talonFxFDService.limFwdMotion) { talonFx, value -> + if(bus=="rio") talonFxService.limFwdMotion = value + else talonFxFDService.limFwdMotion = value + } + "fxs" -> configFXSBooleanParam( + if(bus=="rio") talonFxsService.limFwdMotion + else talonFxsFDService.limFwdMotion) { talonFXS, value -> + if(bus=="rio") talonFxsService.limFwdMotion = value + else talonFxsFDService.limFwdMotion = value + } + else -> throw IllegalArgumentException() + } + LIM_REV_MOT -> when(device){ + "fx" -> configFXBooleanParam( + if(bus=="rio") talonFxService.limRevMotion + else talonFxFDService.limRevMotion) { talonFx, value -> + if(bus=="rio") talonFxService.limRevMotion = value + else talonFxFDService.limRevMotion = value + } + "fxs" -> configFXSBooleanParam( + if(bus=="rio") talonFxsService.limRevMotion + else talonFxsFDService.limRevMotion) { talonFXS, value -> + if(bus=="rio") talonFxsService.limRevMotion = value + else talonFxsFDService.limRevMotion = value + } + else -> throw IllegalArgumentException() } + GRAPHER_FRAME -> when(device) { + "fx" -> configFXDoubleParam( + if (bus == "rio") talonFxService.grapherStatusFrameHz + else talonFxFDService.grapherStatusFrameHz + ) { talonFx, value -> + if (bus == "rio") talonFxService.grapherStatusFrameHz = value + else talonFxFDService.grapherStatusFrameHz = value + + talonFx.acceleration.setUpdateFrequency(value, timeout) + talonFx.bridgeOutput.setUpdateFrequency(value, timeout) + talonFx.deviceTemp.setUpdateFrequency(value, timeout) + talonFx.differentialAveragePosition.setUpdateFrequency(value, timeout) + talonFx.differentialAverageVelocity.setUpdateFrequency(value, timeout) + talonFx.differentialDifferencePosition.setUpdateFrequency(value, timeout) + talonFx.differentialDifferenceVelocity.setUpdateFrequency(value, timeout) + talonFx.dutyCycle.setUpdateFrequency(value, timeout) + talonFx.forwardLimit.setUpdateFrequency(value, timeout) + talonFx.reverseLimit.setUpdateFrequency(value, timeout) + talonFx.isProLicensed.setUpdateFrequency(value, timeout) + talonFx.motionMagicIsRunning.setUpdateFrequency(value, timeout) + talonFx.motorVoltage.setUpdateFrequency(value, timeout) + talonFx.position.setUpdateFrequency(value, timeout) + talonFx.processorTemp.setUpdateFrequency(value, timeout) + talonFx.rotorPosition.setUpdateFrequency(value, timeout) + talonFx.rotorVelocity.setUpdateFrequency(value, timeout) + talonFx.statorCurrent.setUpdateFrequency(value, timeout) + talonFx.supplyCurrent.setUpdateFrequency(value, timeout) + talonFx.supplyVoltage.setUpdateFrequency(value, timeout) + talonFx.torqueCurrent.setUpdateFrequency(value, timeout) + talonFx.velocity.setUpdateFrequency(value, timeout) + + talonFx.closedLoopDerivativeOutput.setUpdateFrequency(value, timeout) + talonFx.closedLoopError.setUpdateFrequency(value, timeout) + talonFx.closedLoopFeedForward.setUpdateFrequency(value, timeout) + talonFx.closedLoopIntegratedOutput.setUpdateFrequency(value, timeout) + talonFx.closedLoopOutput.setUpdateFrequency(value, timeout) + talonFx.closedLoopProportionalOutput.setUpdateFrequency(value, timeout) + talonFx.closedLoopReference.setUpdateFrequency(value, timeout) + talonFx.closedLoopReferenceSlope.setUpdateFrequency(value, timeout) + talonFx.closedLoopSlot.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopDerivativeOutput.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopError.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopFeedForward.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopIntegratedOutput.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopOutput.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopProportionalOutput.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopReference.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopReferenceSlope.setUpdateFrequency(value, timeout) + talonFx.differentialClosedLoopSlot.setUpdateFrequency(value, timeout) + } + + "fxs" -> configFXSDoubleParam( + if (bus == "rio") talonFxsService.grapherStatusFrameHz + else talonFxsFDService.grapherStatusFrameHz + ) { talonFXS, value -> + if (bus == "rio") talonFxsService.grapherStatusFrameHz = value + else talonFxsFDService.grapherStatusFrameHz = value + logger.info { "Grapher Frame Value: " + value } + + talonFXS.acceleration.setUpdateFrequency(value, timeout) + talonFXS.bridgeOutput.setUpdateFrequency(value, timeout) + talonFXS.deviceTemp.setUpdateFrequency(value, timeout) + talonFXS.differentialAveragePosition.setUpdateFrequency(value, timeout) + talonFXS.differentialAverageVelocity.setUpdateFrequency(value, timeout) + talonFXS.differentialDifferencePosition.setUpdateFrequency(value, timeout) + talonFXS.differentialDifferenceVelocity.setUpdateFrequency(value, timeout) + talonFXS.dutyCycle.setUpdateFrequency(value, timeout) + talonFXS.forwardLimit.setUpdateFrequency(value, timeout) + talonFXS.reverseLimit.setUpdateFrequency(value, timeout) + talonFXS.isProLicensed.setUpdateFrequency(value, timeout) + talonFXS.motionMagicIsRunning.setUpdateFrequency(value, timeout) + talonFXS.motorVoltage.setUpdateFrequency(value, timeout) + talonFXS.position.setUpdateFrequency(value, timeout) + talonFXS.processorTemp.setUpdateFrequency(value, timeout) + talonFXS.rotorPosition.setUpdateFrequency(value, timeout) + talonFXS.rotorVelocity.setUpdateFrequency(value, timeout) + talonFXS.statorCurrent.setUpdateFrequency(value, timeout) + talonFXS.supplyCurrent.setUpdateFrequency(value, timeout) + talonFXS.supplyVoltage.setUpdateFrequency(value, timeout) + talonFXS.torqueCurrent.setUpdateFrequency(value, timeout) + talonFXS.velocity.setUpdateFrequency(value, timeout) + + talonFXS.closedLoopDerivativeOutput.setUpdateFrequency(value, timeout) + talonFXS.closedLoopError.setUpdateFrequency(value, timeout) + talonFXS.closedLoopFeedForward.setUpdateFrequency(value, timeout) + talonFXS.closedLoopIntegratedOutput.setUpdateFrequency(value, timeout) + talonFXS.closedLoopOutput.setUpdateFrequency(value, timeout) + talonFXS.closedLoopProportionalOutput.setUpdateFrequency(value, timeout) + talonFXS.closedLoopReference.setUpdateFrequency(value, timeout) + talonFXS.closedLoopReferenceSlope.setUpdateFrequency(value, timeout) + talonFXS.closedLoopSlot.setUpdateFrequency(value, timeout) + talonFXS.differentialClosedLoopDerivativeOutput.setUpdateFrequency(value, timeout) + talonFXS.differentialClosedLoopError.setUpdateFrequency(value, timeout) + talonFXS.differentialClosedLoopFeedForward.setUpdateFrequency(value, timeout) + talonFXS.differentialClosedLoopIntegratedOutput.setUpdateFrequency(value, timeout) + talonFXS.differentialClosedLoopOutput.setUpdateFrequency(value, timeout) + talonFXS.differentialClosedLoopProportionalOutput.setUpdateFrequency(value, timeout) + talonFXS.differentialClosedLoopReference.setUpdateFrequency(value, timeout) + talonFXS.differentialClosedLoopReferenceSlope.setUpdateFrequency(value, timeout) + talonFXS.differentialClosedLoopSlot.setUpdateFrequency(value, timeout) + + talonFXS.ancillaryDeviceTemp.setUpdateFrequency(value, timeout) + talonFXS.externalMotorTemp.setUpdateFrequency(value, timeout) + - MM_ACCEL -> configDoubleParam(config.MotionMagic.MotionMagicAcceleration) { - talonFx, value -> - config.MotionMagic.MotionMagicAcceleration = value - talonFx.configurator.apply(config.MotionMagic) - } - MM_CRUISE_VEL -> configDoubleParam(config.MotionMagic.MotionMagicCruiseVelocity) { - talonFx, value -> - config.MotionMagic.MotionMagicCruiseVelocity = value - talonFx.configurator.apply(config.MotionMagic) - } - MM_JERK -> configDoubleParam(config.MotionMagic.MotionMagicJerk) { - talonFx, value -> - config.MotionMagic.MotionMagicJerk = value - talonFx.configurator.apply(config.MotionMagic) - } - MM_EXPO_KA -> configDoubleParam(config.MotionMagic.MotionMagicExpo_kA) { - talonFx, value -> - config.MotionMagic.MotionMagicExpo_kA = value - talonFx.configurator.apply(config.MotionMagic) - } - MM_EXPO_KV -> configDoubleParam(config.MotionMagic.MotionMagicExpo_kV) { - talonFx, value -> - config.MotionMagic.MotionMagicExpo_kV = value - talonFx.configurator.apply(config.MotionMagic) - } - - PEAK_DIFF_DC -> configDoubleParam(config.DifferentialConstants.PeakDifferentialDutyCycle) { - talonFx, value -> - config.DifferentialConstants.PeakDifferentialDutyCycle = value - talonFx.configurator.apply(config.DifferentialConstants) - } - PEAK_DIFF_TORQUE -> configDoubleParam(config.DifferentialConstants.PeakDifferentialTorqueCurrent) { - talonFx, value -> - config.DifferentialConstants.PeakDifferentialTorqueCurrent = value - talonFx.configurator.apply(config.DifferentialConstants) - } - PEAK_DIFF_VOLT -> configDoubleParam(config.DifferentialConstants.PeakDifferentialVoltage) { - talonFx, value -> - config.DifferentialConstants.PeakDifferentialVoltage = value - talonFx.configurator.apply(config.DifferentialConstants) - } - DIFF_SENSOR_REMOTE_ID -> configIntParam(config.DifferentialSensors.DifferentialRemoteSensorID) { - talonFx, value -> - config.DifferentialSensors.DifferentialRemoteSensorID = value - talonFx.configurator.apply(config.DifferentialSensors) - } - DIFF_FX_ID -> configIntParam(config.DifferentialSensors.DifferentialTalonFXSensorID) { - talonFx, value -> - config.DifferentialSensors.DifferentialTalonFXSensorID = value - talonFx.configurator.apply(config.DifferentialSensors) - } - - STATOR_LIM_EN -> configBooleanParam(config.CurrentLimits.StatorCurrentLimitEnable){ - talonFx, value -> - config.CurrentLimits.StatorCurrentLimitEnable = value - talonFx.configurator.apply(config.CurrentLimits) - } - STATOR_LIM -> configDoubleParam(config.CurrentLimits.StatorCurrentLimit) { - talonFx, value -> - config.CurrentLimits.StatorCurrentLimit = value - talonFx.configurator.apply(config.CurrentLimits) - } - SUPP_LIM_EN -> configBooleanParam(config.CurrentLimits.SupplyCurrentLimitEnable) { - talonFx, value -> - config.CurrentLimits.SupplyCurrentLimitEnable = value - talonFx.configurator.apply(config.CurrentLimits) - } - SUPP_LIM -> configDoubleParam(config.CurrentLimits.SupplyCurrentLimit) { - talonFx, value -> - config.CurrentLimits.SupplyCurrentLimit = value - talonFx.configurator.apply(config.CurrentLimits) - } - SUPP_TRIP_THRES -> configDoubleParam(config.CurrentLimits.SupplyCurrentThreshold) { - talonFx, value -> - config.CurrentLimits.SupplyCurrentThreshold = value - talonFx.configurator.apply(config.CurrentLimits) - } - SUPP_TRIP_TIME -> configDoubleParam(config.CurrentLimits.SupplyTimeThreshold) { - talonFx, value -> - config.CurrentLimits.SupplyTimeThreshold = value - talonFx.configurator.apply(config.CurrentLimits) - } - - CLOSED_LOOP_RAMP_DC -> configDoubleParam(config.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod) { - talonFx, value -> - config.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = value - talonFx.configurator.apply(config.ClosedLoopRamps) - } - PEAK_FWD_DC -> configDoubleParam(config.MotorOutput.PeakForwardDutyCycle) { - talonFx, value -> - config.MotorOutput.PeakForwardDutyCycle = value - talonFx.configurator.apply(config.MotorOutput) - } - PEAK_REV_DC -> configDoubleParam(config.MotorOutput.PeakReverseDutyCycle) { - talonFx, value -> - config.MotorOutput.PeakReverseDutyCycle = value - talonFx.configurator.apply(config.MotorOutput) - } - NEUTRAL_DEADBAND_DC -> configDoubleParam(config.MotorOutput.DutyCycleNeutralDeadband) { - talonFx, value -> - config.MotorOutput.DutyCycleNeutralDeadband = value - talonFx.configurator.apply(config.MotorOutput) - } - OPEN_LOOP_RAMP_DC -> configDoubleParam(config.OpenLoopRamps.DutyCycleOpenLoopRampPeriod) { - talonFx, value -> - config.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = value - talonFx.configurator.apply(config.OpenLoopRamps) - } - PEAK_FWD_V -> configDoubleParam(config.Voltage.PeakForwardVoltage) { - talonFx, value -> - config.Voltage.PeakForwardVoltage = value - talonFx.configurator.apply(config.Voltage) - } - PEAK_REV_V -> configDoubleParam(config.Voltage.PeakReverseVoltage) { - talonFx, value -> - config.Voltage.PeakReverseVoltage = value - talonFx.configurator.apply(config.Voltage) - } - SUPPLY_V_TIME_CONST -> configDoubleParam(config.Voltage.SupplyVoltageTimeConstant) { - talonFx, value -> - config.Voltage.SupplyVoltageTimeConstant = value - talonFx.configurator.apply(config.Voltage) - } - OPEN_LOOP_RAMP_V -> configDoubleParam(config.OpenLoopRamps.VoltageOpenLoopRampPeriod) { - talonFx, value -> - config.OpenLoopRamps.VoltageOpenLoopRampPeriod = value - talonFx.configurator.apply(config.OpenLoopRamps) - } - CLOSED_LOOP_RAMP_V -> configDoubleParam(config.ClosedLoopRamps.VoltageClosedLoopRampPeriod) { - talonFx, value -> - config.ClosedLoopRamps.VoltageClosedLoopRampPeriod = value - talonFx.configurator.apply(config.ClosedLoopRamps) - } - PEAK_FWD_I -> configDoubleParam(config.TorqueCurrent.PeakForwardTorqueCurrent) { - talonFx, value -> - config.TorqueCurrent.PeakForwardTorqueCurrent = value - talonFx.configurator.apply(config.TorqueCurrent) - } - PEAK_REV_I -> configDoubleParam(config.TorqueCurrent.PeakReverseTorqueCurrent) { - talonFx, value -> - config.TorqueCurrent.PeakReverseTorqueCurrent = value - talonFx.configurator.apply(config.TorqueCurrent) - } - TORQUE_NEUTRAL_DEADBAND -> configDoubleParam(config.TorqueCurrent.TorqueNeutralDeadband) { - talonFx, value -> - config.TorqueCurrent.TorqueNeutralDeadband = value - talonFx.configurator.apply(config.TorqueCurrent) - } - OPEN_LOOP_RAMP_I -> configDoubleParam(config.OpenLoopRamps.TorqueOpenLoopRampPeriod) { - talonFx, value -> - config.OpenLoopRamps.TorqueOpenLoopRampPeriod = value - talonFx.configurator.apply(config.OpenLoopRamps) - } - CLOSED_LOOP_RAMP_I -> configDoubleParam(config.ClosedLoopRamps.TorqueClosedLoopRampPeriod) { - talonFx, value -> - config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = value - talonFx.configurator.apply(config.ClosedLoopRamps) - } - CONTINUOUS_WRAP -> configBooleanParam(config.ClosedLoopGeneral.ContinuousWrap){ - talonFx, value -> - config.ClosedLoopGeneral.ContinuousWrap = value - talonFx.configurator.apply(config.ClosedLoopGeneral) - } - - FWD_SOFT_EN -> configBooleanParam(config.SoftwareLimitSwitch.ForwardSoftLimitEnable) { - talonFx, value -> - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = value - talonFx.configurator.apply(config.SoftwareLimitSwitch) - } - FWD_SOFT_THRES -> configDoubleParam(config.SoftwareLimitSwitch.ForwardSoftLimitThreshold) { - talonFx, value -> - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = value - talonFx.configurator.apply(config.SoftwareLimitSwitch) - } - REV_SOFT_EN -> configBooleanParam(config.SoftwareLimitSwitch.ReverseSoftLimitEnable) { - talonFx, value -> - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = value - talonFx.configurator.apply(config.SoftwareLimitSwitch) - } - REV_SOFT_THRES -> configDoubleParam(config.SoftwareLimitSwitch.ReverseSoftLimitThreshold) { - talonFx, value -> - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = value - talonFx.configurator.apply(config.SoftwareLimitSwitch) - } - - FWD_HARD_EN -> configBooleanParam(config.HardwareLimitSwitch.ForwardLimitEnable) { - talonFx, value -> - config.HardwareLimitSwitch.ForwardLimitEnable = value - talonFx.configurator.apply(config.HardwareLimitSwitch) - } - FWD_REMOTE_ID -> configIntParam(config.HardwareLimitSwitch.ForwardLimitRemoteSensorID) { - talonFx, value -> - config.HardwareLimitSwitch.ForwardLimitRemoteSensorID = value - talonFx.configurator.apply(config.HardwareLimitSwitch) - } - FWD_AUTOSET_POS -> configBooleanParam(config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable) { - talonFx, value -> - config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable = value - talonFx.configurator.apply(config.HardwareLimitSwitch) - } - FWD_AUTOSET_POS_VALUE -> configDoubleParam(config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue) { - talonFx, value -> - config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = value - talonFx.configurator.apply(config.HardwareLimitSwitch) - } - REV_HARD_EN -> configBooleanParam(config.HardwareLimitSwitch.ReverseLimitEnable) { - talonFx, value -> - config.HardwareLimitSwitch.ReverseLimitEnable = value - talonFx.configurator.apply(config.HardwareLimitSwitch) - } - REV_REMOTE_ID -> configIntParam(config.HardwareLimitSwitch.ReverseLimitRemoteSensorID) { - talonFx, value -> - config.HardwareLimitSwitch.ReverseLimitRemoteSensorID = value - talonFx.configurator.apply(config.HardwareLimitSwitch) - } - REV_AUTOSET_POS -> configBooleanParam(config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable) { - talonFx, value -> - config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = value - talonFx.configurator.apply(config.HardwareLimitSwitch) - } - REV_AUTOSET_POS_VALUE -> configDoubleParam(config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue) { - talonFx, value -> - config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = value - talonFx.configurator.apply(config.HardwareLimitSwitch) - } - - ALLOW_MUSIC_DIS -> configBooleanParam(config.Audio.AllowMusicDurDisable) { - talonFx, value -> - config.Audio.AllowMusicDurDisable = value - talonFx.configurator.apply(config.Audio) - } - BEEP_ON_BOOT -> configBooleanParam(config.Audio.BeepOnBoot) { - talonFx, value -> - config.Audio.BeepOnBoot = value - talonFx.configurator.apply(config.Audio) - } - BEEP_ON_CONFIG -> configBooleanParam(config.Audio.BeepOnConfig) { - talonFx, value -> - config.Audio.BeepOnConfig = value - talonFx.configurator.apply(config.Audio) - } - POSITION -> configDoubleParam(talonFxService.active.firstOrNull()?.position?.valueAsDouble ?: 2767.0) { - talonFx, value -> - talonFxService.active.forEach{ - talonFx.setPosition(value, timeout) - } - } - VELOCITY -> configDoubleParam(talonFxService.activeVelocity) { - talonFx, value -> - talonFxService.activeVelocity = value - } - ACCELERATION -> configDoubleParam(talonFxService.activeAcceleration){ - talonFx, value -> - talonFxService.activeAcceleration = value - } - JERK -> configDoubleParam(talonFxService.activeJerk){ - talonFx, value -> - talonFxService.activeJerk = value - } - TORQUE_CURRENT_DEADBAND -> configDoubleParam(talonFxService.activeTorqueCurrentDeadband){ - talonFx, value -> - talonFxService.activeTorqueCurrentDeadband = value - } - TORQUE_CURRENT_MAX -> configDoubleParam(talonFxService.activeTorqueCurrentMaxOut){ - talonFx, value -> - talonFxService.activeTorqueCurrentMaxOut = value - } - FEED_FORWARD -> configDoubleParam(talonFxService.activeFeedForward){ - talonFx, value -> - talonFxService.activeFeedForward = value - } - OPPOSE_MAIN -> configBooleanParam(talonFxService.activeOpposeMain){ - talonFx, value -> - talonFxService.activeOpposeMain = value - } - DIFFERENTIAL_SLOT -> configIntParam(talonFxService.activeDifferentialSlot){ - talonFx, value -> - if(value >= 0 && value <= 2){ - talonFxService.activeDifferentialSlot = value - } else { - terminal.warn("${value} is not a valid slot index, must be 0-2") - } - } - DIFFERENTIAL_TARGET -> configDoubleParam(talonFxService.activeDifferentialTarget){ - talonFx, value -> - talonFxService.activeDifferentialTarget = value - } - FOC -> configBooleanParam(talonFxService.activeFOC){ - talonFx, value -> - talonFxService.activeFOC = value - } - OVERRIDE_NEUTRAL -> configBooleanParam(talonFxService.activeOverrideNeutral){ - talonFx, value -> - talonFxService.activeOverrideNeutral = value - } - LIM_FWD_MOT -> configBooleanParam(talonFxService.limFwdMotion) { - talonFx, value -> - talonFxService.limFwdMotion = value; - } - LIM_REV_MOT -> configBooleanParam(talonFxService.limRevMotion) { - talonFx, value -> - talonFxService.limRevMotion = value; - } - GRAPHER_FRAME -> configDoubleParam(talonFxService.grapherStatusFrameHz) { - talonFx, value -> - talonFxService.grapherStatusFrameHz = value; - talonFx.acceleration.setUpdateFrequency(value, timeout) - talonFx.bridgeOutput.setUpdateFrequency(value, timeout) - talonFx.deviceTemp.setUpdateFrequency(value, timeout) - talonFx.differentialAveragePosition.setUpdateFrequency(value, timeout) - talonFx.differentialAverageVelocity.setUpdateFrequency(value, timeout) - talonFx.differentialDifferencePosition.setUpdateFrequency(value, timeout) - talonFx.differentialDifferenceVelocity.setUpdateFrequency(value, timeout) - talonFx.dutyCycle.setUpdateFrequency(value, timeout) - talonFx.forwardLimit.setUpdateFrequency(value, timeout) - talonFx.reverseLimit.setUpdateFrequency(value, timeout) - talonFx.isProLicensed.setUpdateFrequency(value, timeout) - talonFx.motionMagicIsRunning.setUpdateFrequency(value, timeout) - talonFx.motorVoltage.setUpdateFrequency(value, timeout) - talonFx.position.setUpdateFrequency(value, timeout) - talonFx.processorTemp.setUpdateFrequency(value, timeout) - talonFx.rotorPosition.setUpdateFrequency(value, timeout) - talonFx.rotorVelocity.setUpdateFrequency(value, timeout) - talonFx.statorCurrent.setUpdateFrequency(value, timeout) - talonFx.supplyCurrent.setUpdateFrequency(value, timeout) - talonFx.supplyVoltage.setUpdateFrequency(value, timeout) - talonFx.torqueCurrent.setUpdateFrequency(value, timeout) - talonFx.velocity.setUpdateFrequency(value, timeout) - - talonFx.closedLoopDerivativeOutput.setUpdateFrequency(value, timeout) - talonFx.closedLoopError.setUpdateFrequency(value, timeout) - talonFx.closedLoopFeedForward.setUpdateFrequency(value, timeout) - talonFx.closedLoopIntegratedOutput.setUpdateFrequency(value, timeout) - talonFx.closedLoopOutput.setUpdateFrequency(value, timeout) - talonFx.closedLoopProportionalOutput.setUpdateFrequency(value, timeout) - talonFx.closedLoopReference.setUpdateFrequency(value, timeout) - talonFx.closedLoopReferenceSlope.setUpdateFrequency(value, timeout) - talonFx.closedLoopSlot.setUpdateFrequency(value, timeout) - talonFx.differentialClosedLoopDerivativeOutput.setUpdateFrequency(value, timeout) - talonFx.differentialClosedLoopError.setUpdateFrequency(value, timeout) - talonFx.differentialClosedLoopFeedForward.setUpdateFrequency(value, timeout) - talonFx.differentialClosedLoopIntegratedOutput.setUpdateFrequency(value, timeout) - talonFx.differentialClosedLoopOutput.setUpdateFrequency(value, timeout) - talonFx.differentialClosedLoopProportionalOutput.setUpdateFrequency(value, timeout) - talonFx.differentialClosedLoopReference.setUpdateFrequency(value, timeout) - talonFx.differentialClosedLoopReferenceSlope.setUpdateFrequency(value, timeout) - talonFx.differentialClosedLoopSlot.setUpdateFrequency(value, timeout) - + talonFXS.rawPulseWidthPosition.setUpdateFrequency(value,timeout) + logger.info { "Before new frames" } + talonFXS.rawQuadraturePosition.setUpdateFrequency(value, timeout) + talonFXS.rawPulseWidthVelocity.setUpdateFrequency(value, timeout) + talonFXS.rawQuadratureVelocity.setUpdateFrequency(value, timeout) + } + + else -> throw IllegalArgumentException() + } + ABS_SENSOR_DISCONTINUITY -> when(device) { + "fxs" -> configFXSDoubleParam(fxsConfig.ExternalFeedback.AbsoluteSensorDiscontinuityPoint) { talonFxs, value -> + fxsConfig.ExternalFeedback.AbsoluteSensorDiscontinuityPoint = value + talonFxs.configurator.apply(fxsConfig.ExternalFeedback) + } + else -> throw IllegalArgumentException() + } + ABS_SENSOR_OFFSET -> when(device) { + "fxs" -> configFXSDoubleParam(fxsConfig.ExternalFeedback.AbsoluteSensorOffset) { talonFxs, value -> + fxsConfig.ExternalFeedback.AbsoluteSensorOffset = value + talonFxs.configurator.apply(fxsConfig.ExternalFeedback) + } + else -> throw IllegalArgumentException() } + QUAD_EDGE_PER_ROT -> when(device) { + "fxs" -> configFXSIntParam(fxsConfig.ExternalFeedback.QuadratureEdgesPerRotation) { talonFxs, value -> + fxsConfig.ExternalFeedback.QuadratureEdgesPerRotation = value + talonFxs.configurator.apply(fxsConfig.ExternalFeedback) + } + else -> throw IllegalArgumentException() + } + VELOCITY_FILTER_TIME_CONST -> when(device) { + "fxs" -> configFXSDoubleParam(fxsConfig.ExternalFeedback.VelocityFilterTimeConstant) { talonFxs, value -> + fxsConfig.ExternalFeedback.VelocityFilterTimeConstant = value + talonFxs.configurator.apply(fxsConfig.ExternalFeedback) + } + else -> throw IllegalArgumentException() + } + else -> TODO("${param.name} not implemented") } return super.execute() } - private fun configDoubleParam(default: Double, configure: (TalonFX, Double) -> Unit) { + private fun configFXDoubleParam(default: Double, configure: (TalonFX, Double) -> Unit) { val paramValue = param.readDouble(reader, default) if(bus == "rio") { talonFxService.active.forEach { configure(it, paramValue) } @@ -760,7 +1903,19 @@ class Phoenix6ParameterCommand( } else throw IllegalArgumentException() } - private fun configIntParam(default: Int, configure: (TalonFX, Int) -> Unit) { + private fun configFXSDoubleParam(default: Double, configure: (TalonFXS, Double) -> Unit) { + val paramValue = param.readDouble(reader, default) + if(bus == "rio") { + talonFxsService.active.forEach { configure(it, paramValue) } + logger.info { "set ${talonFxsService.active.size} talonFXS's ${param.name}: $paramValue" } + } else if(bus == "canivore") { + talonFxsFDService.active.forEach { configure(it, paramValue) } + logger.info { "set ${talonFxsFDService.active.size} talonFXS's ${param.name}: $paramValue" } + } else throw IllegalArgumentException() + } + + + private fun configFXIntParam(default: Int, configure: (TalonFX, Int) -> Unit) { val paramValue = param.readInt(reader, default) if(bus == "rio") { talonFxService.active.forEach { configure(it, paramValue) } @@ -771,7 +1926,18 @@ class Phoenix6ParameterCommand( } else throw IllegalArgumentException() } - private fun configBooleanParam(default: Boolean, configure: (TalonFX, Boolean) -> Unit) { + private fun configFXSIntParam(default: Int, configure: (TalonFXS, Int) -> Unit) { + val paramValue = param.readInt(reader, default) + if(bus == "rio") { + talonFxsService.active.forEach { configure(it, paramValue) } + logger.info { "set ${talonFxsService.active.size} talonFXS's ${param.name}: $paramValue" } + } else if(bus == "canivore") { + talonFxsFDService.active.forEach { configure(it, paramValue) } + logger.info { "set ${talonFxsFDService.active.size} talonFXS's ${param.name}: $paramValue" } + } else throw IllegalArgumentException() + } + + private fun configFXBooleanParam(default: Boolean, configure: (TalonFX, Boolean) -> Unit) { val paramValue = param.readBoolean(reader, default) if(bus == "rio") { talonFxService.active.forEach { configure(it, paramValue) } @@ -782,6 +1948,17 @@ class Phoenix6ParameterCommand( } else throw IllegalArgumentException() } + private fun configFXSBooleanParam(default: Boolean, configure: (TalonFXS, Boolean) -> Unit) { + val paramValue = param.readBoolean(reader, default) + if(bus == "rio") { + talonFxsService.active.forEach { configure(it, paramValue) } + logger.info { "Set ${talonFxsService.active.size} talonFXS's ${param.name}: $paramValue" } + } else if(bus == "canivore") { + talonFxsFDService.active.forEach { configure(it, paramValue) } + logger.info { "Set ${talonFxsFDService.active.size} XS's ${param.name}: $paramValue" } + } else throw IllegalArgumentException() + } + } \ No newline at end of file diff --git a/src/main/resources/commands.toml b/src/main/resources/commands.toml index 7ab269b..164bf01 100644 --- a/src/main/resources/commands.toml +++ b/src/main/resources/commands.toml @@ -466,1146 +466,2036 @@ type = "menu" param = "FACTORY_DEFAULTS" device = "srx" - [rio.talonFx] +# [rio.talonFx] +# type = "menu" +# order = 15 +# menu = "work with legacy talonFXs" +# [rio.talonFx.run] +# type = "talon.run" +# order = 5 +# menu = "run active talonFXs" +# device = "fx" +# [rio.talonFx.select] +# type = "talon.select" +# order = 10 +# menu = "active talonFXs" +# device = "fx" +# [rio.talonFx.status] +# type = "talon.status" +# order = 12 +# menu = "status of active talonFXs" +# device = "fx" +# [rio.talonFx.mode] +# type = "talon.mode" +# order = 15 +# menu = "control mode" +# device = "fx" +# help = "The output mode to apply. In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped. In Current mode, output value is in amperes. In Velocity mode, output value is in position change / 100ms. In Position mode, output value is in encoder ticks or an analog value, depending on the sensor. In Follower mode, the output value is the integer device ID of the talon to duplicate." +# [rio.talonFx.sensor] +# type = "menu" +# order = 17 +# menu = "configure feedback sensors" +# device = "fx" +# [rio.talonFx.sensor.primary] +# type = "talon.sensor" +# order = 10 +# menu = "primary closed-loop sensor" +# pid = 0 +# device = "fx" +# help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." +# [rio.talonFx.sensor.auxiliary] +# type = "talon.sensor" +# order = 20 +# menu = "auxiliary closed-loop sensor" +# pid = 1 +# device = "fx" +# help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." +# [rio.talonFx.sensor.primary_position] +# type = "talon.sensor.position" +# order = 30 +# menu = "primary sensor feedback position" +# param = "SENSOR_POSITION" +# pid = 0 +# device = "fx" +# [rio.talonFx.sensor.auxiliary_position] +# type = "talon.sensor.position" +# order = 40 +# menu = "auxiliary sensor feedback position" +# param = "SENSOR_POSITION" +# pid = 1 +# device = "fx" +# [rio.talonFx.sensor.primary_coefficient] +# type = "talon.sensor.coefficient" +# order = 50 +# menu = "primary sensor feedback coefficient" +# param = "FEEDBACK_COEFFICIENT" +# pid = 0 +# device = "fx" +# [rio.talonFx.sensor.auxiliary_coefficient] +# type = "talon.sensor.coefficient" +# order = 60 +# menu = "auxiliary sensor feedback coefficient" +# param = "FEEDBACK_COEFFICIENT" +# pid = 1 +# device = "fx" +# [rio.talonFx.sensor.phase] +# type = "talon.param" +# order = 70 +# menu = "sensor phase reversed" +# param = "SENSOR_PHASE" +# device = "fx" +# [rio.talonFx.sensor.offset] +# type = "talon.param" +# order = 80 +# menu = "integrated sensor offset degrees" +# param = "INTEGRATED_SENSOR_OFFSET_DEGREES" +# device = "fx" +# [rio.talonFx.sensor.absolute_range] +# type = "talon.absoluteRange" +# order = 90 +# menu = "absolute sensor range" +# device = "fx" +# [rio.talonFx.sensor.initialization_strategy] +# type = "talon.initStrategy" +# order = 100 +# menu = "integrated sensor initialization strategy" +# device = "fx" +# [rio.talonFx.slot] +# type = "menu" +# order = 20 +# menu = "configure PID slots" +# device = "fx" +# [rio.talonFx.slot.select] +# type = "talon.slot.select" +# order = 10 +# menu = "active slot" +# device = "fx" +# help = "The Talon FX supports up to 4 PIDF Slots, each with a unique set of gains that are stored persistently across power cycles. " +# [rio.talonFx.slot.P] +# type ="talon.param" +# order = 20 +# param = "SLOT_P" +# device = "fx" +# [rio.talonFx.slot.I] +# type ="talon.param" +# order = 30 +# param = "SLOT_I" +# device = "fx" +# [rio.talonFx.slot.D] +# type ="talon.param" +# order = 40 +# param = "SLOT_D" +# device = "fx" +# [rio.talonFx.slot.F] +# type ="talon.param" +# order = 50 +# param = "SLOT_F" +# device = "fx" +# [rio.talonFx.slot.IZone] +# type ="talon.param" +# menu = "I Zone" +# order = 60 +# param = "SLOT_I_ZONE" +# device = "fx" +# [rio.talonFx.slot.AllowableErr] +# type ="talon.param" +# menu = "Allowable Error" +# order = 70 +# param = "SLOT_ALLOWABLE_ERR" +# device = "fx" +# [rio.talonFx.slot.MaxIAccum] +# type ="talon.param" +# menu = "Max I Accum" +# order = 80 +# param = "SLOT_MAX_I_ACCUM" +# device = "fx" +# [rio.talonFx.slot.PeakOutput] +# type = "talon.param" +# menu = "Peak output" +# order = 90 +# param = "SLOT_PEAK_OUTPUT" +# device = "fx" +# [rio.talonFx.motion_magic] +# type = "menu" +# order = 22 +# menu = "configure motion magic" +# device = "fx" +# [rio.talonFx.motion_magic.cruise_velocity] +# type = "talon.param" +# order = 10 +# menu = "motion magic cruise velocity" +# param = "MOTION_CRUISE_VELOCITY" +# device = "fx" +# [rio.talonFx.motion_magic.acceleration] +# type = "talon.param" +# order = 20 +# menu = "motion magic acceleration" +# param = "MOTION_ACCELERATION" +# device = "fx" +# [rio.talonFx.current] +# type = "menu" +# order = 23 +# menu = "configure current limits" +# [rio.talonFx.current.stator_enabled] +# type = "talon.param" +# order = 10 +# menu = "stator current: limit enabled" +# param = "STATOR_CURRENT_LIMIT_ENABLE" +# device = "fx" +# [rio.talonFx.current.stator_limit] +# type = "talon.param" +# order = 20 +# menu = "stator current: current limit (A)" +# param = "STATOR_CURRENT_LIMIT" +# device = "fx" +# [rio.talonFx.current.stator_triggerCurrent] +# type = "talon.param" +# order = 30 +# menu = "stator current: trigger current (A)" +# param = "STATOR_CURRENT_LIMIT_THRES_CURRENT" +# device = "fx" +# help = "current must exceed this threshold before current limiting occurs. If this number is less than currentLimit than that limit is used as the threshold" +# [rio.talonFx.current.stator_triggerTime] +# type = "talon.param" +# order = 40 +# menu = "stator current: trigger time (s)" +# param = "STATOR_CURRENT_LIMIT_THRES_TIME" +# device = "fx" +# help = "current must exceed the threshold for this long before limiting occurs" +# [rio.talonFx.current.supply_enabled] +# type = "talon.param" +# order = 50 +# menu = "supply current: limit enabled" +# param = "SUPPLY_CURRENT_LIMIT_ENABLE" +# device = "fx" +# [rio.talonFx.current.supply_limit] +# type = "talon.param" +# order = 60 +# menu = "supply current: current limit (A)" +# param = "SUPPLY_CURRENT_LIMIT" +# device = "fx" +# [rio.talonFx.current.supply_triggerCurrent] +# type = "talon.param" +# order = 70 +# menu = "supply current: trigger current (A)" +# param = "SUPPLY_CURRENT_LIMIT_THRES_CURRENT" +# device = "fx" +# help = "current must exceed this threshold before limiting occurs. If this number is less than currentLimit, currentLimit is used as the threshold" +# [rio.talonFx.current.supply_triggerTime] +# type = "talon.param" +# order = 80 +# menu = "supply current: trigger time (s)" +# param = "SUPPLY_CURRENT_LIMIT_THRES_TIME" +# device = "fx" +# help = "current must exceed the threshold for this long before limiting occurs" +# [rio.talonFx.output] +# type = "menu" +# order = 25 +# menu = "configure motor output" +# [rio.talonFx.output.brake] +# type = "talon.brake" +# order = 10 +# menu = "brake mode" +# help = "Mode of operation during Neutral output, brake or coast." +# device = "fx" +# [rio.talonFx.output.reversed] +# type = "talon.param" +# order = 20 +# menu = "motor output reversed" +# param = "OUTPUT_REVERSED" +# device = "fx" +# [rio.talonFx.output.open-loop_ramp] +# type = "talon.param" +# order = 30 +# menu = "open-loop ramp rate" +# param = "OPEN_LOOP_RAMP" +# device = "fx" +# [rio.talonFx.output.closed-loop_ramp] +# type = "talon.param" +# order = 40 +# menu = "closed-loop ramp rate" +# param = "CLOSED_LOOP_RAMP" +# device = "fx" +# [rio.talonFx.output.forward_peak_output] +# type = "talon.param" +# order = 50 +# menu = "forward peak output" +# param = "PEAK_OUTPUT_FORWARD" +# device = "fx" +# [rio.talonFx.output.reverse_peak_output] +# type = "talon.param" +# order = 60 +# menu = "reverse peak output" +# param = "PEAK_OUTPUT_REVERSE" +# device = "fx" +# [rio.talonFx.output.forward_nominal_output] +# type = "talon.param" +# order = 70 +# menu = "forward nominal output" +# param = "NOMINAL_OUTPUT_FORWARD" +# device = "fx" +# [rio.talonFx.output.reverse_nominal_output] +# type = "talon.param" +# order = 80 +# menu = "reverse nominal output" +# param = "NOMINAL_OUTPUT_REVERSE" +# device = "fx" +# [rio.talonFx.output.neutral_deadband] +# type = "talon.param" +# order = 90 +# menu = "neutral deadband" +# param = "NEUTRAL_DEADBAND" +# device = "fx" +# [rio.talonFx.output.motor_commutation] +# type = "talon.commutation" +# order = 100 +# menu = "motor commutation" +# device = "fx" +# [rio.talonFx.velocity_measurement] +# type = "menu" +# order = 26 +# menu = "configure velocity measurement" +# [rio.talonFx.velocity_measurement.period] +# type = "talon.velocity.period" +# order = 10 +# menu = "velocity measurement period" +# device = "fx" +# [rio.talonFx.velocity_measurement.window] +# type = "talon.param" +# order = 20 +# menu = "velocity measurement window" +# param = "VELOCITY_MEASUREMENT_WINDOW" +# device = "fx" +# [rio.talonFx.voltage_compensation] +# type = "menu" +# order = 27 +# menu = "configure voltage compensation" +# [rio.talonFx.voltage_compensation.enabled] +# type = "talon.param" +# order = 10 +# menu = "voltage compensation enabled" +# param = "VOLTAGE_COMP_ENABLE" +# device = "fx" +# [rio.talonFx.voltage_compensation.saturation_voltage] +# type = "talon.param" +# order = 20 +# menu = "voltage compensation saturation voltage" +# param = "VOLTAGE_COMP_SATURATION" +# device = "fx" +# [rio.talonFx.voltage_compensation.filter] +# type = "talon.param" +# order = 30 +# menu = "voltage compensation measurement filter" +# param = "VOLTAGE_MEASUREMENT_FILTER" +# device = "fx" +# [rio.talonFx.frame] +# type = "menu" +# order = 60 +# menu = "configure CAN bus frames" +# [rio.talonFx.frame.grapher] +# type = "default" +# order = 10 +# menu = "set grapher defaults" +# device = "fx" +# [rio.talonFx.frame.general] +# type = "talon.param" +# order = 20 +# menu = "general status frame" +# param = "STATUS_GENERAL" +# device = "fx" +# [rio.talonFx.frame.feedback_0] +# type = "talon.param" +# order = 30 +# menu = "feedback 0 status frame" +# param = "STATUS_FEEDBACK0" +# device = "fx" +# [rio.talonFx.frame.quad_encoder] +# type = "talon.param" +# order = 40 +# menu = "quad encoder status frame" +# param = "STATUS_QUAD_ENCODER" +# device = "fx" +# [rio.talonFx.frame.ain_temp_vbat] +# type = "talon.param" +# order = 50 +# menu = "ain/temp/vbat status frame" +# param = "STATUS_AIN_TEMP_VBAT" +# device = "fx" +# [rio.talonFx.frame.pulse_width] +# type = "talon.param" +# order = 60 +# menu = "pulse width status frame" +# param = "STATUS_PULSE_WIDTH" +# device = "fx" +# [rio.talonFx.frame.motion] +# type = "talon.param" +# order = 70 +# menu = "motion status frame" +# param = "STATUS_MOTION" +# device = "fx" +# [rio.talonFx.frame.pidf_0] +# type = "talon.param" +# order = 80 +# menu = "PIDF 0 status frame" +# param = "STATUS_PIDF0" +# device = "fx" +# [rio.talonFx.frame.misc] +# type = "talon.param" +# order = 90 +# menu = "Misc. status frame" +# param = "STATUS_MISC" +# device = "fx" +# [rio.talonFx.frame.comm] +# type = "talon.param" +# order = 100 +# menu = "Communication status frame" +# param = "STATUS_COMM" +# device = "fx" +# [rio.talonFx.frame.motionBuff] +# type = "talon.param" +# order = 110 +# menu = "Motion Profile Buffer status frame" +# param = "STATUS_MOTION_BUFF" +# device = "fx" +# [rio.talonFx.frame.feedback1] +# type = "talon.param" +# order = 120 +# menu = "Feedback1 status frame" +# param = "STATUS_FEEDBACK1" +# device = "fx" +# [rio.talonFx.frame.pidf1] +# type = "talon.param" +# order = 130 +# menu = "PIDF1 status frame" +# param = "STATUS_PIDF1" +# device = "fx" +# [rio.talonFx.frame.firmware] +# type = "talon.param" +# order = 140 +# menu = "Firmware and API status frame" +# param = "STATUS_FIRMWARE_API" +# device = "fx" +# [rio.talonFx.frame.gadgeteer] +# type = "talon.param" +# order = 150 +# menu = "UART Gadgeteer status frame" +# param = "STATUS_UART_GADGETEER" +# device = "fx" +# [rio.talonFx.limit] +# type = "menu" +# order = 70 +# menu = "configure soft and hard limits" +# [rio.talonFx.limit.forward_hard_source] +# type = "talon.hard.source" +# order = 10 +# forward = true +# menu = "forward hard limit source" +# device = "fx" +# help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." +# [rio.talonFx.limit.forward_hard_normal] +# type = "talon.hard.normal" +# order = 20 +# forward = true +# menu = "forward hard limit normal" +# device = "fx" +# help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." +# [rio.talonFx.limit.reverse_hard_source] +# type = "talon.hard.source" +# order = 30 +# forward = false +# menu = "reverse hard limit source" +# device = "fx" +# help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." +# [rio.talonFx.limit.reverse_hard_normal] +# type = "talon.hard.normal" +# order = 40 +# forward = false +# menu = "reverse hard limit normal" +# device = "fx" +# help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." +# [rio.talonFx.limit.forward_soft_enabled] +# type = "talon.param" +# order = 50 +# menu = "forward soft limit enabled" +# param = "SOFT_LIMIT_ENABLE_FORWARD" +# device = "fx" +# [rio.talonFx.limit.forward_soft_threshold] +# type = "talon.param" +# order = 60 +# menu = "forward soft limit threshold" +# param = "SOFT_LIMIT_THRESHOLD_FORWARD" +# device = "fx" +# [rio.talonFx.limit.reverse_soft_enabled] +# type = "talon.param" +# order = 70 +# menu = "reverse soft limit enabled" +# param = "SOFT_LIMIT_ENABLE_REVERSE" +# device = "fx" +# [rio.talonFx.limit.reverse_soft_threshold] +# type = "talon.param" +# order = 80 +# menu = "reverse soft limit threshold" +# param = "SOFT_LIMIT_THRESHOLD_REVERSE" +# device = "fx" +# [rio.talonFx.factory_default] +# type = "talon.param" +# order = 80 +# menu = "configure factory defaults" +# param = "FACTORY_DEFAULTS" +# device = "fx" + + [rio.FxP6] type = "menu" - order = 15 - menu = "work with legacy talonFXs" - [rio.talonFx.run] - type = "talon.run" - order = 5 - menu = "run active talonFXs" - device = "fx" - [rio.talonFx.select] - type = "talon.select" + order = 17 + menu = "work with talonFX's" + [rio.FxP6.run] + type = "p6.run" order = 10 - menu = "active talonFXs" + menu = "run active talon fx's" + bus = "rio" device = "fx" - [rio.talonFx.status] - type = "talon.status" - order = 12 - menu = "status of active talonFXs" + [rio.FxP6.select] + type = "p6.select" + order = 20 + menu = "active fx's" + bus = "rio" device = "fx" - [rio.talonFx.mode] - type = "talon.mode" - order = 15 - menu = "control mode" + [rio.FxP6.status] + type = "p6.status" + order = 30 + menu = "status of active talonFx's" + bus = "rio" device = "fx" - help = "The output mode to apply. In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped. In Current mode, output value is in amperes. In Velocity mode, output value is in position change / 100ms. In Position mode, output value is in encoder ticks or an analog value, depending on the sensor. In Follower mode, the output value is the integer device ID of the talon to duplicate." - [rio.talonFx.sensor] - type = "menu" - order = 17 - menu = "configure feedback sensors" + [rio.FxP6.mode] + type = "p6.modeMenu" + order = 40 + menu = "config active mode" + bus = "rio" device = "fx" - [rio.talonFx.sensor.primary] - type = "talon.sensor" + [rio.FxP6.mode.status] + type = "p6.modeStatus" order = 10 - menu = "primary closed-loop sensor" - pid = 0 + menu = "active mode" + bus = "rio" device = "fx" - help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." - [rio.talonFx.sensor.auxiliary] - type = "talon.sensor" + [rio.FxP6.mode.select] + type = "p6.mode" + order = 15 + menu = "select mode type" + bus = "rio" + device = "fx" + [rio.FxP6.mode.openLoop] + type = "menu" order = 20 - menu = "auxiliary closed-loop sensor" - pid = 1 + menu = "open loop modes" + bus = "rio" device = "fx" - help = "Sensors for motor controllers provide feedback about the position, velocity, and acceleration of the system using that motor controller." - [rio.talonFx.sensor.primary_position] - type = "talon.sensor.position" + [rio.FxP6.mode.openLoop.deadband] + type = "p6.param" + order = 20 + menu = "set torque current deadband (A)" + param = "TORQUE_CURRENT_DEADBAND" + bus = "rio" + device = "fx" + [rio.FxP6.mode.position] + type = "menu" order = 30 - menu = "primary sensor feedback position" - param = "SENSOR_POSITION" - pid = 0 - device = "fx" - [rio.talonFx.sensor.auxiliary_position] - type = "talon.sensor.position" + menu = "position modes" + [rio.FxP6.mode.position.velocity] + type = "p6.param" + order = 20 + menu = "velocity target" + param = "VELOCITY" + bus = "rio" + device = "fx" + [rio.FxP6.mode.position.feedFwd] + type = "p6.param" + order = 30 + menu = "velocity feed forward" + param = "FEED_FORWARD" + bus = "rio" + device = "fx" + [rio.FxP6.mode.velocity] + type = "menu" order = 40 - menu = "auxiliary sensor feedback position" - param = "SENSOR_POSITION" - pid = 1 - device = "fx" - [rio.talonFx.sensor.primary_coefficient] - type = "talon.sensor.coefficient" + menu = "velocity modes" + [rio.FxP6.mode.velocity.acceleration] + type = "p6.param" + order = 20 + menu = "select acceleration target" + param = "ACCELERATION" + bus = "rio" + device = "fx" + [rio.FxP6.mode.velocity.feedFwd] + type = "p6.param" + order = 30 + menu = "feed forward" + param = "FEED_FORWARD" + bus = "rio" + device = "fx" + [rio.FxP6.mode.motion] + type = "menu" order = 50 - menu = "primary sensor feedback coefficient" - param = "FEEDBACK_COEFFICIENT" - pid = 0 - device = "fx" - [rio.talonFx.sensor.auxiliary_coefficient] - type = "talon.sensor.coefficient" + menu = "motion magic modes" + [rio.FxP6.mode.motion.motionType] + type = "p6.mmType" + order = 10 + menu = "select motion magic type" + bus = "rio" + device = "fx" + [rio.FxP6.mode.motion.feedFwd] + type = "p6.param" + order = 20 + menu = "select feed forward (sel unit)" + param = "FEED_FORWARD" + bus = "rio" + device = "fx" + [rio.FxP6.mode.motion.velocity] + type = "p6.param" + order = 30 + menu = "select velocity target (rot/s)" + param = "VELOCITY" + bus = "rio" + device = "fx" + [rio.FxP6.mode.motion.acceleration] + type = "p6.param" + order = 40 + menu = "select acceleration target (rot/s^2)" + param = "ACCELERATION" + bus = "rio" + device = "fx" + [rio.FxP6.mode.motion.jerk] + type = "p6.param" + order = 50 + menu = "select jerk target (rot/s^3)" + param = "JERK" + bus = "rio" + device = "fx" + [rio.FxP6.mode.diff] + type = "menu" order = 60 - menu = "auxiliary sensor feedback coefficient" - param = "FEEDBACK_COEFFICIENT" - pid = 1 - device = "fx" - [rio.talonFx.sensor.phase] - type = "talon.param" + menu = "differential modes" + [rio.FxP6.mode.diff.diffType] + type = "p6.diffType" + order = 10 + menu = "select differential type" + bus = "rio" + device = "fx" + [rio.FxP6.mode.diff.slot] + type = "p6.param" + order = 20 + menu = "select differential slot" + param = "DIFFERENTIAL_SLOT" + bus = "rio" + device = "fx" + [rio.FxP6.mode.diff.target] + type = "p6.param" + order = 30 + menu = "select differential target (rots)" + param = "DIFFERENTIAL_TARGET" + bus = "rio" + device = "fx" + [rio.FxP6.mode.follow] + type = "menu" order = 70 - menu = "sensor phase reversed" - param = "SENSOR_PHASE" - device = "fx" - [rio.talonFx.sensor.offset] - type = "talon.param" + menu = "follower modes" + [rio.FxP6.mode.follow.followType] + type = "p6.followType" + order = 10 + menu = "select follower type" + bus = "rio" + device = "fx" + [rio.FxP6.mode.follow.oppose] + type = "p6.param" + order = 20 + menu = "oppose leader direction" + param = "OPPOSE_MAIN" + bus = "rio" + device = "fx" + [rio.FxP6.mode.neutral] + type = "menu" order = 80 - menu = "integrated sensor offset degrees" - param = "INTEGRATED_SENSOR_OFFSET_DEGREES" + menu = "neutral modes" + [rio.FxP6.mode.neutral.select] + type = "p6.neutralOut" + order = 10 + menu = "select neutral output" + bus = "rio" + device = "fx" + [rio.FxP6.mode.foc] + type = "p6.param" + order = 100 + menu = "(pro) enable FOC" + param = "FOC" + bus = "rio" device = "fx" - [rio.talonFx.sensor.absolute_range] - type = "talon.absoluteRange" - order = 90 - menu = "absolute sensor range" + [rio.FxP6.mode.overrideNeut] + type = "p6.param" + order = 110 + menu = "override neutral mode" + param = "OVERRIDE_NEUTRAL" + bus = "rio" device = "fx" - [rio.talonFx.sensor.initialization_strategy] - type = "talon.initStrategy" - order = 100 - menu = "integrated sensor initialization strategy" + [rio.FxP6.mode.limFwd] + type = "p6.param" + order = 120 + menu = "limit forward motion" + param = "LIM_FWD_MOT" + bus = "rio" + device = "fx" + [rio.FxP6.mode.limRev] + type = "p6.param" + order = 130 + menu = "limit reverse motion" + param = "LIM_REV_MOT" + bus = "rio" device = "fx" - [rio.talonFx.slot] + [rio.FxP6.units] + type = "p6.units" + order = 50 + menu = "select active units" + bus = "rio" + device = "fx" + [rio.FxP6.feedback] type = "menu" - order = 20 - menu = "configure PID slots" + order = 60 + menu = "setup feedback configs" + bus = "rio" device = "fx" - [rio.talonFx.slot.select] - type = "talon.slot.select" + [rio.FxP6.feedback.position] + type = "p6.param" + order = 5 + menu = "set position" + param = "POSITION" + bus = "rio" + device = "fx" + [rio.FxP6.feedback.offset] + type = "p6.param" order = 10 - menu = "active slot" + menu = "set rotor offset" + param = "ROTOR_OFFSET" + bus = "rio" device = "fx" - help = "The Talon FX supports up to 4 PIDF Slots, each with a unique set of gains that are stored persistently across power cycles. " - [rio.talonFx.slot.P] - type ="talon.param" + [rio.FxP6.feedback.sense2mech] + type = "p6.param" order = 20 - param = "SLOT_P" + menu = "set sensor to mechanism ratio" + param = "SENSOR_TO_MECH_RATIO" + bus = "rio" device = "fx" - [rio.talonFx.slot.I] - type ="talon.param" + [rio.FxP6.feedback.rotor2sense] + type = "p6.param" order = 30 - param = "SLOT_I" - device = "fx" - [rio.talonFx.slot.D] - type ="talon.param" - order = 40 - param = "SLOT_D" - device = "fx" - [rio.talonFx.slot.F] - type ="talon.param" - order = 50 - param = "SLOT_F" - device = "fx" - [rio.talonFx.slot.IZone] - type ="talon.param" - menu = "I Zone" - order = 60 - param = "SLOT_I_ZONE" - device = "fx" - [rio.talonFx.slot.AllowableErr] - type ="talon.param" - menu = "Allowable Error" - order = 70 - param = "SLOT_ALLOWABLE_ERR" - device = "fx" - [rio.talonFx.slot.MaxIAccum] - type ="talon.param" - menu = "Max I Accum" - order = 80 - param = "SLOT_MAX_I_ACCUM" - device = "fx" - [rio.talonFx.slot.PeakOutput] - type = "talon.param" - menu = "Peak output" - order = 90 - param = "SLOT_PEAK_OUTPUT" + menu = "set rotor to sensor ratio" + param = "ROTOR_TO_SENSOR_RATIO" + bus = "rio" device = "fx" - [rio.talonFx.motion_magic] - type = "menu" - order = 22 - menu = "configure motion magic" - device = "fx" - [rio.talonFx.motion_magic.cruise_velocity] - type = "talon.param" - order = 10 - menu = "motion magic cruise velocity" - param = "MOTION_CRUISE_VELOCITY" + [rio.FxP6.feedback.source] + type = "p6.feedback" + order = 35 + menu = "set feedback source" + bus = "rio" device = "fx" - [rio.talonFx.motion_magic.acceleration] - type = "talon.param" - order = 20 - menu = "motion magic acceleration" - param = "MOTION_ACCELERATION" + [rio.FxP6.feedback.remoteId] + type = "p6.param" + order = 40 + menu = "set remote sensor id" + param = "REMOTE_SENSOR_ID" + bus = "rio" device = "fx" - [rio.talonFx.current] + [rio.FxP6.slot] type = "menu" - order = 23 - menu = "configure current limits" - [rio.talonFx.current.stator_enabled] - type = "talon.param" + order = 70 + menu = "setup slot configs" + [rio.FxP6.slot.active] + type = "p6.slot" order = 10 - menu = "stator current: limit enabled" - param = "STATOR_CURRENT_LIMIT_ENABLE" + menu = "active slot" + bus = "rio" device = "fx" - [rio.talonFx.current.stator_limit] - type = "talon.param" + [rio.FxP6.slot.kP] + type = "p6.param" order = 20 - menu = "stator current: current limit (A)" - param = "STATOR_CURRENT_LIMIT" + menu = "kP" + param = "SLOT_KP" + bus = "rio" device = "fx" - [rio.talonFx.current.stator_triggerCurrent] - type = "talon.param" + [rio.FxP6.slot.kI] + type = "p6.param" order = 30 - menu = "stator current: trigger current (A)" - param = "STATOR_CURRENT_LIMIT_THRES_CURRENT" + menu = "kI" + param = "SLOT_KI" + bus = "rio" device = "fx" - help = "current must exceed this threshold before current limiting occurs. If this number is less than currentLimit than that limit is used as the threshold" - [rio.talonFx.current.stator_triggerTime] - type = "talon.param" + [rio.FxP6.slot.kD] + type = "p6.param" order = 40 - menu = "stator current: trigger time (s)" - param = "STATOR_CURRENT_LIMIT_THRES_TIME" + menu = "kD" + param = "SLOT_KD" + bus = "rio" device = "fx" - help = "current must exceed the threshold for this long before limiting occurs" - [rio.talonFx.current.supply_enabled] - type = "talon.param" + [rio.FxP6.slot.kS] + type = "p6.param" order = 50 - menu = "supply current: limit enabled" - param = "SUPPLY_CURRENT_LIMIT_ENABLE" + menu = "kS" + param = "SLOT_KS" + bus = "rio" device = "fx" - [rio.talonFx.current.supply_limit] - type = "talon.param" + [rio.FxP6.slot.kV] + type = "p6.param" order = 60 - menu = "supply current: current limit (A)" - param = "SUPPLY_CURRENT_LIMIT" + menu = "kV" + param = "SLOT_KV" + bus = "rio" device = "fx" - [rio.talonFx.current.supply_triggerCurrent] - type = "talon.param" + [rio.FxP6.slot.kA] + type = "p6.param" + order = 65 + menu = "kA" + param = "SLOT_KA" + bus = "rio" + device = "fx" + [rio.FxP6.slot.kG] + type = "p6.param" order = 70 - menu = "supply current: trigger current (A)" - param = "SUPPLY_CURRENT_LIMIT_THRES_CURRENT" + menu = "kG" + param = "SLOT_KG" + bus = "rio" device = "fx" - help = "current must exceed this threshold before limiting occurs. If this number is less than currentLimit, currentLimit is used as the threshold" - [rio.talonFx.current.supply_triggerTime] - type = "talon.param" + [rio.FxP6.slot.gravity] + type = "p6.gravity" order = 80 - menu = "supply current: trigger time (s)" - param = "SUPPLY_CURRENT_LIMIT_THRES_TIME" + menu = "gravity type" + bus = "rio" device = "fx" - help = "current must exceed the threshold for this long before limiting occurs" - [rio.talonFx.output] + [rio.FxP6.motion] type = "menu" - order = 25 - menu = "configure motor output" - [rio.talonFx.output.brake] - type = "talon.brake" + order = 80 + menu = "setup motion magic configs" + [rio.FxP6.motion.accel] + type = "p6.param" order = 10 - menu = "brake mode" - help = "Mode of operation during Neutral output, brake or coast." + menu = "acceleration" + param = "MM_ACCEL" + bus = "rio" device = "fx" - [rio.talonFx.output.reversed] - type = "talon.param" + [rio.FxP6.motion.cruise] + type = "p6.param" order = 20 - menu = "motor output reversed" - param = "OUTPUT_REVERSED" + menu = "cruise velocity" + param = "MM_CRUISE_VEL" + bus = "rio" device = "fx" - [rio.talonFx.output.open-loop_ramp] - type = "talon.param" + [rio.FxP6.motion.jerk] + type = "p6.param" order = 30 - menu = "open-loop ramp rate" - param = "OPEN_LOOP_RAMP" + menu = "jerk" + param = "MM_JERK" + bus = "rio" device = "fx" - [rio.talonFx.output.closed-loop_ramp] - type = "talon.param" + [rio.FxP6.motion.kA] + type = "p6.param" order = 40 - menu = "closed-loop ramp rate" - param = "CLOSED_LOOP_RAMP" + menu = "Expo kA" + param = "MM_EXPO_KA" + bus = "rio" device = "fx" - [rio.talonFx.output.forward_peak_output] - type = "talon.param" + [rio.FxP6.motion.kV] + type = "p6.param" order = 50 - menu = "forward peak output" - param = "PEAK_OUTPUT_FORWARD" - device = "fx" - [rio.talonFx.output.reverse_peak_output] - type = "talon.param" - order = 60 - menu = "reverse peak output" - param = "PEAK_OUTPUT_REVERSE" + menu = "Expo kV" + param = "MM_EXPO_KV" + bus = "rio" device = "fx" - [rio.talonFx.output.forward_nominal_output] - type = "talon.param" - order = 70 - menu = "forward nominal output" - param = "NOMINAL_OUTPUT_FORWARD" + [rio.FxP6.diff] + type = "menu" + order = 90 + menu = "setup differential configs" + [rio.FxP6.diff.duty] + type = "p6.param" + order = 10 + menu = "peak differential duty cycle" + param = "PEAK_DIFF_DC" + bus = "rio" device = "fx" - [rio.talonFx.output.reverse_nominal_output] - type = "talon.param" - order = 80 - menu = "reverse nominal output" - param = "NOMINAL_OUTPUT_REVERSE" + [rio.FxP6.diff.torque] + type = "p6.param" + order = 20 + menu = "peak differential torque current" + param = "PEAK_DIFF_TORQUE" + bus = "rio" device = "fx" - [rio.talonFx.output.neutral_deadband] - type = "talon.param" - order = 90 - menu = "neutral deadband" - param = "NEUTRAL_DEADBAND" + [rio.FxP6.diff.volt] + type = "p6.param" + order = 30 + menu = "peak differential voltage" + param = "PEAK_DIFF_VOLT" + bus = "rio" device = "fx" - [rio.talonFx.output.motor_commutation] - type = "talon.commutation" - order = 100 - menu = "motor commutation" + [rio.FxP6.diff.remote] + type = "p6.param" + order = 40 + menu = "differential remote id" + param = "DIFF_SENSOR_REMOTE_ID" + bus = "rio" device = "fx" - [rio.talonFx.velocity_measurement] - type = "menu" - order = 26 - menu = "configure velocity measurement" - [rio.talonFx.velocity_measurement.period] - type = "talon.velocity.period" - order = 10 - menu = "velocity measurement period" + [rio.FxP6.diff.fx] + type = "p6.param" + order = 50 + menu = "differential fx id" + param = "DIFF_FX_ID" + bus = "rio" device = "fx" - [rio.talonFx.velocity_measurement.window] - type = "talon.param" - order = 20 - menu = "velocity measurement window" - param = "VELOCITY_MEASUREMENT_WINDOW" + [rio.FxP6.diff.source] + type = "p6.diffSource" + order = 60 + menu = "differential source" + bus = "rio" device = "fx" - [rio.talonFx.voltage_compensation] + [rio.FxP6.current] type = "menu" - order = 27 - menu = "configure voltage compensation" - [rio.talonFx.voltage_compensation.enabled] - type = "talon.param" + order = 100 + menu = "setup current limit configs" + [rio.FxP6.current.statorEn] + type = "p6.param" order = 10 - menu = "voltage compensation enabled" - param = "VOLTAGE_COMP_ENABLE" + menu = "stator limit enable" + param = "STATOR_LIM_EN" + bus = "rio" device = "fx" - [rio.talonFx.voltage_compensation.saturation_voltage] - type = "talon.param" + [rio.FxP6.current.statorLim] + type = "p6.param" order = 20 - menu = "voltage compensation saturation voltage" - param = "VOLTAGE_COMP_SATURATION" + menu = "stator current limit" + param = "STATOR_LIM" + bus = "rio" device = "fx" - [rio.talonFx.voltage_compensation.filter] - type = "talon.param" + [rio.FxP6.current.suppEn] + type = "p6.param" order = 30 - menu = "voltage compensation measurement filter" - param = "VOLTAGE_MEASUREMENT_FILTER" - device = "fx" - [rio.talonFx.frame] - type = "menu" - order = 60 - menu = "configure CAN bus frames" - [rio.talonFx.frame.grapher] - type = "default" - order = 10 - menu = "set grapher defaults" - device = "fx" - [rio.talonFx.frame.general] - type = "talon.param" - order = 20 - menu = "general status frame" - param = "STATUS_GENERAL" - device = "fx" - [rio.talonFx.frame.feedback_0] - type = "talon.param" - order = 30 - menu = "feedback 0 status frame" - param = "STATUS_FEEDBACK0" + menu = "supply limit enable" + param = "SUPP_LIM_EN" + bus = "rio" device = "fx" - [rio.talonFx.frame.quad_encoder] - type = "talon.param" + [rio.FxP6.current.suppLim] + type = "p6.param" order = 40 - menu = "quad encoder status frame" - param = "STATUS_QUAD_ENCODER" + menu = "supply current lower limit" + param = "SUPP_LOWER_LIM" + bus = "rio" device = "fx" - [rio.talonFx.frame.ain_temp_vbat] - type = "talon.param" + [rio.FxP6.current.tripI] + type = "p6.param" order = 50 - menu = "ain/temp/vbat status frame" - param = "STATUS_AIN_TEMP_VBAT" + menu = "supply current limit (A)" + param = "SUPP_LIM" + bus = "rio" device = "fx" - [rio.talonFx.frame.pulse_width] - type = "talon.param" + [rio.FxP6.current.tripT] + type = "p6.param" order = 60 - menu = "pulse width status frame" - param = "STATUS_PULSE_WIDTH" - device = "fx" - [rio.talonFx.frame.motion] - type = "talon.param" - order = 70 - menu = "motion status frame" - param = "STATUS_MOTION" - device = "fx" - [rio.talonFx.frame.pidf_0] - type = "talon.param" - order = 80 - menu = "PIDF 0 status frame" - param = "STATUS_PIDF0" - device = "fx" - [rio.talonFx.frame.misc] - type = "talon.param" - order = 90 - menu = "Misc. status frame" - param = "STATUS_MISC" - device = "fx" - [rio.talonFx.frame.comm] - type = "talon.param" - order = 100 - menu = "Communication status frame" - param = "STATUS_COMM" - device = "fx" - [rio.talonFx.frame.motionBuff] - type = "talon.param" - order = 110 - menu = "Motion Profile Buffer status frame" - param = "STATUS_MOTION_BUFF" - device = "fx" - [rio.talonFx.frame.feedback1] - type = "talon.param" - order = 120 - menu = "Feedback1 status frame" - param = "STATUS_FEEDBACK1" - device = "fx" - [rio.talonFx.frame.pidf1] - type = "talon.param" - order = 130 - menu = "PIDF1 status frame" - param = "STATUS_PIDF1" - device = "fx" - [rio.talonFx.frame.firmware] - type = "talon.param" - order = 140 - menu = "Firmware and API status frame" - param = "STATUS_FIRMWARE_API" - device = "fx" - [rio.talonFx.frame.gadgeteer] - type = "talon.param" - order = 150 - menu = "UART Gadgeteer status frame" - param = "STATUS_UART_GADGETEER" + menu = "supply current trip time (s)" + param = "SUPP_TRIP_TIME" + bus = "rio" device = "fx" - [rio.talonFx.limit] + [rio.FxP6.motor] type = "menu" - order = 70 - menu = "configure soft and hard limits" - [rio.talonFx.limit.forward_hard_source] - type = "talon.hard.source" + order = 110 + menu = "setup motor output and ramp configs" + [rio.FxP6.motor.invert] + type = "p6.invert" order = 10 - forward = true - menu = "forward hard limit source" + menu = "motor inversion" + bus = "rio" device = "fx" - help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." - [rio.talonFx.limit.forward_hard_normal] - type = "talon.hard.normal" + [rio.FxP6.motor.neutral] + type = "p6.neutral" order = 20 - forward = true - menu = "forward hard limit normal" - device = "fx" - help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." - [rio.talonFx.limit.reverse_hard_source] - type = "talon.hard.source" - order = 30 - forward = false - menu = "reverse hard limit source" - device = "fx" - help = "Select a limit switch source from the feedback connector, remote Talon SRX, CANifier, or deactivate the feature." - [rio.talonFx.limit.reverse_hard_normal] - type = "talon.hard.normal" - order = 40 - forward = false - menu = "reverse hard limit normal" - device = "fx" - help = "Select whether this limit switch is normally open, normally closed, or disabled, which overrides the setting from the web-based configuration page." - [rio.talonFx.limit.forward_soft_enabled] - type = "talon.param" - order = 50 - menu = "forward soft limit enabled" - param = "SOFT_LIMIT_ENABLE_FORWARD" - device = "fx" - [rio.talonFx.limit.forward_soft_threshold] - type = "talon.param" - order = 60 - menu = "forward soft limit threshold" - param = "SOFT_LIMIT_THRESHOLD_FORWARD" - device = "fx" - [rio.talonFx.limit.reverse_soft_enabled] - type = "talon.param" - order = 70 - menu = "reverse soft limit enabled" - param = "SOFT_LIMIT_ENABLE_REVERSE" - device = "fx" - [rio.talonFx.limit.reverse_soft_threshold] - type = "talon.param" - order = 80 - menu = "reverse soft limit threshold" - param = "SOFT_LIMIT_THRESHOLD_REVERSE" - device = "fx" - [rio.talonFx.factory_default] - type = "talon.param" - order = 80 - menu = "configure factory defaults" - param = "FACTORY_DEFAULTS" - device = "fx" - - [rio.FxP6] - type = "menu" - order = 17 - menu = "work with talonFX's" - [rio.FxP6.run] - type = "p6.run" - order = 10 - menu = "run active talon fx's" - bus = "rio" - [rio.FxP6.select] - type = "p6.select" - order = 20 - menu = "active fx's" - bus = "rio" - [rio.FxP6.status] - type = "p6.status" - order = 30 - menu = "status of active talonFx's" - bus = "rio" - [rio.FxP6.mode] - type = "p6.modeMenu" - order = 40 - menu = "config active mode" - bus = "rio" - [rio.FxP6.mode.status] - type = "p6.modeStatus" - order = 10 - menu = "active mode" - bus = "rio" - [rio.FxP6.mode.select] - type = "p6.mode" - order = 15 - menu = "select mode type" + menu = "neutral mode" bus = "rio" - [rio.FxP6.mode.openLoop] + device = "fx" + [rio.FxP6.motor.duty] type = "menu" - order = 20 - menu = "open loop modes" + order = 30 + menu = "duty cycle configs" bus = "rio" - [rio.FxP6.mode.openLoop.deadband] - type = "p6.param" + device = "fx" + [rio.FxP6.motor.duty.peakFwd] + type= "p6.param" order = 20 - menu = "set torque current deadband (A)" - param = "TORQUE_CURRENT_DEADBAND" + menu = "peak forward %" + param = "PEAK_FWD_DC" bus = "rio" - [rio.FxP6.mode.position] - type = "menu" - order = 30 - menu = "position modes" - [rio.FxP6.mode.position.velocity] + device = "fx" + [rio.FxP6.motor.duty.peakRev] type = "p6.param" - order = 20 - menu = "velocity target" - param = "VELOCITY" + order = 30 + menu = "peak reverse %" + param = "PEAK_REV_DC" bus = "rio" - [rio.FxP6.mode.position.feedFwd] + device = "fx" + [rio.FxP6.motor.duty.dead] type = "p6.param" - order = 30 - menu = "velocity feed forward" - param = "FEED_FORWARD" + order = 40 + menu = "neutral deadband (%)" + param = "NEUTRAL_DEADBAND_DC" bus = "rio" - [rio.FxP6.mode.velocity] - type = "menu" - order = 40 - menu = "velocity modes" - [rio.FxP6.mode.velocity.acceleration] + device = "fx" + [rio.FxP6.motor.duty.openRamp] type = "p6.param" - order = 20 - menu = "select acceleration target" - param = "ACCELERATION" + order = 50 + menu = "open loop ramp (%)" + param = "OPEN_LOOP_RAMP_DC" bus = "rio" - [rio.FxP6.mode.velocity.feedFwd] + device = "fx" + [rio.FxP6.motor.duty.closedRamp] type = "p6.param" - order = 30 - menu = "feed forward" - param = "FEED_FORWARD" + order = 60 + menu = "closed loop ramp (%)" + param = "CLOSED_LOOP_RAMP_DC" bus = "rio" - [rio.FxP6.mode.motion] + device = "fx" + [rio.FxP6.motor.volt] type = "menu" - order = 50 - menu = "motion magic modes" - [rio.FxP6.mode.motion.motionType] - type = "p6.mmType" + order = 40 + menu = "voltage configs" + [rio.FxP6.motor.volt.peakFwd] + type = "p6.param" order = 10 - menu = "select motion magic type" + menu = "peak forward (V)" + param = "PEAK_FWD_V" bus = "rio" - [rio.FxP6.mode.motion.feedFwd] + device = "fx" + [rio.FxP6.motor.volt.peakRev] type = "p6.param" order = 20 - menu = "select feed forward (sel unit)" - param = "FEED_FORWARD" + menu = "peak reverse (V)" + param = "PEAK_REV_V" bus = "rio" - [rio.FxP6.mode.motion.velocity] + device = "fx" + [rio.FxP6.motor.volt.supplyConst] type = "p6.param" order = 30 - menu = "select velocity target (rot/s)" - param = "VELOCITY" + menu = "supply voltage time const" + param = "SUPPLY_V_TIME_CONST" bus = "rio" - [rio.FxP6.mode.motion.acceleration] + device = "fx" + [rio.FxP6.motor.volt.openRamp] type = "p6.param" order = 40 - menu = "select acceleration target (rot/s^2)" - param = "ACCELERATION" + menu = "open loop ramp (V)" + param = "OPEN_LOOP_RAMP_V" bus = "rio" - [rio.FxP6.mode.motion.jerk] + device = "fx" + [rio.FxP6.motor.volt.closedRamp] type = "p6.param" order = 50 - menu = "select jerk target (rot/s^3)" - param = "JERK" + menu = "closed loop ramp (V)" + param = "CLOSED_LOOP_RAMP_V" bus = "rio" - [rio.FxP6.mode.diff] + device = "fx" + [rio.FxP6.motor.torque] type = "menu" - order = 60 - menu = "differential modes" - [rio.FxP6.mode.diff.diffType] - type = "p6.diffType" + order = 50 + menu = "torque current configs" + [rio.FxP6.motor.torque.peakFwd] + type = "p6.param" order = 10 - menu = "select differential type" + menu = "peak forward (A)" + param = "PEAK_FWD_I" bus = "rio" - [rio.FxP6.mode.diff.slot] + device = "fx" + [rio.FxP6.motor.torque.peakRev] type = "p6.param" order = 20 - menu = "select differential slot" - param = "DIFFERENTIAL_SLOT" + menu = "peak reverse (A)" + param = "PEAK_REV_I" bus = "rio" - [rio.FxP6.mode.diff.target] + device = "fx" + [rio.FxP6.motor.torque.dead] type = "p6.param" order = 30 - menu = "select differential target (rots)" - param = "DIFFERENTIAL_TARGET" + menu = "neutral deadband (A)" + param = "TORQUE_NEUTRAL_DEADBAND" bus = "rio" - [rio.FxP6.mode.follow] + device = "fx" + [rio.FxP6.motor.torque.openRamp] + type = "p6.param" + order = 40 + menu = "open loop ramp (A)" + param = "OPEN_LOOP_RAMP_I" + bus = "rio" + device = "fx" + [rio.FxP6.motor.torque.closedRamp] + type = "p6.param" + order = 50 + menu = "closed loop ramp (A)" + param = "CLOSED_LOOP_RAMP_I" + bus = "rio" + device = "fx" + [rio.FxP6.limits] + type = "menu" + order = 120 + menu = "setup hard/soft limits" + [rio.FxP6.limits.hard] type = "menu" - order = 70 - menu = "follower modes" - [rio.FxP6.mode.follow.followType] - type = "p6.followType" + order = 10 + menu = "hard limits" + [rio.FxP6.limits.hard.fwdEn] + type = "p6.param" order = 10 - menu = "select follower type" + menu = "FWD limit enable" + param = "FWD_HARD_EN" bus = "rio" - [rio.FxP6.mode.follow.oppose] - type = "p6.param" + device = "fx" + [rio.FxP6.limits.hard.fwdNormal] + type = "p6.fwdNorm" + order = 15 + menu = "FWD limit normal" + bus = "rio" + device = "fx" + [rio.FxP6.limits.hard.fwdSource] + type = "p6.fwdSource" order = 20 - menu = "oppose leader direction" - param = "OPPOSE_MAIN" + menu = "FWD limit source" bus = "rio" - [rio.FxP6.mode.neutral] + device = "fx" + [rio.FxP6.limits.hard.fwdRemote] + type = "p6.param" + order = 30 + menu = "FWD limit remote id" + param = "FWD_REMOTE_ID" + bus = "rio" + device = "fx" + [rio.FxP6.limits.hard.fwdAutoSet] + type = "p6.param" + order = 40 + menu = "Autoset Position on FWD limit" + param = "FWD_AUTOSET_POS" + bus = "rio" + device = "fx" + [rio.FxP6.limits.hard.fwdAutoSetValue] + type = "p6.param" + order = 50 + menu = "FWD autoset position value" + param = "FWD_AUTOSET_POS_VALUE" + bus = "rio" + device = "fx" + [rio.FxP6.limits.hard.revEn] + type = "p6.param" + order = 60 + menu = "REV limit enable" + param = "REV_HARD_EN" + bus = "rio" + device = "fx" + [rio.FxP6.limits.hard.revNormal] + type = "p6.revNorm" + order = 70 + menu = "REV limit normal" + bus = "rio" + device = "fx" + [rio.FxP6.limits.hard.revSource] + type = "p6.revSource" + order = 80 + menu = "REV limit source" + bus = "rio" + device = "fx" + [rio.FxP6.limits.hard.revRemote] + type = "p6.param" + order = 90 + menu = "REV limit remote id" + param = "REV_REMOTE_ID" + bus = "rio" + device = "fx" + [rio.FxP6.limits.hard.revAutoSet] + type = "p6.param" + order = 100 + menu = "Autoset position on REV limit" + param = "REV_AUTOSET_POS" + bus = "rio" + device = "fx" + [rio.FxP6.limits.hard.revAutoSetValue] + type = "p6.param" + order = 110 + menu = "REV autoset position value" + param = "REV_AUTOSET_POS_VALUE" + bus = "rio" + device = "fx" + [rio.FxP6.limits.soft] type = "menu" - order = 80 - memnu = "neutral modes" - [rio.FxP6.mode.neutral.select] - type = "p6.neutralOut" + order = 20 + menu = "soft limits" + [rio.FxP6.limits.soft.fwdEn] + type = "p6.param" order = 10 - menu = "select neutral output" + menu = "FWD limit enable" + param = "FWD_SOFT_EN" bus = "rio" - [rio.FxP6.mode.foc] + device = "fx" + [rio.FxP6.limits.soft.fwdThres] + type = "p6.param" + order = 20 + menu = "FWD soft limit" + param = "FWD_SOFT_THRES" + bus = "rio" + device = "fx" + [rio.FxP6.limits.soft.revEn] + type = "p6.param" + order = 30 + menu = "REV limit enable" + param = "REV_SOFT_EN" + bus = "rio" + device = "fx" + [rio.FxP6.limits.soft.revThres] + type = "p6.param" + order = 40 + menu = "REV soft limit" + param = "REV_SOFT_THRES" + bus = "rio" + device = "fx" + [rio.FxP6.audio] + type = "menu" + order = 130 + menu = "setup audio configs" + [rio.FxP6.audio.disable] type = "p6.param" - order = 100 - menu = "(pro) enable FOC" - param = "FOC" + order = 10 + menu = "allow music on disable" + param = "ALLOW_MUSIC_DIS" bus = "rio" - [rio.FxP6.mode.overrideNeut] + device = "fx" + [rio.FxP6.audio.beepBoot] type = "p6.param" - order = 110 - menu = "override neutral mode" - param = "OVERRIDE_NEUTRAL" + order = 20 + menu = "beep on boot" + param = "BEEP_ON_BOOT" bus = "rio" - [rio.FxP6.mode.limFwd] + device = "fx" + [rio.FxP6.audio.beepConfig] type = "p6.param" - order = 120 - menu = "limit forward motion" - param = "LIM_FWD_MOT" + order = 30 + menu = "beep on config" + param = "BEEP_ON_CONFIG" bus = "rio" - [rio.FxP6.mode.limRev] + device = "fx" + [rio.FxP6.factory] + type = "p6.factory" + order = 140 + menu = "config factory default" + bus = "rio" + device = "fx" + [rio.FxP6.grapher] + type = "menu" + order = 150 + menu = "set status frames" + [rio.FxP6.grapher.default] + type = "p6.graph" + order = 10 + menu = "set to default update rate" + bus = "rio" + device = "fx" + [rio.FxP6.grapher.rate] type = "p6.param" - order = 130 - menu = "limit reverse motion" - param = "LIM_REV_MOT" + order = 20 + menu = "Grapher Frame Rate (Hz)" + param = "GRAPHER_FRAME" bus = "rio" - [rio.FxP6.units] - type = "p6.units" - order = 50 - menu = "select active units" + device = "fx" + +[rio.FxsP6] + type = "menu" + order = 18 + menu = "work with talonFXS's" + [rio.FxsP6.run] + type = "p6.run" + order = 10 + menu = "run active talon fxs's" + bus = "rio" + device = "fxs" + [rio.FxsP6.select] + type = "p6.select" + order = 20 + menu = "active fxs's" + bus = "rio" + device = "fxs" + [rio.FxsP6.status] + type = "p6.status" + order = 30 + menu = "status of active talonfxs's" + bus = "rio" + device = "fxs" + [rio.FxsP6.mode] + type = "p6.modeMenu" + order = 40 + menu = "config active mode" + bus = "rio" + device = "fxs" + [rio.FxsP6.mode.status] + type = "p6.modeStatus" + order = 10 + menu = "active mode" bus = "rio" - [rio.FxP6.feedback] + device = "fxs" + [rio.FxsP6.mode.select] + type = "p6.mode" + order = 15 + menu = "select mode type" + bus = "rio" + device = "fxs" + [rio.FxsP6.mode.openLoop] type = "menu" - order = 60 - menu = "setup feedback configs" + order = 20 + menu = "open loop modes" bus = "rio" - [rio.FxP6.feedback.position] - type = "p6.param" - order = 5 - menu = "set position" - param = "POSITION" - bus = "rio" - [rio.FxP6.feedback.offset] + device = "fxs" + [rio.FxsP6.mode.openLoop.deadband] type = "p6.param" - order = 10 - menu = "set rotor offset" - param = "ROTOR_OFFSET" + order = 20 + menu = "set torque current deadband (A)" + param = "TORQUE_CURRENT_DEADBAND" bus = "rio" - [rio.FxP6.feedback.sense2mech] + device = "fxs" + [rio.FxsP6.mode.position] + type = "menu" + order = 30 + menu = "position modes" + [rio.FxsP6.mode.position.velocity] type = "p6.param" order = 20 - menu = "set sensor to mechanism ratio" - param = "SENSOR_TO_MECH_RATIO" + menu = "velocity target" + param = "VELOCITY" bus = "rio" - [rio.FxP6.feedback.rotor2sense] + device = "fxs" + [rio.FxsP6.mode.position.feedFwd] type = "p6.param" order = 30 - menu = "set rotor to sensor ratio" - param = "ROTOR_TO_SENSOR_RATIO" + menu = "velocity feed forward" + param = "FEED_FORWARD" bus = "rio" - [rio.FxP6.feedback.source] - type = "p6.feedback" - order = 35 - menu = "set feedback source" - bus = "rio" - [rio.FxP6.feedback.remoteId] - type = "p6.param" + device = "fxs" + [rio.FxsP6.mode.velocity] + type = "menu" order = 40 - menu = "set remote sensor id" - param = "REMOTE_SENSOR_ID" - bus = "rio" - [rio.FxP6.slot] + menu = "velocity modes" + [rio.FxsP6.mode.velocity.acceleration] + type = "p6.param" + order = 20 + menu = "select acceleration target" + param = "ACCELERATION" + bus = "rio" + device = "fxs" + [rio.FxsP6.mode.velocity.feedFwd] + type = "p6.param" + order = 30 + menu = "feed forward" + param = "FEED_FORWARD" + bus = "rio" + device = "fxs" + [rio.FxsP6.mode.motion] type = "menu" - order = 70 - menu = "setup slot configs" - [rio.FxP6.slot.active] - type = "p6.slot" + order = 50 + menu = "motion magic modes" + [rio.FxsP6.mode.motion.motionType] + type = "p6.mmType" order = 10 - menu = "active slot" + menu = "select motion magic type" bus = "rio" - [rio.FxP6.slot.kP] + device = "fxs" + [rio.FxsP6.mode.motion.feedFwd] type = "p6.param" order = 20 - menu = "kP" - param = "SLOT_KP" + menu = "select feed forward (sel unit)" + param = "FEED_FORWARD" bus = "rio" - [rio.FxP6.slot.kI] + device = "fxs" + [rio.FxsP6.mode.motion.velocity] type = "p6.param" order = 30 - menu = "kI" - param = "SLOT_KI" + menu = "select velocity target (rot/s)" + param = "VELOCITY" bus = "rio" - [rio.FxP6.slot.kD] + device = "fxs" + [rio.FxsP6.mode.motion.acceleration] type = "p6.param" order = 40 - menu = "kD" - param = "SLOT_KD" + menu = "select acceleration target (rot/s^2)" + param = "ACCELERATION" bus = "rio" - [rio.FxP6.slot.kS] + device = "fxs" + [rio.FxsP6.mode.motion.jerk] type = "p6.param" order = 50 - menu = "kS" - param = "SLOT_KS" + menu = "select jerk target (rot/s^3)" + param = "JERK" bus = "rio" - [rio.FxP6.slot.kV] - type = "p6.param" - order = 60 - menu = "kV" - param = "SLOT_KV" + device = "fxs" + [rio.FxsP6.mode.diff] + type = "menu" + order = 60 + menu = "differential modes" + [rio.FxsP6.mode.diff.diffType] + type = "p6.diffType" + order = 10 + menu = "select differential type" bus = "rio" - [rio.FxP6.slot.kA] + device = "fxs" + [rio.FxsP6.mode.diff.slot] type = "p6.param" - order = 65 - menu = "kA" - param = "SLOT_KA" + order = 20 + menu = "select differential slot" + param = "DIFFERENTIAL_SLOT" bus = "rio" - [rio.FxP6.slot.kG] + device = "fxs" + [rio.FxsP6.mode.diff.target] type = "p6.param" - order = 70 - menu = "kG" - param = "SLOT_KG" + order = 30 + menu = "select differential target (rots)" + param = "DIFFERENTIAL_TARGET" bus = "rio" - [rio.FxP6.slot.gravity] - type = "p6.gravity" - order = 80 - menu = "gravity type" + device = "fxs" + [rio.FxsP6.mode.follow] + type = "menu" + order = 70 + menu = "follower modes" + [rio.FxsP6.mode.follow.followType] + type = "p6.followType" + order = 10 + menu = "select follower type" bus = "rio" - [rio.FxP6.motion] + device = "fxs" + [rio.FxsP6.mode.follow.oppose] + type = "p6.param" + order = 20 + menu = "oppose leader direction" + param = "OPPOSE_MAIN" + bus = "rio" + device = "fxs" + [rio.FxsP6.mode.neutral] type = "menu" order = 80 - menu = "setup motion magic configs" - [rio.FxP6.motion.accel] + menu = "neutral modes" + [rio.FxsP6.mode.neutral.select] + type = "p6.neutralOut" + order = 10 + menu = "select neutral output" + bus = "rio" + device = "fxs" + [rio.FxsP6.mode.foc] + type = "p6.param" + order = 100 + menu = "(pro) enable FOC" + param = "FOC" + bus = "rio" + device = "fxs" + [rio.FxsP6.mode.overrideNeut] + type = "p6.param" + order = 110 + menu = "override neutral mode" + param = "OVERRIDE_NEUTRAL" + bus = "rio" + device = "fxs" + [rio.FxsP6.mode.limFwd] + type = "p6.param" + order = 120 + menu = "limit forward motion" + param = "LIM_FWD_MOT" + bus = "rio" + device = "fxs" + [rio.FxsP6.mode.limRev] + type = "p6.param" + order = 130 + menu = "limit reverse motion" + param = "LIM_REV_MOT" + bus = "rio" + device = "fxs" + [rio.FxsP6.units] + type = "p6.units" + order = 50 + menu = "select active units" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback] + type = "menu" + order = 60 + menu = "setup feedback configs" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback.position] + type = "p6.param" + order = 5 + menu = "set position" + param = "POSITION" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback.offset] + type = "p6.param" + order = 10 + menu = "set absolute sensor offset" + param = "ABS_SENSOR_OFFSET" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback.discontinuity] + type = "p6.param" + order = 15 + menu = "set absolute sensor discontinuity point" + param = "ABS_SENSOR_DISCONTINUITY" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback.edge] + type = "p6.param" + order = 17 + menu = "set quadrature edges per rotation" + param = "QUAD_EDGE_PER_ROT" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback.sense2mech] + type = "p6.param" + order = 20 + menu = "set sensor to mechanism ratio" + param = "SENSOR_TO_MECH_RATIO" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback.rotor2sense] + type = "p6.param" + order = 30 + menu = "set rotor to sensor ratio" + param = "ROTOR_TO_SENSOR_RATIO" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback.source] + type = "p6.extFeedback" + order = 35 + menu = "set external feedback source" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback.phase] + type = "p6.phase" + order = 37 + menu = "set sensor phase" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback.remoteId] + type = "p6.param" + order = 40 + menu = "set remote sensor id" + param = "REMOTE_SENSOR_ID" + bus = "rio" + device = "fxs" + [rio.FxsP6.feedback.vFiltTC] + type = "p6.param" + order = 45 + menu = "set velocity filter time constant" + param = "VELOCITY_FILTER_TIME_CONST" + bus = "rio" + device = "fxs" + [rio.FxsP6.slot] + type = "menu" + order = 70 + menu = "setup slot configs" + [rio.FxsP6.slot.active] + type = "p6.slot" + order = 10 + menu = "active slot" + bus = "rio" + device = "fxs" + [rio.FxsP6.slot.kP] + type = "p6.param" + order = 20 + menu = "kP" + param = "SLOT_KP" + bus = "rio" + device = "fxs" + [rio.FxsP6.slot.kI] + type = "p6.param" + order = 30 + menu = "kI" + param = "SLOT_KI" + bus = "rio" + device = "fxs" + [rio.FxsP6.slot.kD] + type = "p6.param" + order = 40 + menu = "kD" + param = "SLOT_KD" + bus = "rio" + device = "fxs" + [rio.FxsP6.slot.kS] + type = "p6.param" + order = 50 + menu = "kS" + param = "SLOT_KS" + bus = "rio" + device = "fxs" + [rio.FxsP6.slot.kV] + type = "p6.param" + order = 60 + menu = "kV" + param = "SLOT_KV" + bus = "rio" + device = "fxs" + [rio.FxsP6.slot.kA] + type = "p6.param" + order = 65 + menu = "kA" + param = "SLOT_KA" + bus = "rio" + device = "fxs" + [rio.FxsP6.slot.kG] + type = "p6.param" + order = 70 + menu = "kG" + param = "SLOT_KG" + bus = "rio" + device = "fxs" + [rio.FxsP6.slot.gravity] + type = "p6.gravity" + order = 80 + menu = "gravity type" + bus = "rio" + device = "fxs" + [rio.FxsP6.motion] + type = "menu" + order = 80 + menu = "setup motion magic configs" + [rio.FxsP6.motion.accel] + type = "p6.param" + order = 10 + menu = "acceleration" + param = "MM_ACCEL" + bus = "rio" + device = "fxs" + [rio.FxsP6.motion.cruise] + type = "p6.param" + order = 20 + menu = "cruise velocity" + param = "MM_CRUISE_VEL" + bus = "rio" + device = "fxs" + [rio.FxsP6.motion.jerk] + type = "p6.param" + order = 30 + menu = "jerk" + param = "MM_JERK" + bus = "rio" + device = "fxs" + [rio.FxsP6.motion.kA] + type = "p6.param" + order = 40 + menu = "Expo kA" + param = "MM_EXPO_KA" + bus = "rio" + device = "fxs" + [rio.FxsP6.motion.kV] + type = "p6.param" + order = 50 + menu = "Expo kV" + param = "MM_EXPO_KV" + bus = "rio" + device = "fxs" + [rio.FxsP6.diff] + type = "menu" + order = 90 + menu = "setup differential configs" + [rio.FxsP6.diff.duty] + type = "p6.param" + order = 10 + menu = "peak differential duty cycle" + param = "PEAK_DIFF_DC" + bus = "rio" + device = "fxs" + [rio.FxsP6.diff.torque] + type = "p6.param" + order = 20 + menu = "peak differential torque current" + param = "PEAK_DIFF_TORQUE" + bus = "rio" + device = "fxs" + [rio.FxsP6.diff.volt] + type = "p6.param" + order = 30 + menu = "peak differential voltage" + param = "PEAK_DIFF_VOLT" + bus = "rio" + device = "fxs" + [rio.FxsP6.diff.remote] + type = "p6.param" + order = 40 + menu = "differential remote id" + param = "DIFF_SENSOR_REMOTE_ID" + bus = "rio" + device = "fxs" + [rio.FxsP6.diff.fx] + type = "p6.param" + order = 50 + menu = "differential fx id" + param = "DIFF_FX_ID" + bus = "rio" + device = "fxs" + [rio.FxsP6.diff.source] + type = "p6.diffSource" + order = 60 + menu = "differential source" + bus = "rio" + device = "fxs" + [rio.FxsP6.current] + type = "menu" + order = 100 + menu = "setup current limit configs" + [rio.FxsP6.current.statorEn] type = "p6.param" order = 10 - menu = "acceleration" - param = "MM_ACCEL" + menu = "stator limit enable" + param = "STATOR_LIM_EN" bus = "rio" - [rio.FxP6.motion.cruise] + device = "fxs" + [rio.FxsP6.current.statorLim] type = "p6.param" order = 20 - menu = "cruise velocity" - param = "MM_CRUISE_VEL" + menu = "stator current limit" + param = "STATOR_LIM" bus = "rio" - [rio.FxP6.motion.jerk] + device = "fxs" + [rio.FxsP6.current.suppEn] type = "p6.param" order = 30 - menu = "jerk" - param = "MM_JERK" + menu = "supply limit enable" + param = "SUPP_LIM_EN" bus = "rio" - [rio.FxP6.motion.kA] + device = "fxs" + [rio.FxsP6.current.suppLim] type = "p6.param" order = 40 - menu = "Expo kA" - param = "MM_EXPO_KA" + menu = "supply current lower limit" + param = "SUPP_LOWER_LIM" bus = "rio" - [rio.FxP6.motion.kV] + device = "fxs" + [rio.FxsP6.current.tripI] type = "p6.param" order = 50 - menu = "Expo kV" - param = "MM_EXPO_KV" + menu = "supply current limit (A)" + param = "SUPP_LIM" bus = "rio" - [rio.FxP6.diff] - type = "menu" - order = 90 - menu = "setup differential configs" - [rio.FxP6.diff.duty] + device = "fxs" + [rio.FxsP6.current.tripT] type = "p6.param" - order = 10 - menu = "peak differential duty cycle" - param = "PEAK_DIFF_DC" + order = 60 + menu = "supply current trip time (s)" + param = "SUPP_TRIP_TIME" bus = "rio" - [rio.FxP6.diff.torque] - type = "p6.param" + device = "fxs" + [rio.FxsP6.motor] + type = "menu" + order = 110 + menu = "setup motor output and ramp configs" + [rio.FxsP6.motor.invert] + type = "p6.invert" + order = 10 + menu = "motor inversion" + bus = "rio" + device = "fxs" + [rio.FxsP6.motor.neutral] + type = "p6.neutral" + order = 20 + menu = "neutral mode" + bus = "rio" + device = "fxs" + [rio.FxsP6.motor.duty] + type = "menu" + order = 30 + menu = "duty cycle configs" + bus = "rio" + device = "fxs" + [rio.FxsP6.motor.duty.peakFwd] + type= "p6.param" order = 20 - menu = "peak differential torque current" - param = "PEAK_DIFF_TORQUE" + menu = "peak forward %" + param = "PEAK_FWD_DC" bus = "rio" - [rio.FxP6.diff.volt] + device = "fxs" + [rio.FxsP6.motor.duty.peakRev] type = "p6.param" order = 30 - menu = "peak differential voltage" - param = "PEAK_DIFF_VOLT" + menu = "peak reverse %" + param = "PEAK_REV_DC" bus = "rio" - [rio.FxP6.diff.remote] + device = "fxs" + [rio.FxsP6.motor.duty.dead] type = "p6.param" order = 40 - menu = "differential remote id" - param = "DIFF_SENSOR_REMOTE_ID" + menu = "neutral deadband (%)" + param = "NEUTRAL_DEADBAND_DC" bus = "rio" - [rio.FxP6.diff.fx] + device = "fxs" + [rio.FxsP6.motor.duty.openRamp] type = "p6.param" order = 50 - menu = "differential fx id" - param = "DIFF_FX_ID" + menu = "open loop ramp (%)" + param = "OPEN_LOOP_RAMP_DC" bus = "rio" - [rio.FxP6.current] + device = "fxs" + [rio.FxsP6.motor.duty.closedRamp] + type = "p6.param" + order = 60 + menu = "closed loop ramp (%)" + param = "CLOSED_LOOP_RAMP_DC" + bus = "rio" + device = "fxs" + [rio.FxsP6.motor.volt] type = "menu" - order = 100 - menu = "setup current limit configs" - [rio.FxP6.current.statorEn] + order = 40 + menu = "voltage configs" + [rio.FxsP6.motor.volt.peakFwd] type = "p6.param" order = 10 - menu = "stator limit enable" - param = "STATOR_LIM_EN" + menu = "peak forward (V)" + param = "PEAK_FWD_V" bus = "rio" - [rio.FxP6.current.statorLim] + device = "fxs" + [rio.FxsP6.motor.volt.peakRev] type = "p6.param" order = 20 - menu = "stator current limit" - param = "STATOR_LIM" + menu = "peak reverse (V)" + param = "PEAK_REV_V" bus = "rio" - [rio.FxP6.current.suppEn] + device = "fxs" + [rio.FxsP6.motor.volt.supplyConst] type = "p6.param" order = 30 - menu = "supply limit enable" - param = "SUPP_LIM_EN" + menu = "supply voltage time const" + param = "SUPPLY_V_TIME_CONST" bus = "rio" - [rio.FxP6.current.suppLim] + device = "fxs" + [rio.FxsP6.motor.volt.openRamp] type = "p6.param" order = 40 - menu = "supply current limit" - param = "SUPP_LIM" + menu = "open loop ramp (V)" + param = "OPEN_LOOP_RAMP_V" bus = "rio" - [rio.FxP6.current.tripI] + device = "fxs" + [rio.FxsP6.motor.volt.closedRamp] type = "p6.param" order = 50 - menu = "supply current trip threshold (A)" - param = "SUPP_TRIP_THRES" + menu = "closed loop ramp (V)" + param = "CLOSED_LOOP_RAMP_V" bus = "rio" - [rio.FxP6.current.tripT] + device = "fxs" + [rio.FxsP6.motor.torque] + type = "menu" + order = 50 + menu = "torque current configs" + [rio.FxsP6.motor.torque.openRamp] type = "p6.param" - order = 60 - menu = "supply current trip time (s)" - param = "SUPP_TRIP_TIME" + order = 40 + menu = "open loop ramp (A)" + param = "OPEN_LOOP_RAMP_I" bus = "rio" - [rio.FxP6.motor] + device = "fxs" + [rio.FxsP6.motor.torque.closedRamp] + type = "p6.param" + order = 50 + menu = "closed loop ramp (A)" + param = "CLOSED_LOOP_RAMP_I" + bus = "rio" + device = "fxs" + [rio.FxsP6.motor.arrange] + type = "p6.motorArrange" + order = 60 + menu = "set attached motor" + bus = "rio" + device = "fxs" + [rio.FxsP6.motor.wire] + type = "p6.wiring" + order = 70 + menu = "set brushed motor wiring config" + bus = "rio" + device = "fxs" + [rio.FxsP6.motor.advHall] + type = "p6.advHall" + order = 80 + menu = "enable advanced hall support" + bus = "rio" + device = "fxs" + + [rio.FxsP6.limits] + type = "menu" + order = 120 + menu = "setup hard/soft limits" + [rio.FxsP6.limits.hard] type = "menu" - order = 110 - menu = "setup motor output and ramp configs" - [rio.FxP6.motor.invert] - type = "p6.invert" + order = 10 + menu = "hard limits" + [rio.FxsP6.limits.hard.fwdEn] + type = "p6.param" order = 10 - menu = "motor inversion" + menu = "FWD limit enable" + param = "FWD_HARD_EN" bus = "rio" - [rio.FxP6.motor.neutral] - type = "p6.neutral" + device = "fxs" + [rio.FxsP6.limits.hard.fwdNormal] + type = "p6.fwdNorm" + order = 15 + menu = "FWD limit normal" + bus = "rio" + device = "fxs" + [rio.FxsP6.limits.hard.fwdSource] + type = "p6.fwdSource" order = 20 - menu = "neutral mode" + menu = "FWD limit source" bus = "rio" - [rio.FxP6.motor.duty] - type = "menu" + device = "fxs" + [rio.FxsP6.limits.hard.fwdRemote] + type = "p6.param" order = 30 - menu = "duty cycle configs" + menu = "FWD limit remote id" + param = "FWD_REMOTE_ID" bus = "rio" - [rio.FxP6.motor.duty.peakFwd] - type= "p6.param" - order = 20 - menu = "peak forward %" - param = "PEAK_FWD_DC" - bus = "rio" - [rio.FxP6.motor.duty.peakRev] - type = "p6.param" - order = 30 - menu = "peak reverse %" - param = "PEAK_REV_DC" - bus = "rio" - [rio.FxP6.motor.duty.dead] - type = "p6.param" - order = 40 - menu = "neutral deadband (%)" - param = "NEUTRAL_DEADBAND_DC" - bus = "rio" - [rio.FxP6.motor.duty.openRamp] - type = "p6.param" - order = 50 - menu = "open loop ramp (%)" - param = "OPEN_LOOP_RAMP_DC" - bus = "rio" - [rio.FxP6.motor.duty.closedRamp] - type = "p6.param" - order = 60 - menu = "closed loop ramp (%)" - param = "CLOSED_LOOP_RAMP_DC" - bus = "rio" - [rio.FxP6.motor.volt] - type = "menu" + device = "fxs" + [rio.FxsP6.limits.hard.fwdAutoSet] + type = "p6.param" order = 40 - menu = "voltage configs" - [rio.FxP6.motor.volt.peakFwd] - type = "p6.param" - order = 10 - menu = "peak forward (V)" - param = "PEAK_FWD_V" - bus = "rio" - [rio.FxP6.motor.volt.peakRev] - type = "p6.param" - order = 20 - menu = "peak reverse (V)" - param = "PEAK_REV_V" - bus = "rio" - [rio.FxP6.motor.volt.supplyConst] - type = "p6.param" - order = 30 - menu = "supply voltage time const" - param = "SUPPLY_V_TIME_CONST" - bus = "rio" - [rio.FxP6.motor.volt.openRamp] - type = "p6.param" - order = 40 - menu = "open loop ramp (V)" - param = "OPEN_LOOP_RAMP_V" - bus = "rio" - [rio.FxP6.motor.volt.closedRamp] - type = "p6.param" - order = 50 - menu = "closed loop ramp (V)" - param = "CLOSED_LOOP_RAMP_V" - bus = "rio" - [rio.FxP6.motor.torque] - type = "menu" + menu = "Autoset Position on FWD limit" + param = "FWD_AUTOSET_POS" + bus = "rio" + device = "fxs" + [rio.FxsP6.limits.hard.fwdAutoSetValue] + type = "p6.param" order = 50 - menu = "torque current configs" - [rio.FxP6.motor.torque.peakFwd] - type = "p6.param" - order = 10 - menu = "peak forward (A)" - param = "PEAK_FWD_I" - bus = "rio" - [rio.FxP6.motor.torque.peakRev] - type = "p6.param" - order = 20 - menu = "peak reverse (A)" - param = "PEAK_REV_I" - bus = "rio" - [rio.FxP6.motor.torque.dead] - type = "p6.param" - order = 30 - menu = "neutral deadband (A)" - param = "TORQUE_NEUTRAL_DEADBAND" - bus = "rio" - [rio.FxP6.motor.openRamp] - type = "p6.param" - order = 40 - menu = "open loop ramp (A)" - param = "OPEN_LOOP_RAMP_I" - bus = "rio" - [rio.FxP6.motor.closedRamp] - type = "p6.param" - order = 50 - menuu = "closed loop ramp (A)" - param = "CLOSED_LOOP_RAMP_I" - bus = "rio" - [rio.FxP6.limits] - type = "menu" - order = 120 - menu = "setup hard/soft limits" - [rio.FxP6.limits.hard] - type = "menu" - order = 10 - menu = "hard limits" - [rio.FxP6.limits.hard.fwdEn] - type = "p6.param" - order = 10 - menu = "FWD limit enable" - param = "FWD_HARD_EN" - bus = "rio" - [rio.FxP6.limits.hard.fwdNormal] - type = "p6.fwdNorm" - order = 10 - menu = "FWD limit normal" - bus = "rio" - [rio.FxP6.limits.hard.fwdSource] - type = "p6.fwdSource" - order = 20 - menu = "FWD limit source" - bus = "rio" - [rio.FxP6.limits.hard.fwdRemote] - type = "p6.param" - order = 30 - menu = "FWD limit remote id" - param = "FWD_REMOTE_ID" - bus = "rio" - [rio.FxP6.limits.hard.fwdAutoSet] - type = "p6.param" - order = 40 - menu = "Autoset Position on FWD limit" - param = "FWD_AUTOSET_POS" - bus = "rio" - [rio.FxP6.limits.hard.fwdAutoSetValue] - type = "p6.param" - order = 50 - menu = "FWD autoset position value" - param = "FWD_AUTOSET_POS_VALUE" - bus = "rio" - [rio.FxP6.limits.hard.revEn] - type = "p6.param" - order = 60 - menu = "REV limit enable" - param = "REV_HARD_EN" - bus = "rio" - [rio.FxP6.limits.hard.revNormal] - type = "p6.revNorm" - order = 70 - menu = "REV limit normal" - bus = "rio" - [rio.FxP6.limits.hard.revSource] - type = "p6.revSource" - order = 80 - menu = "REV limit source" - bus = "rio" - [rio.FxP6.limits.hard.revRemote] - type = "p6.param" - order = 90 - menu = "REV limit remote id" - param = "REV_REMOTE_ID" - bus = "rio" - [rio.FxP6.limits.hard.revAutoSet] - type = "p6.param" - order = 100 - menu = "Autoset position on REV limit" - param = "REV_AUTOSET_POS" - bus = "rio" - [rio.FxP6.limits.hard.revAutoSetValue] - type = "p6.param" - order = 110 - menu = "REV autoset position value" - param = "REV_AUTOSET_POS_VALUE" - bus = "rio" - [rio.FxP6.limits.soft] - type = "menu" - order = 20 - menu = "soft limits" - [rio.FxP6.limits.soft.fwdEn] - type = "p6.param" - order = 10 - menu = "FWD limit enable" - param = "FWD_SOFT_EN" - bus = "rio" - [rio.FxP6.limits.soft.fwdThres] - type = "p6.param" - order = 20 - menu = "FWD soft limit" - param = "FWD_SOFT_THRES" - bus = "rio" - [rio.FxP6.limits.soft.revEn] - type = "p6.param" - order = 30 - menu = "REV limit enable" - param = "REV_SOFT_EN" - bus = "rio" - [rio.FxP6.limits.soft.revThres] - type = "p6.param" - order = 40 - menu = "REV soft limit" - param = "REV_SOFT_THRES" - bus = "rio" - [rio.FxP6.audio] - type = "menu" - order = 130 - menu = "setup audio configs" - [rio.FxP6.audio.disable] + menu = "FWD autoset position value" + param = "FWD_AUTOSET_POS_VALUE" + bus = "rio" + device = "fxs" + [rio.FxsP6.limits.hard.revEn] type = "p6.param" - order = 10 - menu = "allow music on disable" - param = "ALLOW_MUSIC_DIS" + order = 60 + menu = "REV limit enable" + param = "REV_HARD_EN" bus = "rio" - [rio.FxP6.audio.beepBoot] + device = "fxs" + [rio.FxsP6.limits.hard.revNormal] + type = "p6.revNorm" + order = 70 + menu = "REV limit normal" + bus = "rio" + device = "fxs" + [rio.FxsP6.limits.hard.revSource] + type = "p6.revSource" + order = 80 + menu = "REV limit source" + bus = "rio" + device = "fxs" + [rio.FxsP6.limits.hard.revRemote] type = "p6.param" - order = 20 - menu = "beep on boot" - param = "BEEP_ON_BOOT" + order = 90 + menu = "REV limit remote id" + param = "REV_REMOTE_ID" bus = "rio" - [rio.FxP6.audio.beepConfig] + device = "fxs" + [rio.FxsP6.limits.hard.revAutoSet] type = "p6.param" - order = 30 - menu = "beep on config" - param = "BEEP_ON_CONFIG" + order = 100 + menu = "Autoset position on REV limit" + param = "REV_AUTOSET_POS" bus = "rio" - [rio.FxP6.factory] - type = "p6.factory" - order = 140 - menu = "config factory default" - bus = "rio" - [rio.FxP6.grapher] + device = "fxs" + [rio.FxsP6.limits.hard.revAutoSetValue] + type = "p6.param" + order = 110 + menu = "REV autoset position value" + param = "REV_AUTOSET_POS_VALUE" + bus = "rio" + device = "fxs" + [rio.FxsP6.limits.soft] type = "menu" - order = 150 - menu = "set status frames" - [rio.FxP6.grapher.default] - type = "p6.graph" + order = 20 + menu = "soft limits" + [rio.FxsP6.limits.soft.fwdEn] + type = "p6.param" order = 10 - menu = "set to default update rate" + menu = "FWD limit enable" + param = "FWD_SOFT_EN" bus = "rio" - [rio.FxP6.grapher.rate] + device = "fxs" + [rio.FxsP6.limits.soft.fwdThres] type = "p6.param" order = 20 - menu = "Grapher Frame Rate (Hz)" - param = "GRAPHER_FRAME" + menu = "FWD soft limit" + param = "FWD_SOFT_THRES" bus = "rio" - - [rio.servo] + device = "fxs" + [rio.FxsP6.limits.soft.revEn] + type = "p6.param" + order = 30 + menu = "REV limit enable" + param = "REV_SOFT_EN" + bus = "rio" + device = "fxs" + [rio.FxsP6.limits.soft.revThres] + type = "p6.param" + order = 40 + menu = "REV soft limit" + param = "REV_SOFT_THRES" + bus = "rio" + device = "fxs" + [rio.FxsP6.audio] type = "menu" - order = 20 - menu = "work with servos" - [rio.servo.run] - type = "servo.run" + order = 130 + menu = "setup audio configs" + [rio.FxsP6.audio.disable] + type = "p6.param" order = 10 - menu = "set servo positions" - [rio.servo.select] - type = "servo.select" + menu = "allow music on disable" + param = "ALLOW_MUSIC_DIS" + bus = "rio" + device = "fxs" + [rio.FxsP6.audio.beepBoot] + type = "p6.param" + order = 20 + menu = "beep on boot" + param = "BEEP_ON_BOOT" + bus = "rio" + device = "fxs" + [rio.FxsP6.audio.beepConfig] + type = "p6.param" + order = 30 + menu = "beep on config" + param = "BEEP_ON_CONFIG" + bus = "rio" + device = "fxs" + [rio.FxsP6.factory] + type = "p6.factory" + order = 140 + menu = "config factory default" + bus = "rio" + device = "fxs" + [rio.FxsP6.grapher] + type = "menu" + order = 150 + menu = "set status frames" + [rio.FxsP6.grapher.default] + type = "p6.graph" + order = 10 + menu = "set to default update rate" + bus = "rio" + device = "fxs" + [rio.FxsP6.grapher.rate] + type = "p6.param" + order = 20 + menu = "Grapher Frame Rate (Hz)" + param = "GRAPHER_FRAME" + bus = "rio" + device = "fxs" + + + [rio.servo] + type = "menu" + order = 20 + menu = "work with servos" + [rio.servo.run] + type = "servo.run" + order = 10 + menu = "set servo positions" + [rio.servo.select] + type = "servo.select" order = 20 menu = "active servos" @@ -1783,6 +2673,36 @@ type = "menu" order = 30 menu = "active azimuth adjustment" + [rio.FXswerve] + type = "menu" + order = 55 + menu = "work with FX swerve" + [rio.FXswerve.lock] + type = "p6.swerve.azimuth" + order = 10 + bus = "rio" + menu = "lock azimuth position" + [rio.FXswerve.azimuth] + type = "menu" + order = 20 + bus = "rio" + menu = "update azimuth zero positions" + [rio.FXswerve.azimuth.save] + type = "p6.swerve.azimuth.save" + order = 10 + bus = "rio" + menu = "save azimuth zero positions" + [rio.FXswerve.azimuth.select] + type = "p6.swerve.azimuth.select" + order = 20 + bus = "rio" + menu = "active azimuth" + [rio.FXswerve.azimuth.adjust] + type = "p6.swerve.azimuth.adjust" + order = 30 + bus = "rio" + menu = "active azimuth adjustment" + [rio.gyro] type = "menu" order = 60 @@ -1876,698 +2796,1587 @@ type = "menu" [rio.gyro.pigeon.defaults] type = "pigeon.param" order = 30 - menu = "config factory defaults" - param = "FACTORY_DEFAULT" - - [rio.cancoder] - type = "menu" - order = 70 - menu = "work with CANcoders" - [rio.cancoder.select] - type = "cancoder.select" - order = 10 - menu = "select active CANcoders" - bus = "rio" - [rio.cancoder.status] - type = "cancoder.status" - order = 20 - menu = "status of active CANcoders" - bus = "rio" - [rio.cancoder.position] - type = "cancoder.param" - param = "POSITION" - order = 30 - menu = "set CANcoder position" - bus = "rio" - [rio.cancoder.magOffset] - type = "cancoder.param" - param = "MAG_OFFSET" - order = 40 - bus = "rio" - [rio.cancoder.absRange] - type = "cancoder.absRange" - order = 50 - bus = "rio" - [rio.cancoder.sensorDirection] - type = "cancoder.sensorDirection" - order = 60 - bus = "rio" - - + menu = "config factory defaults" + param = "FACTORY_DEFAULT" + + [rio.cancoder] + type = "menu" + order = 70 + menu = "work with CANcoders" + [rio.cancoder.select] + type = "cancoder.select" + order = 10 + menu = "select active CANcoders" + bus = "rio" + [rio.cancoder.status] + type = "cancoder.status" + order = 20 + menu = "status of active CANcoders" + bus = "rio" + [rio.cancoder.position] + type = "cancoder.param" + param = "POSITION" + order = 30 + menu = "set CANcoder position" + bus = "rio" + [rio.cancoder.magOffset] + type = "cancoder.param" + param = "MAG_OFFSET" + order = 40 + bus = "rio" +# [rio.cancoder.absRange] +# type = "cancoder.absRange" +# order = 50 +# bus = "rio" + [rio.cancoder.sensorDirection] + type = "cancoder.sensorDirection" + order = 60 + bus = "rio" + + + +[canivore] + type = "menu" + order = 20 + menu = "work on CANivore" + [canivore.FxP6] + type = "menu" + order = 17 + menu = "work with talonFX's" + [canivore.FxP6.run] + type = "p6.run" + order = 10 + menu = "run active talon fx's" + bus = "canivore" + device = "fx" + [canivore.FxP6.select] + type = "p6.select" + order = 20 + menu = "active fx's" + bus = "canivore" + device = "fx" + [canivore.FxP6.status] + type = "p6.status" + order = 30 + menu = "status of active talonFx's" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode] + type = "p6.modeMenu" + order = 40 + menu = "config active mode" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.status] + type = "p6.modeStatus" + order = 10 + menu = "active mode" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.select] + type = "p6.mode" + order = 15 + menu = "select mode type" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.openLoop] + type = "menu" + order = 20 + menu = "open loop modes" + [canivore.FxP6.mode.openLoop.deadband] + type = "p6.param" + order = 20 + menu = "set torque current deadband (A)" + param = "TORQUE_CURRENT_DEADBAND" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.position] + type = "menu" + order = 30 + menu = "position modes" + [canivore.FxP6.mode.position.velocity] + type = "p6.param" + order = 20 + menu = "velocity target" + param = "VELOCITY" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.position.feedFwd] + type = "p6.param" + order = 30 + menu = "velocity feed forward" + param = "FEED_FORWARD" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.velocity] + type = "menu" + order = 40 + menu = "velocity modes" + [canivore.FxP6.mode.velocity.acceleration] + type = "p6.param" + order = 20 + menu = "select acceleration target" + param = "ACCELERATION" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.velocity.feedFwd] + type = "p6.param" + order = 30 + menu = "feed forward" + param = "FEED_FORWARD" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.motion] + type = "menu" + order = 50 + menu = "motion magic modes" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.motion.motionType] + type = "p6.mmType" + order = 10 + menu = "select motion magic type" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.motion.feedFwd] + type = "p6.param" + order = 20 + menu = "select feed forward (sel unit)" + param = "FEED_FORWARD" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.motion.velocity] + type = "p6.param" + order = 30 + menu = "select velocity target (rot/s)" + param = "VELOCITY" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.motion.acceleration] + type = "p6.param" + order = 40 + menu = "select acceleration target (rot/s^2)" + param = "ACCELERATION" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.motion.jerk] + type = "p6.param" + order = 50 + menu = "select jerk target (rot/s^3)" + param = "JERK" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.diff] + type = "menu" + order = 60 + menu = "differential modes" + [canivore.FxP6.mode.diff.diffType] + type = "p6.diffType" + order = 10 + menu = "select differential type" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.diff.slot] + type = "p6.param" + order = 20 + menu = "select differential slot" + param = "DIFFERENTIAL_SLOT" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.diff.target] + type = "p6.param" + order = 30 + menu = "select differential target (rots)" + param = "DIFFERENTIAL_TARGET" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.follow] + type = "menu" + order = 70 + menu = "follower modes" + [canivore.FxP6.mode.follow.followType] + type = "p6.followType" + order = 10 + menu = "select follower type" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.follow.oppose] + type = "p6.param" + order = 20 + menu = "oppose leader direction" + param = "OPPOSE_MAIN" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.neutral] + type = "menu" + order = 80 + menu = "neutral modes" + [canivore.FxP6.mode.neutral.select] + type = "p6.neutralOut" + order = 10 + menu = "select neutral output" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.foc] + type = "p6.param" + order = 100 + menu = "(pro) enable FOC" + param = "FOC" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.overrideNeut] + type = "p6.param" + order = 110 + menu = "override neutral mode" + param = "OVERRIDE_NEUTRAL" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.limFwd] + type = "p6.param" + order = 120 + menu = "limit forward motion" + param = "LIM_FWD_MOT" + bus = "canivore" + device = "fx" + [canivore.FxP6.mode.limRev] + type = "p6.param" + order = 130 + menu = "limit reverse motion" + param = "LIM_REV_MOT" + bus = "canivore" + device = "fx" + [canivore.FxP6.units] + type = "p6.units" + order = 50 + menu = "select active units" + bus = "canivore" + device = "fx" + [canivore.FxP6.feedback] + type = "menu" + order = 60 + menu = "setup feedback configs" + [canivore.FxP6.feedback.position] + type = "p6.param" + order = 5 + menu = "set position" + param = "POSITION" + bus = "canivore" + device = "fx" + [canivore.FxP6.feedback.offset] + type = "p6.param" + order = 10 + menu = "set rotor offset" + param = "ROTOR_OFFSET" + bus = "canivore" + device = "fx" + [canivore.FxP6.feedback.sense2mech] + type = "p6.param" + order = 20 + menu = "set sensor to mechanism ratio" + param = "SENSOR_TO_MECH_RATIO" + bus = "canivore" + device = "fx" + [canivore.FxP6.feedback.rotor2sense] + type = "p6.param" + order = 30 + menu = "set rotor to sensor ratio" + param = "ROTOR_TO_SENSOR_RATIO" + bus = "canivore" + device = "fx" + [canivore.FxP6.feedback.source] + type = "p6.feedback" + order = 35 + menu = "set feedback source" + bus = "canivore" + device = "fx" + [canivore.FxP6.feedback.remoteId] + type = "p6.param" + order = 40 + menu = "set remote sensor id" + param = "REMOTE_SENSOR_ID" + bus = "canivore" + device = "fx" + [canivore.FxP6.slot] + type = "menu" + order = 70 + menu = "setup slot configs" + [canivore.FxP6.slot.active] + type = "p6.slot" + order = 10 + menu = "active slot" + bus = "canivore" + device = "fx" + [canivore.FxP6.slot.kP] + type = "p6.param" + order = 20 + menu = "kP" + param = "SLOT_KP" + bus = "canivore" + device = "fx" + [canivore.FxP6.slot.kI] + type = "p6.param" + order = 30 + menu = "kI" + param = "SLOT_KI" + bus = "canivore" + device = "fx" + [canivore.FxP6.slot.kD] + type = "p6.param" + order = 40 + menu = "kD" + param = "SLOT_KD" + bus = "canivore" + device = "fx" + [canivore.FxP6.slot.kS] + type = "p6.param" + order = 50 + menu = "kS" + param = "SLOT_KS" + bus = "canivore" + device = "fx" + [canivore.FxP6.slot.kV] + type = "p6.param" + order = 60 + menu = "kV" + param = "SLOT_KV" + bus = "canivore" + device = "fx" + [canivore.FxP6.slot.kA] + type = "p6.param" + order = 65 + menu = "kA" + param = "SLOT_KA" + bus = "canivore" + device = "fx" + [canivore.FxP6.slot.kG] + type = "p6.param" + order = 70 + menu = "kG" + param = "SLOT_KG" + bus = "canivore" + device = "fx" + [canivore.FxP6.slot.gravity] + type = "p6.gravity" + order = 80 + menu = "gravity type" + bus = "canivore" + device = "fx" + [canivore.FxP6.motion] + type = "menu" + order = 80 + menu = "setup motion magic configs" + [canivore.FxP6.motion.accel] + type = "p6.param" + order = 10 + menu = "acceleration" + param = "MM_ACCEL" + bus = "canivore" + device = "fx" + [canivore.FxP6.motion.cruise] + type = "p6.param" + order = 20 + menu = "cruise velocity" + param = "MM_CRUISE_VEL" + bus = "canivore" + device = "fx" + [canivore.FxP6.motion.jerk] + type = "p6.param" + order = 30 + menu = "jerk" + param = "MM_JERK" + bus = "canivore" + device = "fx" + [canivore.FxP6.motion.kA] + type = "p6.param" + order = 40 + menu = "Expo kA" + param = "MM_EXPO_KA" + bus = "canivore" + device = "fx" + [canivore.FxP6.motion.kV] + type = "p6.param" + order = 50 + menu = "Expo kV" + param = "MM_EXPO_KV" + bus = "canivore" + device = "fx" + [canivore.FxP6.diff] + type = "menu" + order = 90 + menu = "setup differential configs" + [canivore.FxP6.diff.duty] + type = "p6.param" + order = 10 + menu = "peak differential duty cycle" + param = "PEAK_DIFF_DC" + bus = "canivore" + device = "fx" + [canivore.FxP6.diff.torque] + type = "p6.param" + order = 20 + menu = "peak differential torque current" + param = "PEAK_DIFF_TORQUE" + bus = "canivore" + device = "fx" + [canivore.FxP6.diff.volt] + type = "p6.param" + order = 30 + menu = "peak differential voltage" + param = "PEAK_DIFF_VOLT" + bus = "canivore" + device = "fx" + [canivore.FxP6.diff.remote] + type = "p6.param" + order = 40 + menu = "differential remote id" + param = "DIFF_SENSOR_REMOTE_ID" + bus = "canivore" + device = "fx" + [canivore.FxP6.diff.fx] + type = "p6.param" + order = 50 + menu = "differential fx id" + param = "DIFF_FX_ID" + bus = "canivore" + device = "fx" + [canivore.FxP6.diff.source] + type = "p6.diffSource" + order = 60 + menu = "differential source" + bus = "canivore" + device = "fx" + [canivore.FxP6.current] + type = "menu" + order = 100 + menu = "setup current limit configs" + [canivore.FxP6.current.statorEn] + type = "p6.param" + order = 10 + menu = "stator limit enable" + param = "STATOR_LIM_EN" + bus = "canivore" + device = "fx" + [canivore.FxP6.current.statorLim] + type = "p6.param" + order = 20 + menu = "stator current limit" + param = "STATOR_LIM" + bus = "canivore" + device = "fx" + [canivore.FxP6.current.suppEn] + type = "p6.param" + order = 30 + menu = "supply limit enable" + param = "SUPP_LIM_EN" + bus = "canivore" + device = "fx" + [canivore.FxP6.current.suppLim] + type = "p6.param" + order = 40 + menu = "supply current lower limit" + param = "SUPP_LOWER_LIM" + bus = "canivore" + device = "fx" + [canivore.FxP6.current.tripI] + type = "p6.param" + order = 50 + menu = "supply current limit (A)" + param = "SUPP_LIM" + bus = "canivore" + device = "fx" + [canivore.FxP6.current.tripT] + type = "p6.param" + order = 60 + menu = "supply current trip time (s)" + param = "SUPP_TRIP_TIME" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor] + type = "menu" + order = 110 + menu = "setup motor output and ramp configs" + [canivore.FxP6.motor.invert] + type = "p6.invert" + order = 10 + menu = "motor inversion" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.neutral] + type = "p6.neutral" + order = 20 + menu = "neutral mode" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.duty] + type = "menu" + order = 30 + menu = "duty cycle configs" + [canivore.FxP6.motor.duty.peakFwd] + type= "p6.param" + order = 20 + menu = "peak forward %" + param = "PEAK_FWD_DC" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.duty.peakRev] + type = "p6.param" + order = 30 + menu = "peak reverse %" + param = "PEAK_REV_DC" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.duty.dead] + type = "p6.param" + order = 40 + menu = "neutral deadband (%)" + param = "NEUTRAL_DEADBAND_DC" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.duty.openRamp] + type = "p6.param" + order = 50 + menu = "open loop ramp (%)" + param = "OPEN_LOOP_RAMP_DC" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.duty.closedRamp] + type = "p6.param" + order = 60 + menu = "closed loop ramp (%)" + param = "CLOSED_LOOP_RAMP_DC" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.volt] + type = "menu" + order = 40 + menu = "voltage configs" + [canivore.FxP6.motor.volt.peakFwd] + type = "p6.param" + order = 10 + menu = "peak forward (V)" + param = "PEAK_FWD_V" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.volt.peakRev] + type = "p6.param" + order = 20 + menu = "peak reverse (V)" + param = "PEAK_REV_V" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.volt.supplyConst] + type = "p6.param" + order = 30 + menu = "supply voltage time const" + param = "SUPPLY_V_TIME_CONST" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.volt.openRamp] + type = "p6.param" + order = 40 + menu = "open loop ramp (V)" + param = "OPEN_LOOP_RAMP_V" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.volt.closedRamp] + type = "p6.param" + order = 50 + menu = "closed loop ramp (V)" + param = "CLOSED_LOOP_RAMP_V" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.torque] + type = "menu" + order = 50 + menu = "torque current configs" + [canivore.FxP6.motor.torque.peakFwd] + type = "p6.param" + order = 10 + menu = "peak forward (A)" + param = "PEAK_FWD_I" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.torque.peakRev] + type = "p6.param" + order = 20 + menu = "peak reverse (A)" + param = "PEAK_REV_I" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.torque.dead] + type = "p6.param" + order = 30 + menu = "neutral deadband (A)" + param = "TORQUE_NEUTRAL_DEADBAND" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.openRamp] + type = "p6.param" + order = 40 + menu = "open loop ramp (A)" + param = "OPEN_LOOP_RAMP_I" + bus = "canivore" + device = "fx" + [canivore.FxP6.motor.closedRamp] + type = "p6.param" + order = 50 + menu = "closed loop ramp (A)" + param = "CLOSED_LOOP_RAMP_I" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits] + type = "menu" + order = 120 + menu = "setup hard/soft limits" + [canivore.FxP6.limits.hard] + type = "menu" + order = 10 + menu = "hard limits" + [canivore.FxP6.limits.hard.fwdEn] + type = "p6.param" + order = 5 + menu = "FWD limit enable" + param = "FWD_HARD_EN" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.fwdNormal] + type = "p6.fwdNorm" + order = 10 + menu = "FWD limit normal" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.fwdSource] + type = "p6.fwdSource" + order = 20 + menu = "FWD limit source" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.fwdRemote] + type = "p6.param" + order = 30 + menu = "FWD limit remote id" + param = "FWD_REMOTE_ID" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.fwdAutoSet] + type = "p6.param" + order = 40 + menu = "Autoset Position on FWD limit" + param = "FWD_AUTOSET_POS" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.fwdAutoSetValue] + type = "p6.param" + order = 50 + menu = "FWD autoset position value" + param = "FWD_AUTOSET_POS_VALUE" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.revEn] + type = "p6.param" + order = 60 + menu = "REV limit enable" + param = "REV_HARD_EN" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.revNormal] + type = "p6.revNorm" + order = 70 + menu = "REV limit normal" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.revSource] + type = "p6.revSource" + order = 80 + menu = "REV limit source" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.revRemote] + type = "p6.param" + order = 90 + menu = "REV limit remote id" + param = "REV_REMOTE_ID" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.revAutoSet] + type = "p6.param" + order = 100 + menu = "Autoset position on REV limit" + param = "REV_AUTOSET_POS" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.hard.revAutoSetValue] + type = "p6.param" + order = 110 + menu = "REV autoset position value" + param = "REV_AUTOSET_POS_VALUE" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.soft] + type = "menu" + order = 20 + menu = "soft limits" + [canivore.FxP6.limits.soft.fwdEn] + type = "p6.param" + order = 10 + menu = "FWD limit enable" + param = "FWD_SOFT_EN" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.soft.fwdThres] + type = "p6.param" + order = 20 + menu = "FWD soft limit" + param = "FWD_SOFT_THRES" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.soft.revEn] + type = "p6.param" + order = 30 + menu = "REV limit enable" + param = "REV_SOFT_EN" + bus = "canivore" + device = "fx" + [canivore.FxP6.limits.soft.revThres] + type = "p6.param" + order = 40 + menu = "REV soft limit" + param = "REV_SOFT_THRES" + bus = "canivore" + device = "fx" + [canivore.FxP6.audio] + type = "menu" + order = 130 + menu = "setup audio configs" + [canivore.FxP6.audio.disable] + type = "p6.param" + order = 10 + menu = "allow music on disable" + param = "ALLOW_MUSIC_DIS" + bus = "canivore" + device = "fx" + [canivore.FxP6.audio.beepBoot] + type = "p6.param" + order = 20 + menu = "beep on boot" + param = "BEEP_ON_BOOT" + bus = "canivore" + device = "fx" + [canivore.FxP6.audio.beepConfig] + type = "p6.param" + order = 30 + menu = "beep on config" + param = "BEEP_ON_CONFIG" + bus = "canivore" + device = "fx" + [canivore.FxP6.factory] + type = "p6.factory" + order = 140 + menu = "config factory default" + bus = "canivore" + device = "fx" + [canivore.FxP6.grapher] + type = "menu" + order = 150 + menu = "set status frames" + [canivore.FxP6.grapher.default] + type = "p6.graph" + order = 10 + menu = "set to default update rate" + bus = "canivore" + device = "fx" + [canivore.FxP6.grapher.rate] + type = "p6.param" + order = 20 + menu = "Grapher Frame Rate (Hz)" + param = "GRAPHER_FRAME" + bus = "canivore" + device = "fx" -[canivore] - type = "menu" - order = 20 - menu = "work on CANivore" - [canivore.FxP6] + [canivore.FxsP6] type = "menu" - order = 17 - menu = "work with talonFX's" - [canivore.FxP6.run] + order = 18 + menu = "work with talonFXS's" + [canivore.FxsP6.run] type = "p6.run" order = 10 - menu = "run active talon fx's" + menu = "run active talon fxs's" bus = "canivore" - [canivore.FxP6.select] + device = "fxs" + [canivore.FxsP6.select] type = "p6.select" order = 20 - menu = "active fx's" + menu = "active fxs's" bus = "canivore" - [canivore.FxP6.status] + device = "fxs" + [canivore.FxsP6.status] type = "p6.status" order = 30 - menu = "status of active talonFx's" + menu = "status of active talonfxs's" bus = "canivore" - [canivore.FxP6.mode] + device = "fxs" + [canivore.FxsP6.mode] type = "p6.modeMenu" order = 40 menu = "config active mode" bus = "canivore" - [canivore.FxP6.mode.status] + device = "fxs" + [canivore.FxsP6.mode.status] type = "p6.modeStatus" order = 10 menu = "active mode" bus = "canivore" - [canivore.FxP6.mode.select] + device = "fxs" + [canivore.FxsP6.mode.select] type = "p6.mode" order = 15 menu = "select mode type" bus = "canivore" - [canivore.FxP6.mode.openLoop] + device = "fxs" + [canivore.FxsP6.mode.openLoop] type = "menu" order = 20 menu = "open loop modes" - [canivore.FxP6.mode.openLoop.deadband] + bus = "canivore" + device = "fxs" + [canivore.FxsP6.mode.openLoop.deadband] type = "p6.param" order = 20 menu = "set torque current deadband (A)" param = "TORQUE_CURRENT_DEADBAND" bus = "canivore" - [canivore.FxP6.mode.position] + device = "fxs" + [canivore.FxsP6.mode.position] type = "menu" order = 30 menu = "position modes" - [canivore.FxP6.mode.position.velocity] + [canivore.FxsP6.mode.position.velocity] type = "p6.param" order = 20 menu = "velocity target" param = "VELOCITY" bus = "canivore" - [canivore.FxP6.mode.position.feedFwd] + device = "fxs" + [canivore.FxsP6.mode.position.feedFwd] type = "p6.param" order = 30 menu = "velocity feed forward" param = "FEED_FORWARD" bus = "canivore" - [canivore.FxP6.mode.velocity] + device = "fxs" + [canivore.FxsP6.mode.velocity] type = "menu" order = 40 menu = "velocity modes" - [canivore.FxP6.mode.velocity.acceleration] + [canivore.FxsP6.mode.velocity.acceleration] type = "p6.param" order = 20 menu = "select acceleration target" param = "ACCELERATION" bus = "canivore" - [canivore.FxP6.mode.velocity.feedFwd] + device = "fxs" + [canivore.FxsP6.mode.velocity.feedFwd] type = "p6.param" order = 30 menu = "feed forward" param = "FEED_FORWARD" bus = "canivore" - [canivore.FxP6.mode.motion] + device = "fxs" + [canivore.FxsP6.mode.motion] type = "menu" order = 50 menu = "motion magic modes" - bus = "canivore" - [canivore.FxP6.mode.motion.motionType] + [canivore.FxsP6.mode.motion.motionType] type = "p6.mmType" order = 10 menu = "select motion magic type" bus = "canivore" - [canivore.FxP6.mode.motion.feedFwd] + device = "fxs" + [canivore.FxsP6.mode.motion.feedFwd] type = "p6.param" order = 20 menu = "select feed forward (sel unit)" param = "FEED_FORWARD" bus = "canivore" - [canivore.FxP6.mode.motion.velocity] + device = "fxs" + [canivore.FxsP6.mode.motion.velocity] type = "p6.param" order = 30 menu = "select velocity target (rot/s)" param = "VELOCITY" bus = "canivore" - [canivore.FxP6.mode.motion.acceleration] + device = "fxs" + [canivore.FxsP6.mode.motion.acceleration] type = "p6.param" order = 40 menu = "select acceleration target (rot/s^2)" param = "ACCELERATION" bus = "canivore" - [canivore.FxP6.mode.motion.jerk] + device = "fxs" + [canivore.FxsP6.mode.motion.jerk] type = "p6.param" order = 50 menu = "select jerk target (rot/s^3)" param = "JERK" bus = "canivore" - [canivore.FxP6.mode.diff] + device = "fxs" + [canivore.FxsP6.mode.diff] type = "menu" order = 60 menu = "differential modes" - [canivore.FxP6.mode.diff.diffType] + [canivore.FxsP6.mode.diff.diffType] type = "p6.diffType" order = 10 menu = "select differential type" bus = "canivore" - [canivore.FxP6.mode.diff.slot] + device = "fxs" + [canivore.FxsP6.mode.diff.slot] type = "p6.param" order = 20 menu = "select differential slot" param = "DIFFERENTIAL_SLOT" bus = "canivore" - [canivore.FxP6.mode.diff.target] + device = "fxs" + [canivore.FxsP6.mode.diff.target] type = "p6.param" order = 30 menu = "select differential target (rots)" param = "DIFFERENTIAL_TARGET" bus = "canivore" - [canivore.FxP6.mode.follow] + device = "fxs" + [canivore.FxsP6.mode.follow] type = "menu" order = 70 menu = "follower modes" - [canivore.FxP6.mode.follow.followType] + [canivore.FxsP6.mode.follow.followType] type = "p6.followType" order = 10 menu = "select follower type" bus = "canivore" - [canivore.FxP6.mode.follow.oppose] + device = "fxs" + [canivore.FxsP6.mode.follow.oppose] type = "p6.param" order = 20 menu = "oppose leader direction" param = "OPPOSE_MAIN" bus = "canivore" - [canivore.FxP6.mode.neutral] + device = "fxs" + [canivore.FxsP6.mode.neutral] type = "menu" order = 80 menu = "neutral modes" - [canivore.FxP6.mode.neutral.select] + [canivore.FxsP6.mode.neutral.select] type = "p6.neutralOut" order = 10 menu = "select neutral output" bus = "canivore" - [canivore.FxP6.mode.foc] + device = "fxs" + [canivore.FxsP6.mode.foc] type = "p6.param" order = 100 menu = "(pro) enable FOC" param = "FOC" bus = "canivore" - [canivore.FxP6.mode.overrideNeut] + device = "fxs" + [canivore.FxsP6.mode.overrideNeut] type = "p6.param" order = 110 menu = "override neutral mode" param = "OVERRIDE_NEUTRAL" bus = "canivore" - [canivore.FxP6.mode.limFwd] + device = "fxs" + [canivore.FxsP6.mode.limFwd] type = "p6.param" order = 120 menu = "limit forward motion" param = "LIM_FWD_MOT" bus = "canivore" - [canivore.FxP6.mode.limRev] + device = "fxs" + [canivore.FxsP6.mode.limRev] type = "p6.param" order = 130 menu = "limit reverse motion" param = "LIM_REV_MOT" bus = "canivore" - [canivore.FxP6.units] + device = "fxs" + [canivore.FxsP6.units] type = "p6.units" order = 50 menu = "select active units" bus = "canivore" - [canivore.FxP6.feedback] + device = "fxs" + [canivore.FxsP6.feedback] type = "menu" order = 60 menu = "setup feedback configs" - [canivore.FxP6.feedback.position] + bus = "canivore" + device = "fxs" + [canivore.FxsP6.feedback.position] type = "p6.param" order = 5 menu = "set position" param = "POSITION" bus = "canivore" - [canivore.FxP6.feedback.offset] + device = "fxs" + [canivore.FxsP6.feedback.offset] type = "p6.param" order = 10 - menu = "set rotor offset" - param = "ROTOR_OFFSET" + menu = "set absolute sensor offset" + param = "ABS_SENSOR_OFFSET" bus = "canivore" - [canivore.FxP6.feedback.sense2mech] + device = "fxs" + [canivore.FxsP6.feedback.discontinuity] + type = "p6.param" + order = 15 + menu = "set absolute sensor discontinuity point" + param = "ABS_SENSOR_DISCONTINUITY" + bus = "canivore" + device = "fxs" + [canivore.FxsP6.feedback.edge] + type = "p6.param" + order = 17 + menu = "set quadrature edges per rotation" + param = "QUAD_EDGE_PER_ROT" + bus = "canivore" + device = "fxs" + [canivore.FxsP6.feedback.sense2mech] type = "p6.param" order = 20 menu = "set sensor to mechanism ratio" param = "SENSOR_TO_MECH_RATIO" bus = "canivore" - [canivore.FxP6.feedback.rotor2sense] + device = "fxs" + [canivore.FxsP6.feedback.rotor2sense] type = "p6.param" order = 30 menu = "set rotor to sensor ratio" param = "ROTOR_TO_SENSOR_RATIO" bus = "canivore" - [canivore.FxP6.feedback.source] - type = "p6.feedback" + device = "fxs" + [canivore.FxsP6.feedback.source] + type = "p6.extFeedback" order = 35 - menu = "set feedback source" + menu = "set external feedback source" bus = "canivore" - [canivore.FxP6.feedback.remoteId] + device = "fxs" + [canivore.FxsP6.feedback.phase] + type = "p6.phase" + order = 37 + menu = "set sensor phase" + bus = "canivore" + device = "fxs" + [canivore.FxsP6.feedback.remoteId] type = "p6.param" order = 40 menu = "set remote sensor id" param = "REMOTE_SENSOR_ID" bus = "canivore" - [canivore.FxP6.slot] + device = "fxs" + [canivore.FxsP6.feedback.vFiltTC] + type = "p6.param" + order = 45 + menu = "set velocity filter time constant" + param = "VELOCITY_FILTER_TIME_CONST" + bus = "canivore" + device = "fxs" + [canivore.FxsP6.slot] type = "menu" order = 70 menu = "setup slot configs" - [canivore.FxP6.slot.active] + [canivore.FxsP6.slot.active] type = "p6.slot" order = 10 menu = "active slot" bus = "canivore" - [canivore.FxP6.slot.kP] + device = "fxs" + [canivore.FxsP6.slot.kP] type = "p6.param" order = 20 menu = "kP" param = "SLOT_KP" bus = "canivore" - [canivore.FxP6.slot.kI] + device = "fxs" + [canivore.FxsP6.slot.kI] type = "p6.param" order = 30 menu = "kI" param = "SLOT_KI" bus = "canivore" - [canivore.FxP6.slot.kD] + device = "fxs" + [canivore.FxsP6.slot.kD] type = "p6.param" order = 40 menu = "kD" param = "SLOT_KD" bus = "canivore" - [canivore.FxP6.slot.kS] + device = "fxs" + [canivore.FxsP6.slot.kS] type = "p6.param" order = 50 menu = "kS" param = "SLOT_KS" bus = "canivore" - [canivore.FxP6.slot.kV] + device = "fxs" + [canivore.FxsP6.slot.kV] type = "p6.param" order = 60 menu = "kV" param = "SLOT_KV" bus = "canivore" - [canivore.FxP6.slot.kA] + device = "fxs" + [canivore.FxsP6.slot.kA] type = "p6.param" order = 65 menu = "kA" param = "SLOT_KA" bus = "canivore" - [canivore.FxP6.slot.kG] + device = "fxs" + [canivore.FxsP6.slot.kG] type = "p6.param" order = 70 menu = "kG" param = "SLOT_KG" bus = "canivore" - [canivore.FxP6.slot.gravity] + device = "fxs" + [canivore.FxsP6.slot.gravity] type = "p6.gravity" order = 80 menu = "gravity type" bus = "canivore" - [canivore.FxP6.motion] + device = "fxs" + [canivore.FxsP6.motion] type = "menu" order = 80 menu = "setup motion magic configs" - [canivore.FxP6.motion.accel] + [canivore.FxsP6.motion.accel] type = "p6.param" order = 10 menu = "acceleration" param = "MM_ACCEL" bus = "canivore" - [canivore.FxP6.motion.cruise] + device = "fxs" + [canivore.FxsP6.motion.cruise] type = "p6.param" order = 20 menu = "cruise velocity" param = "MM_CRUISE_VEL" bus = "canivore" - [canivore.FxP6.motion.jerk] + device = "fxs" + [canivore.FxsP6.motion.jerk] type = "p6.param" order = 30 menu = "jerk" param = "MM_JERK" bus = "canivore" - [canivore.FxP6.motion.kA] + device = "fxs" + [canivore.FxsP6.motion.kA] type = "p6.param" order = 40 menu = "Expo kA" param = "MM_EXPO_KA" bus = "canivore" - [canivore.FxP6.motion.kV] + device = "fxs" + [canivore.FxsP6.motion.kV] type = "p6.param" order = 50 menu = "Expo kV" param = "MM_EXPO_KV" bus = "canivore" - [canivore.FxP6.diff] + device = "fxs" + [canivore.FxsP6.diff] type = "menu" order = 90 menu = "setup differential configs" - [canivore.FxP6.diff.duty] + [canivore.FxsP6.diff.duty] type = "p6.param" order = 10 menu = "peak differential duty cycle" param = "PEAK_DIFF_DC" bus = "canivore" - [canivore.FxP6.diff.torque] + device = "fxs" + [canivore.FxsP6.diff.torque] type = "p6.param" order = 20 menu = "peak differential torque current" param = "PEAK_DIFF_TORQUE" bus = "canivore" - [canivore.FxP6.diff.volt] + device = "fxs" + [canivore.FxsP6.diff.volt] type = "p6.param" order = 30 menu = "peak differential voltage" param = "PEAK_DIFF_VOLT" bus = "canivore" - [canivore.FxP6.diff.remote] + device = "fxs" + [canivore.FxsP6.diff.remote] type = "p6.param" order = 40 menu = "differential remote id" param = "DIFF_SENSOR_REMOTE_ID" bus = "canivore" - [canivore.FxP6.diff.fx] + device = "fxs" + [canivore.FxsP6.diff.fx] type = "p6.param" order = 50 menu = "differential fx id" param = "DIFF_FX_ID" bus = "canivore" - [canivore.FxP6.current] + device = "fxs" + [canivore.FxsP6.diff.source] + type = "p6.diffSource" + order = 60 + menu = "differential source" + bus = "canivore" + device = "fxs" + [canivore.FxsP6.current] type = "menu" order = 100 menu = "setup current limit configs" - [canivore.FxP6.current.statorEn] + [canivore.FxsP6.current.statorEn] type = "p6.param" order = 10 menu = "stator limit enable" param = "STATOR_LIM_EN" bus = "canivore" - [canivore.FxP6.current.statorLim] + device = "fxs" + [canivore.FxsP6.current.statorLim] type = "p6.param" order = 20 menu = "stator current limit" param = "STATOR_LIM" bus = "canivore" - [canivore.FxP6.current.suppEn] + device = "fxs" + [canivore.FxsP6.current.suppEn] type = "p6.param" order = 30 menu = "supply limit enable" param = "SUPP_LIM_EN" bus = "canivore" - [canivore.FxP6.current.suppLim] + device = "fxs" + [canivore.FxsP6.current.suppLim] type = "p6.param" order = 40 - menu = "supply current limit" - param = "SUPP_LIM" + menu = "supply current lower limit" + param = "SUPP_LOWER_LIM" bus = "canivore" - [canivore.FxP6.current.tripI] + device = "fxs" + [canivore.FxsP6.current.tripI] type = "p6.param" order = 50 - menu = "supply current trip threshold (A)" - param = "SUPP_TRIP_THRES" + menu = "supply current limit (A)" + param = "SUPP_LIM" bus = "canivore" - [canivore.FxP6.current.tripT] + device = "fxs" + [canivore.FxsP6.current.tripT] type = "p6.param" order = 60 menu = "supply current trip time (s)" param = "SUPP_TRIP_TIME" bus = "canivore" - [canivore.FxP6.motor] + device = "fxs" + [canivore.FxsP6.motor] type = "menu" order = 110 menu = "setup motor output and ramp configs" - [canivore.FxP6.motor.invert] + [canivore.FxsP6.motor.invert] type = "p6.invert" order = 10 menu = "motor inversion" bus = "canivore" - [canivore.FxP6.motor.neutral] + device = "fxs" + [canivore.FxsP6.motor.neutral] type = "p6.neutral" order = 20 menu = "neutral mode" bus = "canivore" - [canivore.FxP6.motor.duty] + device = "fxs" + [canivore.FxsP6.motor.duty] type = "menu" order = 30 menu = "duty cycle configs" - [canivore.FxP6.motor.duty.peakFwd] + bus = "canivore" + device = "fxs" + [canivore.FxsP6.motor.duty.peakFwd] type= "p6.param" order = 20 menu = "peak forward %" param = "PEAK_FWD_DC" bus = "canivore" - [canivore.FxP6.motor.duty.peakRev] + device = "fxs" + [canivore.FxsP6.motor.duty.peakRev] type = "p6.param" order = 30 menu = "peak reverse %" param = "PEAK_REV_DC" bus = "canivore" - [canivore.FxP6.motor.duty.dead] + device = "fxs" + [canivore.FxsP6.motor.duty.dead] type = "p6.param" order = 40 menu = "neutral deadband (%)" param = "NEUTRAL_DEADBAND_DC" bus = "canivore" - [canivore.FxP6.motor.duty.openRamp] + device = "fxs" + [canivore.FxsP6.motor.duty.openRamp] type = "p6.param" order = 50 menu = "open loop ramp (%)" param = "OPEN_LOOP_RAMP_DC" bus = "canivore" - [canivore.FxP6.motor.duty.closedRamp] + device = "fxs" + [canivore.FxsP6.motor.duty.closedRamp] type = "p6.param" order = 60 menu = "closed loop ramp (%)" param = "CLOSED_LOOP_RAMP_DC" bus = "canivore" - [canivore.FxP6.motor.volt] + device = "fxs" + [canivore.FxsP6.motor.volt] type = "menu" order = 40 menu = "voltage configs" - [canivore.FxP6.motor.volt.peakFwd] + [canivore.FxsP6.motor.volt.peakFwd] type = "p6.param" order = 10 menu = "peak forward (V)" param = "PEAK_FWD_V" bus = "canivore" - [canivore.FxP6.motor.volt.peakRev] + device = "fxs" + [canivore.FxsP6.motor.volt.peakRev] type = "p6.param" order = 20 menu = "peak reverse (V)" param = "PEAK_REV_V" bus = "canivore" - [canivore.FxP6.motor.volt.supplyConst] + device = "fxs" + [canivore.FxsP6.motor.volt.supplyConst] type = "p6.param" order = 30 menu = "supply voltage time const" param = "SUPPLY_V_TIME_CONST" bus = "canivore" - [canivore.FxP6.motor.volt.openRamp] + device = "fxs" + [canivore.FxsP6.motor.volt.openRamp] type = "p6.param" order = 40 menu = "open loop ramp (V)" param = "OPEN_LOOP_RAMP_V" bus = "canivore" - [canivore.FxP6.motor.volt.closedRamp] + device = "fxs" + [canivore.FxsP6.motor.volt.closedRamp] type = "p6.param" order = 50 menu = "closed loop ramp (V)" param = "CLOSED_LOOP_RAMP_V" bus = "canivore" - [canivore.FxP6.motor.torque] + device = "fxs" + [canivore.FxsP6.motor.torque] type = "menu" order = 50 menu = "torque current configs" - [canivore.FxP6.motor.torque.peakFwd] - type = "p6.param" - order = 10 - menu = "peak forward (A)" - param = "PEAK_FWD_I" - bus = "canivore" - [canivore.FxP6.motor.torque.peakRev] - type = "p6.param" - order = 20 - menu = "peak reverse (A)" - param = "PEAK_REV_I" - bus = "canivore" - [canivore.FxP6.motor.torque.dead] - type = "p6.param" - order = 30 - menu = "neutral deadband (A)" - param = "TORQUE_NEUTRAL_DEADBAND" - bus = "canivore" - [canivore.FxP6.motor.openRamp] + [canivore.FxsP6.motor.torque.openRamp] type = "p6.param" order = 40 menu = "open loop ramp (A)" param = "OPEN_LOOP_RAMP_I" bus = "canivore" - [canivore.FxP6.motor.closedRamp] + device = "fxs" + [canivore.FxsP6.motor.torque.closedRamp] type = "p6.param" order = 50 menu = "closed loop ramp (A)" param = "CLOSED_LOOP_RAMP_I" bus = "canivore" - [canivore.FxP6.limits] + device = "fxs" + [canivore.FxsP6.motor.arrange] + type = "p6.motorArrange" + order = 60 + menu = "set attached motor" + bus = "canivore" + device = "fxs" + [canivore.FxsP6.motor.wire] + type = "p6.wiring" + order = 70 + menu = "set brushed motor wiring config" + bus = "canivore" + device = "fxs" + [canivore.FxsP6.motor.advHall] + type = "p6.advHall" + order = 80 + menu = "enable advanced hall support" + bus = "canivore" + device = "fxs" + [canivore.FxsP6.limits] type = "menu" order = 120 menu = "setup hard/soft limits" - [canivore.FxP6.limits.hard] + [canivore.FxsP6.limits.hard] type = "menu" order = 10 menu = "hard limits" - [canivore.FxP6.limits.hard.fwdEn] + [canivore.FxsP6.limits.hard.fwdEn] type = "p6.param" - order = 5 + order = 10 menu = "FWD limit enable" param = "FWD_HARD_EN" bus = "canivore" - [canivore.FxP6.limits.hard.fwdNormal] + device = "fxs" + [canivore.FxsP6.limits.hard.fwdNormal] type = "p6.fwdNorm" - order = 10 + order = 15 menu = "FWD limit normal" bus = "canivore" - [canivore.FxP6.limits.hard.fwdSource] + device = "fxs" + [canivore.FxsP6.limits.hard.fwdSource] type = "p6.fwdSource" order = 20 menu = "FWD limit source" bus = "canivore" - [canivore.FxP6.limits.hard.fwdRemote] + device = "fxs" + [canivore.FxsP6.limits.hard.fwdRemote] type = "p6.param" order = 30 menu = "FWD limit remote id" param = "FWD_REMOTE_ID" bus = "canivore" - [canivore.FxP6.limits.hard.fwdAutoSet] + device = "fxs" + [canivore.FxsP6.limits.hard.fwdAutoSet] type = "p6.param" order = 40 menu = "Autoset Position on FWD limit" param = "FWD_AUTOSET_POS" bus = "canivore" - [canivore.FxP6.limits.hard.fwdAutoSetValue] + device = "fxs" + [canivore.FxsP6.limits.hard.fwdAutoSetValue] type = "p6.param" order = 50 menu = "FWD autoset position value" param = "FWD_AUTOSET_POS_VALUE" bus = "canivore" - [canivore.FxP6.limits.hard.revEn] + device = "fxs" + [canivore.FxsP6.limits.hard.revEn] type = "p6.param" order = 60 menu = "REV limit enable" param = "REV_HARD_EN" bus = "canivore" - [canivore.FxP6.limits.hard.revNormal] + device = "fxs" + [canivore.FxsP6.limits.hard.revNormal] type = "p6.revNorm" order = 70 menu = "REV limit normal" bus = "canivore" - [canivore.FxP6.limits.hard.revSource] + device = "fxs" + [canivore.FxsP6.limits.hard.revSource] type = "p6.revSource" order = 80 menu = "REV limit source" bus = "canivore" - [canivore.FxP6.limits.hard.revRemote] + device = "fxs" + [canivore.FxsP6.limits.hard.revRemote] type = "p6.param" order = 90 menu = "REV limit remote id" param = "REV_REMOTE_ID" bus = "canivore" - [canivore.FxP6.limits.hard.revAutoSet] + device = "fxs" + [canivore.FxsP6.limits.hard.revAutoSet] type = "p6.param" order = 100 menu = "Autoset position on REV limit" param = "REV_AUTOSET_POS" bus = "canivore" - [canivore.FxP6.limits.hard.revAutoSetValue] + device = "fxs" + [canivore.FxsP6.limits.hard.revAutoSetValue] type = "p6.param" order = 110 menu = "REV autoset position value" param = "REV_AUTOSET_POS_VALUE" bus = "canivore" - [canivore.FxP6.limits.soft] + device = "fxs" + [canivore.FxsP6.limits.soft] type = "menu" order = 20 menu = "soft limits" - [canivore.FxP6.limits.soft.fwdEn] + [canivore.FxsP6.limits.soft.fwdEn] type = "p6.param" order = 10 menu = "FWD limit enable" param = "FWD_SOFT_EN" bus = "canivore" - [canivore.FxP6.limits.soft.fwdThres] + device = "fxs" + [canivore.FxsP6.limits.soft.fwdThres] type = "p6.param" order = 20 menu = "FWD soft limit" param = "FWD_SOFT_THRES" bus = "canivore" - [canivore.FxP6.limits.soft.revEn] + device = "fxs" + [canivore.FxsP6.limits.soft.revEn] type = "p6.param" order = 30 menu = "REV limit enable" param = "REV_SOFT_EN" bus = "canivore" - [canivore.FxP6.limits.soft.revThres] + device = "fxs" + [canivore.FxsP6.limits.soft.revThres] type = "p6.param" order = 40 menu = "REV soft limit" param = "REV_SOFT_THRES" bus = "canivore" - [canivore.FxP6.audio] + device = "fxs" + [canivore.FxsP6.audio] type = "menu" order = 130 menu = "setup audio configs" - [canivore.FxP6.audio.disable] + [canivore.FxsP6.audio.disable] type = "p6.param" order = 10 menu = "allow music on disable" param = "ALLOW_MUSIC_DIS" bus = "canivore" - [canivore.FxP6.audio.beepBoot] + device = "fxs" + [canivore.FxsP6.audio.beepBoot] type = "p6.param" order = 20 menu = "beep on boot" param = "BEEP_ON_BOOT" bus = "canivore" - [canivore.FxP6.audio.beepConfig] + device = "fxs" + [canivore.FxsP6.audio.beepConfig] type = "p6.param" order = 30 menu = "beep on config" param = "BEEP_ON_CONFIG" bus = "canivore" - [canivore.FxP6.factory] + device = "fxs" + [canivore.FxsP6.factory] type = "p6.factory" order = 140 menu = "config factory default" bus = "canivore" - [canivore.FxP6.grapher] + device = "fxs" + [canivore.FxsP6.grapher] type = "menu" order = 150 menu = "set status frames" - [canivore.FxP6.grapher.default] + [canivore.FxsP6.grapher.default] type = "p6.graph" order = 10 menu = "set to default update rate" bus = "canivore" - [canivore.FxP6.grapher.rate] + device = "fxs" + [canivore.FxsP6.grapher.rate] type = "p6.param" order = 20 menu = "Grapher Frame Rate (Hz)" param = "GRAPHER_FRAME" bus = "canivore" + device = "fxs" + + + [canivore.cancoder] type = "menu" @@ -2594,15 +4403,45 @@ type = "menu" param = "MAG_OFFSET" order = 40 bus = "canivore" - [canivore.cancoder.absRange] - type = "cancoder.absRange" - order = 50 - bus = "canivore" +# [canivore.cancoder.absRange] +# type = "cancoder.absRange" +# order = 50 +# bus = "canivore" [canivore.cancoder.sensorDirection] type = "cancoder.sensorDirection" order = 60 bus = "canivore" + [canivore.FXswerve] + type = "menu" + order = 25 + menu = "work with FX swerve" + [canivore.FXswerve.lock] + type = "p6.swerve.azimuth" + order = 10 + bus = "canivore" + menu = "lock azimuth position" + [canivore.FXswerve.azimuth] + type = "menu" + order = 20 + bus = "canivore" + menu = "update azimuth zero positions" + [canivore.FXswerve.azimuth.save] + type = "p6.swerve.azimuth.save" + order = 10 + bus = "canivore" + menu = "save azimuth zero positions" + [canivore.FXswerve.azimuth.select] + type = "p6.swerve.azimuth.select" + order = 20 + bus = "canivore" + menu = "active azimuth" + [canivore.FXswerve.azimuth.adjust] + type = "p6.swerve.azimuth.adjust" + order = 30 + bus = "canivore" + menu = "active azimuth adjustment" + # [dio] diff --git a/src/main/resources/phoenix6.toml b/src/main/resources/phoenix6.toml index 91996c6..ad7257d 100644 --- a/src/main/resources/phoenix6.toml +++ b/src/main/resources/phoenix6.toml @@ -126,21 +126,21 @@ name = "Supply Current Limit Enable" type = "BOOLEAN" help = "Enable motor supply current limiting." +[SUPP_LOWER_LIM] + name = "Supply Current Lower Limit" + type = "DOUBLE" + help = "The amount of supply current allowed after the regular SupplyCurrentLimit is active for longer than SupplyCurrentLowerTime. This allows higher current draws for a fixed period of time before reducing the current limit to protect breakers. This has no effect if SupplyCurrentLimit is lower than this value or SupplyCurrentLowerTime is 0. Default = 40A. Units: A" + range = [0.0, 500.0] [SUPP_LIM] name = "Supply Current Limit" type = "DOUBLE" - help = "The amount of supply current allowed. This is only applicable for non-torque current control modes. Note this requires the corresponding enable to be true. Use SupplyCurrentThreshold and SupplyTimeThreshold to allow brief periods of high-current before limiting occurs. Units: A" + help = "The absolute maximum amount of supply current allowed. Note this requires SupplyCurrentLimitEnable to be true. Use SupplyCurrentLowerLimit and SupplyCurrentLowerTime to reduce the supply current limit after the time threshold is exceeded. Default Value is 70A. Units: A" range = [0.0, 800.0] -[SUPP_TRIP_THRES] - name = "Supply Current Trip Threshold" - type = "DOUBLE" - help = "Delay supply current limiting until current exceeds this threshold for longer than SupplyTimeThreshold. This allows current draws above SupplyCurrentLimit for a fixed period of time. This has no effect if SupplyCurrentLimit is greater than this value. Units: A" - range = [0.0, 511.0] [SUPP_TRIP_TIME] - name = "Supply Current Trip Time" + name = "Supply Current Lower Time" type = "DOUBLE" - help = "Allows unlimited current for a period of time before current limiting occurs. Current threshold is the maximum of SupplyCurrentThreshold and SupplyCurrentLimit. Units: sec" - range = [0.0, 1.275] + help = "Reduces supply current to the SupplyCurrentLowerLimit after limiting to SupplyCurrentLimit for this period of time. If this is set to 0, SupplyCurrentLowerLimit will be ignored. Default = 1.0s. Units: sec" + range = [0.0, 2.5] #Motor & Ramp Configs @@ -354,4 +354,26 @@ [GRAPHER_FRAME] name = "Grapher Update Frequency (Hz)" type = "DOUBLE" - help = "Input update frequncy for all singals on the grapher in Hz, if this value shows 0 it is using the default update frequency" \ No newline at end of file + help = "Input update frequncy for all singals on the grapher in Hz, if this value shows 0 it is using the default update frequency" + +# FXS Added External Feedback +[ABS_SENSOR_DISCONTINUITY] + name = "Absolute Sensor Discontinuity Point" + type = "DOUBLE" + help = "The positive discontinuity point of the absolute sensor in rotations. This determines the point at which the absolute sensor wraps around, keeping the absolute position in the range [x-1, x). Setting this to 1 makes the absolute position unsigned [0, 1) Setting this to 0.5 makes the absolute position signed [-0.5, 0.5) Setting this to 0 makes the absolute position always negative [-1, 0) Many rotational mechanisms such as arms have a region of motion that is unreachable. This should be set to the center of that region of motion, in non-negative rotations. This affects the position of the device at bootup." + range = [0.0, 1.0] +[ABS_SENSOR_OFFSET] + name = "Absolute Sensor Offset" + type = "DOUBLE" + help = "The offset applied to any absolute sensor connected to the Talon data port. This is only supported when using the PulseWidth sensor source. This can be used to zero the sensor position in applications where the sensor is 1:1 with the mechanism." + range = [-1.0, 1.0] +[QUAD_EDGE_PER_ROT] + name = "Quadrature Edges Per Rotation" + type = "INTEGER" + help = "The number of quadrature edges in one rotation for the quadrature sensor connected to the Talon data port. This is the total number of transitions from high-to-low or low-to-high across both channels per rotation of the sensor. This is also equivalent to the Counts Per Revolution when using 4x decoding. For example, the SRX Mag Encoder has 4096 edges per rotation, and a US Digital 1024 CPR (Cycles Per Revolution) quadrature encoder has 4096 edges per rotation. On the Talon FXS, this can be at most 2,000,000,000 / Peak RPM." + range = [1.0, 1_000_000.0] +[VELOCITY_FILTER_TIME_CONST] + name = "Velocity Filter Time Constant" + type = "DOUBLE" + help = "The configurable time constant of the Kalman velocity filter in seconds. The velocity Kalman filter will adjust to act as a low-pass with this value as its time constant. If the user is aiming for an expected cutoff frequency, the frequency is calculated as 1 / (2 * π * τ) with τ being the time constant." + range = [0.0, 1.0] diff --git a/src/test/kotlin/org/strykeforce/thirdcoast/CheckTomlTest.kt b/src/test/kotlin/org/strykeforce/thirdcoast/CheckTomlTest.kt index c2d0593..6ca6f04 100644 --- a/src/test/kotlin/org/strykeforce/thirdcoast/CheckTomlTest.kt +++ b/src/test/kotlin/org/strykeforce/thirdcoast/CheckTomlTest.kt @@ -1,8 +1,10 @@ package org.strykeforce.thirdcoast import net.consensys.cava.toml.TomlTable +import org.assertj.core.api.Assertions.`as` import org.junit.jupiter.api.Test import org.assertj.core.api.Assertions.assertThat +import org.strykeforce.thirdcoast.talon.phoenix6.Phoenix6Parameter internal class CheckTomlTest { @@ -19,4 +21,33 @@ internal class CheckTomlTest { val table = toml["SLOT_P"] as TomlTable assertThat(table["type"]).isEqualTo("DOUBLE") } + + @Test + fun `check phoenix6`() { + val toml = parseResource("/phoenix6.toml") + val table = toml["SUPPLY_V_TIME_CONST"] as TomlTable + assertThat(table["type"]).isEqualTo("DOUBLE") + } + + @Test + fun `check cancoder`() { + val toml = parseResource("/cancoder.toml") + val table = toml["POSITION"] as TomlTable + assertThat(table["type"]).isEqualTo("DOUBLE") + } + + @Test + fun `check canifier`() { + val toml = parseResource("/canifier.toml") + val table = toml["FACTORY_DEFAULTS"] as TomlTable + assertThat(table["type"]).isEqualTo("BOOLEAN") + } + + + @Test + fun `checkPigeon`() { + val toml = parseResource("/pigeon.toml") + val table = toml["ACCUM_Z_ANGLE"] as TomlTable + assertThat(table["type"]).isEqualTo("DOUBLE") + } } diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json index e978a5f..ddb0e44 100644 --- a/vendordeps/NavX.json +++ b/vendordeps/NavX.json @@ -1,40 +1,71 @@ { - "fileName": "NavX.json", - "name": "NavX", - "version": "2024.1.0", + "fileName": "Studica-2025.0.0.json", + "name": "Studica", + "version": "2025.0.0", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [ - "https://dev.studica.com/maven/release/2024/" + "https://dev.studica.com/maven/release/2025/" ], - "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-java", - "version": "2024.1.0" - } - ], - "jniDependencies": [], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json", "cppDependencies": [ { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-cpp", - "version": "2024.1.0", + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", "headerClassifier": "headers", - "sourcesClassifier": "sources", + "libName": "Studica", "sharedLibrary": false, - "libName": "navx_frc", "skipInvalidPlatforms": true, + "version": "2025.0.0" + }, + { + "artifactId": "Studica-driver", "binaryPlatforms": [ "linuxathena", - "linuxraspbian", "linuxarm32", "linuxarm64", "linuxx86-64", "osxuniversal", "windowsx86-64" - ] + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.0" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.0" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.0" } ] } \ No newline at end of file diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json index 88a68dd..bdf2f44 100644 --- a/vendordeps/Phoenix5.json +++ b/vendordeps/Phoenix5.json @@ -1,56 +1,81 @@ { - "fileName": "Phoenix5.json", + "fileName": "Phoenix5-replay-5.35.1.json", "name": "CTRE-Phoenix (v5)", - "version": "5.33.0", - "frcYear": 2024, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "version": "5.35.1", + "frcYear": "2025", + "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-replay-frc2025-latest.json", "requires": [ { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + "offlineFileName": "Phoenix6-replay-frc2025-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2025-latest.json" + } + ], + "conflictsWith": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Users must use the regular Phoenix 5 vendordep when using the regular Phoenix 6 vendordep.", + "offlineFileName": "Phoenix6-frc2025-latest.json" + }, + { + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", + "offlineFileName": "Phoenix5-frc2025-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.33.0" + "version": "5.35.1" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.33.0" + "version": "5.35.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.0", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.replay", + "artifactId": "cci-replay", + "version": "5.35.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", - "linuxathena" + "linuxarm64", + "osxuniversal" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.0", + "version": "5.35.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -60,14 +85,12 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.33.0", + "version": "5.35.1", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", "linuxathena" ], "simMode": "hwsim" @@ -75,14 +98,12 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.33.0", + "version": "5.35.1", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", "linuxathena" ], "simMode": "hwsim" @@ -90,22 +111,68 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.0", + "version": "5.35.1", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.replay", + "artifactId": "wpiapi-cpp-replay", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPIReplay", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", - "linuxathena" + "linuxarm64", + "osxuniversal" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.replay", + "artifactId": "api-cpp-replay", + "version": "5.35.1", + "libName": "CTRE_PhoenixReplay", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.replay", + "artifactId": "cci-replay", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCIReplay", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.33.0", + "version": "5.35.1", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -113,6 +180,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -120,7 +188,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.33.0", + "version": "5.35.1", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -128,6 +196,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -135,7 +204,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.0", + "version": "5.35.1", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -143,6 +212,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 2b7d172..f94c108 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,76 +1,116 @@ { - "fileName": "Phoenix6.json", - "name": "CTRE-Phoenix (v6)", - "version": "24.2.0", - "frcYear": 2024, - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "fileName": "Phoenix6-replay-25.2.2.json", + "name": "CTRE-Phoenix (v6) Replay", + "version": "25.2.2", + "frcYear": "2025", + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2025-latest.json", "conflictsWith": [ { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-frc2025-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.2.0" + "version": "25.2.2" } ], "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena" + ], + "simMode": "hwsim" + }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.replay", + "artifactId": "api-cpp-replay", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", - "linuxathena" + "linuxarm64", + "osxuniversal" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.replay", + "artifactId": "tools-replay", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" ], "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.2.0", + "artifactId": "api-cpp-sim", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.2.0", + "artifactId": "tools-sim", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.2.0", + "artifactId": "simTalonSRX", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -78,12 +118,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -91,12 +132,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -104,12 +146,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -117,12 +160,27 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -130,12 +188,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -143,12 +202,27 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -158,14 +232,12 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.2.0", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", "linuxathena" ], "simMode": "hwsim" @@ -173,74 +245,92 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "25.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.replay", + "artifactId": "wpiapi-cpp-replay", + "version": "25.2.2", + "libName": "CTRE_Phoenix6_WPIReplay", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", - "linuxathena" + "linuxarm64", + "osxuniversal" ], "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "24.2.0", - "libName": "CTRE_Phoenix6_WPISim", + "groupId": "com.ctre.phoenix6.replay", + "artifactId": "tools-replay", + "version": "25.2.2", + "libName": "CTRE_PhoenixTools_Replay", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], - "simMode": "swsim" + "simMode": "hwsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.2.0", - "libName": "CTRE_PhoenixTools_Sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.2.2", + "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.2.0", - "libName": "CTRE_SimTalonSRX", + "artifactId": "tools-sim", + "version": "25.2.2", + "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.2.0", - "libName": "CTRE_SimTalonFX", + "artifactId": "simTalonSRX", + "version": "25.2.2", + "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -248,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "25.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,6 +346,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -263,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "25.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -271,6 +362,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -278,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "25.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -286,6 +378,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -293,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "25.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -301,6 +394,23 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.2", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -308,7 +418,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "25.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -316,6 +426,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -323,7 +434,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "25.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -331,6 +442,23 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.2.2", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index 5157d62..b1be243 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,18 +1,18 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "24.0.6", + "version": "25.0.4", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [ - "https://dl9vkdsodilqp.cloudfront.net/repo" + "https://s3.us-east-2.amazonaws.com/packages.strykeforce.org/repo/" ], - "jsonUrl": "http://maven.strykeforce.org/thirdcoast.json", + "jsonUrl": "https://s3.us-east-2.amazonaws.com/packages.strykeforce.org/thirdcoast.json", "javaDependencies": [ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "24.0.6" + "version": "25.0.4" } ], "jniDependencies": [],