Skip to content

Commit 0e3a468

Browse files
jhhjhh
committed
Add CANifier support
1 parent af4ae9d commit 0e3a468

27 files changed

+593
-45
lines changed

‎build.gradle‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ buildscript {
33
kotlin_version = '1.3.0'
44
junit_version = '5.2.0'
55
koin_version = '1.0.1'
6-
thirdcoast_version = '18.7.2'
6+
thirdcoast_version = '18.7.3'
77
}
88

99
repositories {
@@ -24,6 +24,7 @@ repositories {
2424
jcenter()
2525
maven { url = "https://raw.githubusercontent.com/Open-RIO/Maven-Mirror/master/m2" }
2626
maven { url = "http://first.wpi.edu/FRC/roborio/maven/release" }
27+
mavenLocal()
2728
}
2829

2930
dependencies {

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

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

3+
import com.ctre.phoenix.CANifier
34
import com.ctre.phoenix.motorcontrol.FeedbackDevice
45
import com.ctre.phoenix.motorcontrol.NeutralMode
56
import com.ctre.phoenix.motorcontrol.can.TalonSRX
@@ -12,6 +13,7 @@ import org.jline.terminal.Terminal
1213
import org.jline.terminal.TerminalBuilder
1314
import org.koin.dsl.module.module
1415
import org.strykeforce.thirdcoast.command.Command
16+
import org.strykeforce.thirdcoast.device.CanifierService
1517
import org.strykeforce.thirdcoast.device.DigitalOutputService
1618
import org.strykeforce.thirdcoast.device.ServoService
1719
import org.strykeforce.thirdcoast.device.TalonService
@@ -39,6 +41,8 @@ val tctModule = module {
3941

4042
single { DigitalOutputService { id -> DigitalOutput(id) } }
4143

44+
single { CanifierService(get()) { id -> CANifier(id) } }
45+
4246
single { (command: Command) -> Shell(command, get()) }
4347

4448
single<Terminal> { TerminalBuilder.terminal() }

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,3 +42,4 @@ internal fun greedyWordwrap(text: String, lineWidth: Int = 72): String {
4242
return sb.toString()
4343
}
4444

45+
internal fun Double.format(digits: Int) = java.lang.String.format("%.${digits}f", this)

‎src/main/kotlin/org/strykeforce/thirdcoast/canifier/CanifierGeneralInputsCommand.kt‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
package org.strykeforce.thirdcoast.canifier
2+
3+
import com.ctre.phoenix.CANifier
4+
import net.consensys.cava.toml.TomlTable
5+
import org.koin.standalone.inject
6+
import org.strykeforce.thirdcoast.command.AbstractCommand
7+
import org.strykeforce.thirdcoast.command.Command
8+
import org.strykeforce.thirdcoast.device.CanifierService
9+
10+
class CanifierGeneralInputsCommand(
11+
parent: Command?,
12+
key: String,
13+
toml: TomlTable
14+
) : AbstractCommand(parent, key, toml) {
15+
16+
private val canifierService: CanifierService by inject()
17+
18+
override fun execute(): Command {
19+
val writer = terminal.writer()
20+
canifierService.active.forEach {
21+
val pins = CANifier.PinValues()
22+
it.getGeneralInputs(pins)
23+
writer.println("${it.deviceID}.LIMF: ${pins.LIMF}")
24+
writer.println("${it.deviceID}.LIMR: ${pins.LIMR}")
25+
writer.println("${it.deviceID}.QUAD_A: ${pins.QUAD_A}")
26+
writer.println("${it.deviceID}.QUAD_B: ${pins.QUAD_B}")
27+
writer.println("${it.deviceID}.QUAD_IDX: ${pins.QUAD_IDX}")
28+
writer.println("${it.deviceID}.SCL: ${pins.SCL}")
29+
writer.println("${it.deviceID}.SPI_CLK_PWM0: ${pins.SPI_CLK_PWM0}")
30+
writer.println("${it.deviceID}.SPI_MOSI_PWM1: ${pins.SPI_MOSI_PWM1}")
31+
writer.println("${it.deviceID}.SPI_MISO_PWM2: ${pins.SPI_MISO_PWM2}")
32+
writer.println("${it.deviceID}.SPI_CS_PWM3: ${pins.SPI_CS_PWM3}")
33+
}
34+
return super.execute()
35+
}
36+
}

‎src/main/kotlin/org/strykeforce/thirdcoast/canifier/CanifierParameter.kt‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
package org.strykeforce.thirdcoast.canifier
2+
3+
import net.consensys.cava.toml.TomlTable
4+
import org.strykeforce.thirdcoast.command.AbstractParameter
5+
import org.strykeforce.thirdcoast.command.Command
6+
import org.strykeforce.thirdcoast.parseResource
7+
8+
class CanifierParameter(command: Command, toml: TomlTable, val enum: Enum) : AbstractParameter(command, toml) {
9+
10+
enum class Enum {
11+
QUAD_POSITION,
12+
STATUS_GENERAL1,
13+
STATUS_GENERAL2,
14+
STATUS_PWM_INPUTS0,
15+
STATUS_PWM_INPUTS1,
16+
STATUS_PWM_INPUTS2,
17+
STATUS_PWM_INPUTS3,
18+
FACTORY_DEFAULTS,
19+
}
20+
21+
companion object {
22+
private val tomlTable by lazy { parseResource("/canifier.toml") }
23+
24+
fun create(command: Command, param: String): CanifierParameter {
25+
val toml = tomlTable.getTable(param) ?: throw IllegalArgumentException("missing param: $param")
26+
return CanifierParameter(command, toml, Enum.valueOf(param))
27+
}
28+
}
29+
30+
}

‎src/main/kotlin/org/strykeforce/thirdcoast/canifier/CanifierParameterCommand.kt‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
package org.strykeforce.thirdcoast.canifier
2+
3+
import com.ctre.phoenix.CANifier
4+
import com.ctre.phoenix.CANifierConfiguration
5+
import com.ctre.phoenix.CANifierStatusFrame
6+
import com.ctre.phoenix.CANifierStatusFrame.*
7+
import com.ctre.phoenix.motorcontrol.can.SlotConfiguration
8+
import mu.KotlinLogging
9+
import net.consensys.cava.toml.TomlTable
10+
import org.koin.standalone.inject
11+
import org.strykeforce.thirdcoast.canifier.CanifierParameter.Enum.*
12+
import org.strykeforce.thirdcoast.command.AbstractCommand
13+
import org.strykeforce.thirdcoast.command.Command
14+
import org.strykeforce.thirdcoast.device.CanifierService
15+
16+
private val logger = KotlinLogging.logger {}
17+
18+
class CanifierParameterCommand(
19+
parent: Command?,
20+
key: String,
21+
toml: TomlTable
22+
) : AbstractCommand(parent, key, toml) {
23+
24+
companion object {
25+
var reset = true
26+
private var slot: SlotConfiguration = SlotConfiguration()
27+
private var config = CANifierConfiguration()
28+
}
29+
30+
private val canifierService: CanifierService by inject()
31+
private val timeout = canifierService.timeout
32+
private val param = CanifierParameter.create(this, toml.getString("param") ?: "UNKNOWN")
33+
34+
override val menu: String
35+
get() {
36+
return when (param.enum) {
37+
QUAD_POSITION -> formatMenu(canifierService.active.firstOrNull()?.quadraturePosition ?: 0)
38+
STATUS_GENERAL1 -> formatMenu(defaultFor(Status_1_General))
39+
STATUS_GENERAL2 -> formatMenu(defaultFor(Status_2_General))
40+
STATUS_PWM_INPUTS0 -> formatMenu(defaultFor(Status_3_PwmInputs0))
41+
STATUS_PWM_INPUTS1 -> formatMenu(defaultFor(Status_4_PwmInputs1))
42+
STATUS_PWM_INPUTS2 -> formatMenu(defaultFor(Status_5_PwmInputs2))
43+
STATUS_PWM_INPUTS3 -> formatMenu(defaultFor(Status_6_PwmInputs3))
44+
FACTORY_DEFAULTS -> tomlMenu
45+
}
46+
}
47+
48+
override fun execute(): Command {
49+
when (param.enum) {
50+
QUAD_POSITION -> configIntParam(0) { talon, value ->
51+
talon.setQuadraturePosition(value, timeout)
52+
}
53+
STATUS_GENERAL1 -> configIntParam(defaultFor(Status_1_General)) { talon, value ->
54+
talon.setStatusFramePeriod(Status_1_General, value, timeout)
55+
}
56+
STATUS_GENERAL2 -> configIntParam(defaultFor(Status_2_General)) { talon, value ->
57+
talon.setStatusFramePeriod(Status_2_General, value, timeout)
58+
}
59+
STATUS_PWM_INPUTS0 -> configIntParam(defaultFor(Status_3_PwmInputs0)) { talon, value ->
60+
talon.setStatusFramePeriod(Status_3_PwmInputs0, value, timeout)
61+
}
62+
STATUS_PWM_INPUTS1 -> configIntParam(defaultFor(Status_4_PwmInputs1)) { talon, value ->
63+
talon.setStatusFramePeriod(Status_4_PwmInputs1, value, timeout)
64+
}
65+
STATUS_PWM_INPUTS2 -> configIntParam(defaultFor(Status_5_PwmInputs2)) { talon, value ->
66+
talon.setStatusFramePeriod(Status_5_PwmInputs2, value, timeout)
67+
}
68+
STATUS_PWM_INPUTS3 -> configIntParam(defaultFor(Status_6_PwmInputs3)) { talon, value ->
69+
talon.setStatusFramePeriod(Status_6_PwmInputs3, value, timeout)
70+
}
71+
FACTORY_DEFAULTS -> configBooleanParam(false) { talon, value ->
72+
if (value) talon.configFactoryDefault(timeout)
73+
}
74+
75+
}
76+
return super.execute()
77+
}
78+
79+
private fun configBooleanParam(default: Boolean, config: (CANifier, Boolean) -> Unit) {
80+
val paramValue = param.readBoolean(reader, default)
81+
canifierService.active.forEach { config(it, paramValue) }
82+
logger.debug { "set ${canifierService.active.size} talon ${param.name}: $paramValue" }
83+
}
84+
85+
86+
private fun configIntParam(default: Int, config: (CANifier, Int) -> Unit) {
87+
val paramValue = param.readInt(reader, default)
88+
canifierService.active.forEach { config(it, paramValue) }
89+
logger.debug { "set ${canifierService.active.size} talon ${param.name}: $paramValue" }
90+
}
91+
92+
private fun configDoubleParam(default: Double, config: (CANifier, Double) -> Unit) {
93+
val paramValue = param.readDouble(reader, default)
94+
canifierService.active.forEach { config(it, paramValue) }
95+
logger.debug { "set ${canifierService.active.size} talon ${param.name}: $paramValue" }
96+
}
97+
98+
private fun defaultFor(frame: CANifierStatusFrame): Int =
99+
canifierService.active.first().getStatusFramePeriod(frame)
100+
101+
}

‎src/main/kotlin/org/strykeforce/thirdcoast/canifier/CanifierPwmInputsCommand.kt‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
package org.strykeforce.thirdcoast.canifier
2+
3+
import com.ctre.phoenix.CANifier
4+
import net.consensys.cava.toml.TomlTable
5+
import org.koin.standalone.inject
6+
import org.strykeforce.thirdcoast.command.AbstractCommand
7+
import org.strykeforce.thirdcoast.command.Command
8+
import org.strykeforce.thirdcoast.device.CanifierService
9+
import org.strykeforce.thirdcoast.format
10+
11+
class CanifierPwmInputsCommand(
12+
parent: Command?,
13+
key: String,
14+
toml: TomlTable
15+
) : AbstractCommand(parent, key, toml) {
16+
17+
private val canifierService: CanifierService by inject()
18+
19+
override fun execute(): Command {
20+
val writer = terminal.writer()
21+
canifierService.active.forEach { canifier ->
22+
val pulseWidthAndPeriod = DoubleArray(2)
23+
CANifier.PWMChannel.values().forEach { channel ->
24+
canifier.getPWMInput(channel, pulseWidthAndPeriod)
25+
printChannel(canifier.deviceID, channel, pulseWidthAndPeriod)
26+
}
27+
}
28+
return super.execute()
29+
}
30+
31+
private fun printChannel(id: Int, channel: CANifier.PWMChannel, pulseWidthAndPeriod: DoubleArray) {
32+
val writer = terminal.writer()
33+
val pulseWidth = pulseWidthAndPeriod[0].format(1)
34+
val period = pulseWidthAndPeriod[1].format(1)
35+
val position = (4096.0 * pulseWidthAndPeriod[0] / pulseWidthAndPeriod[1]).format(0)
36+
writer.println("$id.$channel: pulsewidth = $pulseWidth us, period = $period us, position = $position")
37+
}
38+
}

‎src/main/kotlin/org/strykeforce/thirdcoast/canifier/CanifierQuadInputCommand.kt‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
package org.strykeforce.thirdcoast.canifier
2+
3+
import com.ctre.phoenix.CANifier
4+
import net.consensys.cava.toml.TomlTable
5+
import org.koin.standalone.inject
6+
import org.strykeforce.thirdcoast.command.AbstractCommand
7+
import org.strykeforce.thirdcoast.command.Command
8+
import org.strykeforce.thirdcoast.device.CanifierService
9+
10+
class CanifierQuadInputCommand(
11+
parent: Command?,
12+
key: String,
13+
toml: TomlTable
14+
) : AbstractCommand(parent, key, toml) {
15+
16+
private val canifierService: CanifierService by inject()
17+
18+
override fun execute(): Command {
19+
val writer = terminal.writer()
20+
canifierService.active.forEach { canifier ->
21+
val pos = canifier.quadraturePosition
22+
val spd = canifier.quadratureVelocity
23+
writer.println("${canifier.deviceID}.quadrature encoder: position = $pos, speed = $spd")
24+
}
25+
return super.execute()
26+
}
27+
}

‎src/main/kotlin/org/strykeforce/thirdcoast/canifier/CanifierStatusCommand.kt‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
package org.strykeforce.thirdcoast.canifier
2+
3+
import com.ctre.phoenix.CANifierConfiguration
4+
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration
5+
import net.consensys.cava.toml.TomlTable
6+
import org.koin.standalone.inject
7+
import org.strykeforce.thirdcoast.command.AbstractCommand
8+
import org.strykeforce.thirdcoast.command.Command
9+
import org.strykeforce.thirdcoast.device.CanifierService
10+
import org.strykeforce.thirdcoast.device.TalonService
11+
12+
class CanifierStatusCommand(
13+
parent: Command?,
14+
key: String,
15+
toml: TomlTable
16+
) : AbstractCommand(parent, key, toml) {
17+
18+
private val canifierService: CanifierService by inject()
19+
20+
override fun execute(): Command {
21+
val writer = terminal.writer()
22+
canifierService.active.forEach {
23+
val config = CANifierConfiguration()
24+
it.getAllConfigs(config)
25+
writer.println(config.toString(it.deviceID.toString()))
26+
}
27+
return super.execute()
28+
}
29+
}

‎src/main/kotlin/org/strykeforce/thirdcoast/canifier/RunCanifierPwmCommand.kt‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
package org.strykeforce.thirdcoast.canifier
2+
3+
import mu.KotlinLogging
4+
import net.consensys.cava.toml.TomlTable
5+
import org.jline.reader.EndOfFileException
6+
import org.koin.standalone.inject
7+
import org.strykeforce.thirdcoast.command.AbstractCommand
8+
import org.strykeforce.thirdcoast.command.Command
9+
import org.strykeforce.thirdcoast.command.prompt
10+
import org.strykeforce.thirdcoast.device.CanifierService
11+
import org.strykeforce.thirdcoast.warn
12+
13+
private val logger = KotlinLogging.logger {}
14+
15+
class RunCanifierPwmCommand(
16+
parent: Command?,
17+
key: String,
18+
toml: TomlTable
19+
) : AbstractCommand(parent, key, toml) {
20+
21+
private val canifierService: CanifierService by inject()
22+
23+
override fun execute(): Command {
24+
while (true) {
25+
try {
26+
val line = reader.readLine(prompt("channel, duty cycle"))
27+
if (line.isEmpty()) throw EndOfFileException()
28+
val setpoints = line.split(',')
29+
30+
if (setpoints.size != 2) throw IllegalArgumentException()
31+
32+
33+
val channel = setpoints[0].toInt()
34+
if (!(0..2).contains(channel)) throw IllegalArgumentException("PWM output valid for channels 0, 1, and 2 only")
35+
36+
37+
val dutyCycle = setpoints[1].toDouble()
38+
if (!(0.0..1.0).contains(dutyCycle)) throw IllegalArgumentException("duty cycle must be in range 0.0 - 1.0")
39+
40+
canifierService.active.forEach { it.enablePWMOutput(channel, true) }
41+
canifierService.active.forEach { it.setPWMOutput(channel, dutyCycle) }
42+
logger.debug { "set PWM channel $channel to duty cycle $dutyCycle" }
43+
break
44+
} catch (e: Exception) {
45+
terminal.warn(e.message ?: "please enter PWM channel and duty cycle separated by ','")
46+
}
47+
}
48+
49+
return super.execute()
50+
}
51+
}

0 commit comments

Comments
 (0)