Skip to content

Commit 7697a12

Browse files
mwitcpalekmwitcpalek
committed
Add talonFx to tct
1 parent eba3914 commit 7697a12

27 files changed

+1286
-637
lines changed

‎gradle.properties‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
kotlin.code.style=official

‎src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@ package org.strykeforce.thirdcoast
33
import com.ctre.phoenix.CANifier
44
import com.ctre.phoenix.motorcontrol.FeedbackDevice
55
import com.ctre.phoenix.motorcontrol.NeutralMode
6+
import com.ctre.phoenix.motorcontrol.can.TalonFX
67
import com.ctre.phoenix.motorcontrol.can.TalonSRX
78
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration
89
import edu.wpi.first.wpilibj.DigitalOutput
@@ -34,6 +35,8 @@ val tctModule = module {
3435
single { TelemetryService(Function { inventory -> TelemetryController(inventory, get(), SERVER_PORT) }) }
3536

3637
single { TalonService(get()) { id -> TalonSRX(id) } }
38+
39+
single { TalonFxService(get()) { id -> TalonFX(id) } }
3740

3841
single { ServoService { id -> Servo(id) } }
3942

‎src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package org.strykeforce.thirdcoast.command
22

3+
import mu.KotlinLogging
34
import net.consensys.cava.toml.TomlTable
45
import org.jline.reader.LineReader
56
import org.jline.terminal.Terminal
@@ -20,7 +21,7 @@ import org.strykeforce.thirdcoast.swerve.SelectAzimuthCommand
2021
import org.strykeforce.thirdcoast.swerve.SetAzimuthCommand
2122
import org.strykeforce.thirdcoast.talon.*
2223

23-
//private val logger = KotlinLogging.logger {}
24+
private val logger = KotlinLogging.logger {}
2425

2526
interface Command {
2627
val key: String
@@ -34,10 +35,12 @@ interface Command {
3435
const val MENU_KEY = "menu"
3536
const val TYPE_KEY = "type"
3637
const val ORDER_KEY = "order"
38+
const val DEVICE_KEY = "device"
3739

3840
fun createFromToml(toml: TomlTable, parent: MenuCommand? = null, key: String = "ROOT"): Command {
3941

4042
val type = toml.getString(TYPE_KEY) ?: throw Exception("$key: $TYPE_KEY missing")
43+
logger.info { "type: $type, key: $key" }
4144

4245
return when (type) {
4346
"menu" -> createMenuCommand(parent, key, toml)
@@ -54,6 +57,9 @@ interface Command {
5457
"talon.hard.source" -> SelectHardLimitSourceCommand(parent, key, toml)
5558
"talon.hard.normal" -> SelectHardLimitNormalCommand(parent, key, toml)
5659
"talon.velocity.period" -> SelectVelocityMeasurmentPeriodCommand(parent, key, toml)
60+
"talon.commutation" -> SelectMotorCommutationCommand(parent, key, toml)
61+
"talon.absoluteRange" -> SelectAbsoluteSensorRange(parent, key, toml)
62+
"talon.initStrategy" -> SelectInitializationStrategy(parent, key, toml)
5763
"servo.select" -> SelectServosCommand(parent, key, toml)
5864
"servo.run" -> RunServosCommand(parent, key, toml)
5965
"solenoid.select" -> SelectSolenoidsCommand(parent, key, toml)
@@ -81,6 +87,7 @@ interface Command {
8187
.forEach { k ->
8288
val child = createFromToml(toml.getTable(k)!!, command, k)
8389
command.children.add(child)
90+
logger.info { "Create Menu: $k, ${command.validMenuChoices}" }
8491
}
8592
return command
8693
}

‎src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFXService.kt‎ renamed to ‎src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ private const val SENSOR_PHASE_INVERTED_DEFAULT = false
2121
private const val OUTPUT_INVERTED_DEFAULT = false
2222

2323

24-
class TalonFXService(private val telemetryService: TelemetryService, factory: (id:Int) -> TalonFX ):
24+
class TalonFxService(private val telemetryService: TelemetryService, factory: (id:Int) -> TalonFX ):
2525
AbstractDeviceService<TalonFX>(factory) {
2626

2727
val timeout = 10

‎src/main/kotlin/org/strykeforce/thirdcoast/device/TalonService.kt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@ package org.strykeforce.thirdcoast.device
22

33
import com.ctre.phoenix.motorcontrol.ControlMode
44
import com.ctre.phoenix.motorcontrol.NeutralMode
5+
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration
56
import com.ctre.phoenix.motorcontrol.can.SlotConfiguration
67
import com.ctre.phoenix.motorcontrol.can.TalonSRX
78
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration
@@ -17,6 +18,10 @@ private const val VOLTAGE_COMPENSATION_ENABLED_DEFAULT = true
1718
private const val CURRENT_LIMIT_ENABLED_DEFAULT = false
1819
private const val SENSOR_PHASE_INVERTED_DEFAULT = false
1920
private const val OUTPUT_INVERTED_DEFAULT = false
21+
private const val SUPPLY_CURRENT_LIMIT_ENABLE_DEFAULT = false
22+
private const val SUPPLY_CURRENT_LIMIT_DEFAULT = 0.0
23+
private const val SUPPLY_CURRENT_LIMIT_TRIG_CURRENT_DEFAULT = 0.0
24+
private const val SUPPLY_CURRENT_LIMIT_TRIG_TIME_DEFAULT = 0.0
2025

2126
/**
2227
* Holds the active state for all Talons that have been activated by the user. Talons that are currently active
@@ -50,6 +55,7 @@ class TalonService(private val telemetryService: TelemetryService, factory: (id:
5055
var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT
5156
var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT
5257
var currentLimit = CURRENT_LIMIT_ENABLED_DEFAULT
58+
var supplyCurrentLimit = SupplyCurrentLimitConfiguration(SUPPLY_CURRENT_LIMIT_ENABLE_DEFAULT, SUPPLY_CURRENT_LIMIT_DEFAULT, SUPPLY_CURRENT_LIMIT_TRIG_CURRENT_DEFAULT, SUPPLY_CURRENT_LIMIT_TRIG_TIME_DEFAULT)
5359

5460
var activeConfiguration = TalonSRXConfiguration()
5561
get() {

‎src/main/kotlin/org/strykeforce/thirdcoast/talon/FeedbackCoefficientCommand.kt‎

Lines changed: 21 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,12 @@
11
package org.strykeforce.thirdcoast.talon
22

3+
import com.ctre.phoenix.motorcontrol.can.BaseTalonConfiguration
34
import mu.KotlinLogging
45
import net.consensys.cava.toml.TomlTable
56
import org.koin.standalone.inject
67
import org.strykeforce.thirdcoast.command.AbstractCommand
78
import org.strykeforce.thirdcoast.command.Command
9+
import org.strykeforce.thirdcoast.device.TalonFxService
810
import org.strykeforce.thirdcoast.device.TalonService
911

1012
private val logger = KotlinLogging.logger {}
@@ -16,28 +18,43 @@ class FeedbackCoefficientCommand(
1618
) : AbstractCommand(parent, key, toml) {
1719

1820
private val talonService: TalonService by inject()
21+
private val talonFxService: TalonFxService by inject()
1922

23+
val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing")
2024
private val timeout = talonService.timeout
2125
private val param = TalonParameter.create(this, toml.getString("param") ?: "UNKNOWN")
2226
private val pidIndex = toml.getLong("pid")?.toInt() ?: 0
2327

2428
override val menu: String
2529
get() = formatMenu(
2630
when (pidIndex) {
27-
0 -> talonService.activeConfiguration.primaryPID.selectedFeedbackCoefficient
28-
else -> talonService.activeConfiguration.auxiliaryPID.selectedFeedbackCoefficient
31+
0 -> {
32+
if(type == "srx") talonService.activeConfiguration.primaryPID.selectedFeedbackCoefficient
33+
else if(type == "fx") talonFxService.activeConfiguration.primaryPID.selectedFeedbackCoefficient
34+
else throw IllegalArgumentException()
35+
}
36+
else -> {
37+
if(type == "srx") talonService.activeConfiguration.auxiliaryPID.selectedFeedbackCoefficient
38+
else if(type == "fx") talonFxService.activeConfiguration.auxiliaryPID.selectedFeedbackCoefficient
39+
else throw IllegalArgumentException()
40+
}
2941
}
3042
)
3143

3244
override fun execute(): Command {
33-
val config = talonService.activeConfiguration
45+
var config: BaseTalonConfiguration
46+
if(type == "srx") config = talonService.activeConfiguration
47+
else if(type == "fx") config = talonFxService.activeConfiguration
48+
else throw IllegalArgumentException()
49+
3450
val default = when (pidIndex) {
3551
0 -> config.primaryPID.selectedFeedbackCoefficient
3652
else -> config.auxiliaryPID.selectedFeedbackCoefficient
3753
}
3854

3955
val paramValue = param.readDouble(reader, default)
40-
talonService.active.forEach { it.configSelectedFeedbackCoefficient(paramValue, pidIndex, timeout) }
56+
if(type == "srx") talonService.active.forEach { it.configSelectedFeedbackCoefficient(paramValue, pidIndex, talonService.timeout) }
57+
else if(type == "fx") talonFxService.active.forEach { it.configSelectedFeedbackCoefficient(paramValue, pidIndex, talonFxService.timeout) }
4158
logger.debug { "set ${talonService.active.size} talon ${param.name}: $paramValue" }
4259

4360
return super.execute()

‎src/main/kotlin/org/strykeforce/thirdcoast/talon/HardLimitCommands.kt‎

Lines changed: 59 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,18 @@
11
package org.strykeforce.thirdcoast.talon
22

3+
import com.ctre.phoenix.motorcontrol.FeedbackDevice
34
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal
45
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal.*
56
import com.ctre.phoenix.motorcontrol.LimitSwitchSource
67
import com.ctre.phoenix.motorcontrol.LimitSwitchSource.*
8+
import com.ctre.phoenix.motorcontrol.can.BaseTalon
9+
import com.ctre.phoenix.motorcontrol.can.BaseTalonConfiguration
710
import mu.KotlinLogging
811
import net.consensys.cava.toml.TomlTable
912
import org.koin.standalone.inject
1013
import org.strykeforce.thirdcoast.command.AbstractSelectCommand
1114
import org.strykeforce.thirdcoast.command.Command
15+
import org.strykeforce.thirdcoast.device.TalonFxService
1216
import org.strykeforce.thirdcoast.device.TalonService
1317

1418
private val logger = KotlinLogging.logger {}
@@ -26,31 +30,50 @@ class SelectHardLimitSourceCommand(
2630
) {
2731

2832
private val talonService: TalonService by inject()
33+
private val talonFxService: TalonFxService by inject()
34+
35+
val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing")
2936
val isForward = toml.getBoolean("forward") ?: true
3037

3138
val activeSource
3239
get() = values[activeIndex]
3340

3441

35-
override val activeIndex
36-
get() = values.indexOf(
37-
if (isForward)
38-
talonService.activeConfiguration.forwardLimitSwitchSource
39-
else
40-
talonService.activeConfiguration.reverseLimitSwitchSource
41-
)
42+
override val activeIndex: Int
43+
get() {
44+
var config: BaseTalonConfiguration
45+
if(type == "srx") config = talonService.activeConfiguration
46+
else if(type == "fx") config = talonFxService.activeConfiguration
47+
else throw IllegalArgumentException()
48+
return values.indexOf(
49+
if (isForward)
50+
config.forwardLimitSwitchSource
51+
else
52+
config.reverseLimitSwitchSource
53+
)
54+
}
4255

4356
override fun setActive(index: Int) {
44-
val active = talonService.active
57+
var active: Set<BaseTalon>
58+
var config: BaseTalonConfiguration
59+
if(type == "srx") {
60+
active = talonService.active
61+
config = talonService.activeConfiguration
62+
}
63+
else if(type == "fx") {
64+
active = talonFxService.active
65+
config = talonFxService.activeConfiguration
66+
}
67+
else throw IllegalArgumentException()
4568
val source = values[index]
4669
val normal = getNormal()
4770
if (isForward) {
4871
active.forEach { it.configForwardLimitSwitchSource(source, normal) }
49-
talonService.activeConfiguration.forwardLimitSwitchSource = source
72+
config.forwardLimitSwitchSource = source
5073
logger.info { "set forward hard limit to $source, $normal" }
5174
} else {
5275
active.forEach { it.configReverseLimitSwitchSource(source, normal) }
53-
talonService.activeConfiguration.reverseLimitSwitchSource = source
76+
config.reverseLimitSwitchSource = source
5477
logger.info { "set reverse hard limit to $source, $normal" }
5578
}
5679
}
@@ -78,30 +101,46 @@ class SelectHardLimitNormalCommand(
78101
) {
79102

80103
private val talonService: TalonService by inject()
104+
private val talonFxService: TalonFxService by inject()
105+
val type = toml.getString(Command.DEVICE_KEY) ?: throw Exception("$key: ${Command.DEVICE_KEY} missing")
81106
val isForward = toml.getBoolean("forward") ?: true
82107

108+
var activeConfig = BaseTalonConfiguration(FeedbackDevice.CTRE_MagEncoder_Relative)
109+
83110
val activeNormal
84111
get() = values[activeIndex]
85112

86-
override val activeIndex
87-
get() = values.indexOf(
88-
if (isForward)
89-
talonService.activeConfiguration.forwardLimitSwitchNormal
90-
else
91-
talonService.activeConfiguration.reverseLimitSwitchNormal
92-
)
113+
override val activeIndex: Int
114+
get() {
115+
if(type == "srx") activeConfig = talonService.activeConfiguration
116+
else if(type == "fx") activeConfig = talonFxService.activeConfiguration
117+
else throw IllegalArgumentException()
118+
return values.indexOf(
119+
if (isForward)
120+
activeConfig.forwardLimitSwitchNormal
121+
else
122+
activeConfig.reverseLimitSwitchNormal
123+
)
124+
}
93125

94126
override fun setActive(index: Int) {
95-
val active = talonService.active
127+
val active: Set<BaseTalon>
128+
if(type == "srx"){
129+
activeConfig = talonService.activeConfiguration
130+
active = talonService.active
131+
} else if(type == "fx"){
132+
activeConfig = talonFxService.activeConfiguration
133+
active = talonFxService.active
134+
} else throw IllegalArgumentException()
96135
val source = getSource()
97136
val normal = values[index]
98137
if (isForward) {
99138
active.forEach { it.configForwardLimitSwitchSource(source, normal) }
100-
talonService.activeConfiguration.forwardLimitSwitchNormal = normal
139+
activeConfig.forwardLimitSwitchNormal = normal
101140
logger.info { "set forward hard limit to $source, $normal" }
102141
} else {
103142
active.forEach { it.configReverseLimitSwitchSource(source, normal) }
104-
talonService.activeConfiguration.reverseLimitSwitchNormal = normal
143+
activeConfig.reverseLimitSwitchNormal = normal
105144
logger.info { "set reverse hard limit to $source, $normal" }
106145
}
107146
}

‎src/main/kotlin/org/strykeforce/thirdcoast/talon/RunTalonFxCommand.kt‎

Lines changed: 0 additions & 63 deletions
This file was deleted.

0 commit comments

Comments
 (0)