1- package org.strykeforce.thirdcoast.device
2-
3- import com.ctre.phoenix.motorcontrol.ControlMode
4- import com.ctre.phoenix.motorcontrol.NeutralMode
5- import com.ctre.phoenix.motorcontrol.StatusFrame
6- import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced
7- import com.ctre.phoenix.motorcontrol.can.SlotConfiguration
8- import com.ctre.phoenix.motorcontrol.can.TalonFX
9- import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration
10- import mu.KotlinLogging
11- import org.strykeforce.thirdcoast.telemetry.TelemetryService
12- import java.lang.IllegalStateException
13- import kotlin.math.log
14-
15- private val logger = KotlinLogging .logger {}
16-
17- private val CONTROL_MODE_DEFAULT = ControlMode .PercentOutput
18- private const val ACTIVE_SLOT_DEFAULT = 0
19- private val NEUTRAL_MODE_DEFAULT = NeutralMode .EEPROMSetting
20- private const val VOLTAGE_COMPENSATION_ENABLED_DEFAULT = true
21- private const val CURRENT_LIMIT_ENABLED_DEFAULT = false
22- private const val SENSOR_PHASE_INVERTED_DEFAULT = false
23- private const val OUTPUT_INVERTED_DEFAULT = false
24-
25-
26- class TalonFxService (private val telemetryService : TelemetryService , factory : (id:Int ) -> TalonFX ):
27- AbstractDeviceService <TalonFX >(factory) {
28-
29- val timeout = 10
30- var dirty = true
31- var neutralMode = NEUTRAL_MODE_DEFAULT
32- var controlMode = CONTROL_MODE_DEFAULT
33- var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT
34- var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT
35- var activeSlotIndex: Int = ACTIVE_SLOT_DEFAULT
36-
37- var activeConfiguration = TalonFXConfiguration ()
38- get() {
39- if (! dirty) return field
40- active.firstOrNull()?.getAllConfigs(field,timeout)? : logger.debug(" no active talon fx's, returning default config" )
41- dirty = false
42- return field
43- }
44-
45- val activeSlot: SlotConfiguration
46- get() = when (activeSlotIndex){
47- 0 -> activeConfiguration.slot0
48- 1 -> activeConfiguration.slot1
49- 2 -> activeConfiguration.slot2
50- 3 -> activeConfiguration.slot3
51- else -> throw IllegalStateException (" invalid slot: $activeSlotIndex " )
52- }
53-
54- val outputInverted: Boolean
55- get() = active.firstOrNull()?.inverted? : OUTPUT_INVERTED_DEFAULT
56-
57- override fun activate (ids : Collection <Int >): Set <Int > {
58- dirty = true
59-
60- val new = super .activate(ids)
61- telemetryService.stop()
62- active.filter { new.contains(it.deviceID) }.forEach(){
63- it.setNeutralMode(neutralMode)
64- it.selectProfileSlot(activeSlotIndex,0 )
65- it.enableVoltageCompensation(voltageCompensation)
66- it.setSensorPhase(sensorPhase)
67- it.setInverted(OUTPUT_INVERTED_DEFAULT )
68- telemetryService.register(it)
69- }
70- telemetryService.start()
71- return new
72- }
1+ package org.strykeforce.thirdcoast.device
2+
3+ import com.ctre.phoenix.motorcontrol.ControlMode
4+ import com.ctre.phoenix.motorcontrol.NeutralMode
5+ import com.ctre.phoenix.motorcontrol.StatusFrame
6+ import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced
7+ import com.ctre.phoenix.motorcontrol.can.SlotConfiguration
8+ import com.ctre.phoenix.motorcontrol.can.TalonFX
9+ import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration
10+ import mu.KotlinLogging
11+ import org.strykeforce.thirdcoast.telemetry.TelemetryService
12+ import java.lang.IllegalStateException
13+ import kotlin.math.log
14+
15+ private val logger = KotlinLogging .logger {}
16+
17+ private val CONTROL_MODE_DEFAULT = ControlMode .PercentOutput
18+ private const val ACTIVE_SLOT_DEFAULT = 0
19+ private val NEUTRAL_MODE_DEFAULT = NeutralMode .EEPROMSetting
20+ private const val VOLTAGE_COMPENSATION_ENABLED_DEFAULT = true
21+ private const val CURRENT_LIMIT_ENABLED_DEFAULT = false
22+ private const val SENSOR_PHASE_INVERTED_DEFAULT = false
23+ private const val OUTPUT_INVERTED_DEFAULT = false
24+
25+
26+ class TalonFxService (private val telemetryService : TelemetryService , factory : (id:Int ) -> TalonFX ):
27+ AbstractDeviceService <TalonFX >(factory) {
28+
29+ val timeout = 10
30+ var dirty = true
31+ var neutralMode = NEUTRAL_MODE_DEFAULT
32+ var controlMode = CONTROL_MODE_DEFAULT
33+ var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT
34+ var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT
35+ var activeSlotIndex: Int = ACTIVE_SLOT_DEFAULT
36+
37+ var activeConfiguration = TalonFXConfiguration ()
38+ get() {
39+ if (! dirty) return field
40+ active.firstOrNull()?.getAllConfigs(field,timeout)? : logger.debug(" no active talon fx's, returning default config" )
41+ dirty = false
42+ return field
43+ }
44+
45+ val activeSlot: SlotConfiguration
46+ get() = when (activeSlotIndex){
47+ 0 -> activeConfiguration.slot0
48+ 1 -> activeConfiguration.slot1
49+ 2 -> activeConfiguration.slot2
50+ 3 -> activeConfiguration.slot3
51+ else -> throw IllegalStateException (" invalid slot: $activeSlotIndex " )
52+ }
53+
54+ val outputInverted: Boolean
55+ get() = active.firstOrNull()?.inverted? : OUTPUT_INVERTED_DEFAULT
56+
57+ override fun activate (ids : Collection <Int >): Set <Int > {
58+ dirty = true
59+
60+ val new = super .activate(ids)
61+ telemetryService.stop()
62+ active.filter { new.contains(it.deviceID) }.forEach(){
63+ it.setNeutralMode(neutralMode)
64+ it.selectProfileSlot(activeSlotIndex,0 )
65+ it.enableVoltageCompensation(voltageCompensation)
66+ it.setSensorPhase(sensorPhase)
67+ it.setInverted(OUTPUT_INVERTED_DEFAULT )
68+ telemetryService.register(it)
69+ }
70+ telemetryService.start()
71+ return new
72+ }
7373}
0 commit comments