Skip to content

Commit 8825a59

Browse files
committed
Add pigeonIMU
1 parent 7697a12 commit 8825a59

File tree

13 files changed

+601
-2
lines changed

13 files changed

+601
-2
lines changed

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

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode
66
import com.ctre.phoenix.motorcontrol.can.TalonFX
77
import com.ctre.phoenix.motorcontrol.can.TalonSRX
88
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration
9+
import com.ctre.phoenix.sensors.PigeonIMU
910
import edu.wpi.first.wpilibj.DigitalOutput
1011
import edu.wpi.first.wpilibj.Servo
1112
import edu.wpi.first.wpilibj.Solenoid
@@ -46,6 +47,8 @@ val tctModule = module {
4647

4748
single { CanifierService(get()) { id -> CANifier(id) } }
4849

50+
single { PigeonService(get()) { id -> PigeonIMU(id) } }
51+
4952
single { (command: Command) -> Shell(command, get()) }
5053

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

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

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,8 @@ import org.koin.standalone.inject
1111
import org.strykeforce.thirdcoast.canifier.*
1212
import org.strykeforce.thirdcoast.dio.RunDigitalOutputsCommand
1313
import org.strykeforce.thirdcoast.dio.SelectDigitalOutputsCommand
14+
import org.strykeforce.thirdcoast.gyro.PigeonParameterCommand
15+
import org.strykeforce.thirdcoast.gyro.SelectPigeonCommand
1416
import org.strykeforce.thirdcoast.servo.RunServosCommand
1517
import org.strykeforce.thirdcoast.servo.SelectServosCommand
1618
import org.strykeforce.thirdcoast.solenoid.RunSolenoidsCommand
@@ -77,6 +79,8 @@ interface Command {
7779
"swerve.azimuth.save" -> SaveZeroCommand(parent, key, toml)
7880
"swerve.azimuth.select" -> SelectAzimuthCommand(parent, key, toml)
7981
"swerve.azimuth.adjust" -> AdjustAzimuthCommand(parent, key, toml)
82+
"pigeon.select" -> SelectPigeonCommand(parent, key, toml)
83+
"pigeon.param" -> PigeonParameterCommand(parent, key, toml)
8084
else -> DefaultCommand(parent, key, toml)
8185
}
8286
}
Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
package org.strykeforce.thirdcoast.device
2+
3+
import com.ctre.phoenix.sensors.PigeonIMU
4+
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame
5+
import org.strykeforce.thirdcoast.telemetry.item.PigeonIMUItem
6+
import mu.KotlinLogging
7+
import org.strykeforce.thirdcoast.telemetry.TelemetryService
8+
9+
private val logger = KotlinLogging.logger {}
10+
11+
private const val DEFAULT_TEMP_COMP = false
12+
13+
14+
class PigeonService(private val telemetryService: TelemetryService, factory: (id: Int) -> PigeonIMU) :
15+
AbstractDeviceService<PigeonIMU>(factory) {
16+
17+
val timeout = 10
18+
var tempCompensation = DEFAULT_TEMP_COMP
19+
20+
21+
override fun activate(ids: Collection<Int>): Set<Int> {
22+
val new = super.activate(ids)
23+
telemetryService.stop()
24+
active.forEach {telemetryService.register(PigeonIMUItem(it)) }
25+
active.forEach {
26+
it.setTemperatureCompensationDisable(tempCompensation)
27+
}
28+
29+
telemetryService.start()
30+
return new
31+
}
32+
33+
}

src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@ 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.StatusFrame
6+
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced
57
import com.ctre.phoenix.motorcontrol.can.SlotConfiguration
68
import com.ctre.phoenix.motorcontrol.can.TalonFX
79
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration
Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
package org.strykeforce.thirdcoast.gyro
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+
import org.strykeforce.thirdcoast.talon.TalonParameter
8+
9+
class PigeonParameter(command: Command, toml: TomlTable, val enum: PigeonParameter.Enum) : AbstractParameter(command, toml) {
10+
11+
enum class Enum {
12+
ACCUM_Z_ANGLE,
13+
FUSED_HEADING,
14+
TEMP_COMP_DISABLE,
15+
YAW,
16+
GENERAL_STATUS,
17+
SIX_DEG_STATUS,
18+
FUSED_STATUS,
19+
GYRO_ACCUM_STATUS,
20+
GEN_COMPASS_STATUS,
21+
GEN_ACCEL_STATUS,
22+
SIX_QUAT_STATUS,
23+
MAG_STATUS,
24+
BIAS_GYRO_STATUS,
25+
BIAS_MAG_STATUS,
26+
BIAS_ACCEL_STATUS,
27+
FACTORY_DEFAULT,
28+
}
29+
30+
companion object {
31+
private val tomlTable by lazy { parseResource("/pigeon.toml") }
32+
33+
fun create(command: Command, param: String): PigeonParameter {
34+
val toml = tomlTable.getTable(param) ?: throw java.lang.IllegalArgumentException("missing param: $param")
35+
return PigeonParameter(command, toml, Enum.valueOf(param))
36+
}
37+
}
38+
}
Lines changed: 144 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,144 @@
1+
package org.strykeforce.thirdcoast.gyro
2+
3+
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced
4+
import com.ctre.phoenix.motorcontrol.can.TalonFX
5+
import com.ctre.phoenix.motorcontrol.can.TalonSRX
6+
import com.ctre.phoenix.sensors.PigeonIMU
7+
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame
8+
import mu.KotlinLogging
9+
import net.consensys.cava.toml.TomlTable
10+
import org.koin.standalone.inject
11+
import org.strykeforce.thirdcoast.command.AbstractCommand
12+
import org.strykeforce.thirdcoast.command.Command
13+
import org.strykeforce.thirdcoast.device.PigeonService
14+
15+
private val logger = KotlinLogging.logger {}
16+
17+
class PigeonParameterCommand(
18+
parent: Command?,
19+
key: String,
20+
toml: TomlTable
21+
) : AbstractCommand(parent, key, toml) {
22+
23+
private val pigeonService: PigeonService by inject()
24+
private val param = PigeonParameter.create(this, toml.getString("param")?: "UNKNOWN")
25+
26+
override val menu: String
27+
get() {
28+
var xyzArray = DoubleArray(3)
29+
return when(param.enum){
30+
PigeonParameter.Enum.ACCUM_Z_ANGLE -> {
31+
pigeonService.active.map { it.getAccumGyro(xyzArray) }
32+
formatMenu(xyzArray[2])
33+
}
34+
PigeonParameter.Enum.FUSED_HEADING -> formatMenu(pigeonService.active.map { it.fusedHeading }.joinToString())
35+
PigeonParameter.Enum.TEMP_COMP_DISABLE -> formatMenu(pigeonService.tempCompensation)
36+
PigeonParameter.Enum.YAW -> {
37+
pigeonService.active.map { it.getYawPitchRoll(xyzArray) }
38+
formatMenu(xyzArray[0])
39+
}
40+
PigeonParameter.Enum.GENERAL_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.CondStatus_1_General))
41+
PigeonParameter.Enum.SIX_DEG_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR))
42+
PigeonParameter.Enum.FUSED_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion))
43+
PigeonParameter.Enum.GYRO_ACCUM_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.CondStatus_3_GeneralAccel))
44+
PigeonParameter.Enum.GEN_COMPASS_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.CondStatus_2_GeneralCompass))
45+
PigeonParameter.Enum.GEN_ACCEL_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.CondStatus_3_GeneralAccel))
46+
PigeonParameter.Enum.SIX_QUAT_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.CondStatus_10_SixDeg_Quat))
47+
PigeonParameter.Enum.MAG_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.RawStatus_4_Mag))
48+
PigeonParameter.Enum.BIAS_GYRO_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.BiasedStatus_2_Gyro))
49+
PigeonParameter.Enum.BIAS_MAG_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.BiasedStatus_4_Mag))
50+
PigeonParameter.Enum.BIAS_ACCEL_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame.BiasedStatus_6_Accel))
51+
PigeonParameter.Enum.FACTORY_DEFAULT -> tomlMenu
52+
else -> TODO("${param.enum} not implemented")
53+
}
54+
}
55+
56+
override fun execute(): Command {
57+
val timeout = pigeonService.timeout
58+
when(param.enum){
59+
PigeonParameter.Enum.ACCUM_Z_ANGLE -> {
60+
val xyzArray = DoubleArray(3)
61+
pigeonService.active.firstOrNull()?.getAccumGyro(xyzArray)
62+
val default = xyzArray[2]
63+
val paramValue = param.readDouble(reader, default)
64+
pigeonService.active.forEach { it.setAccumZAngle(paramValue, timeout) }
65+
}
66+
PigeonParameter.Enum.FUSED_HEADING -> {
67+
val default = pigeonService.active.firstOrNull()?.fusedHeading?: 0.0
68+
val paramValue = param.readDouble(reader, default)
69+
pigeonService.active.forEach { it.setFusedHeading(paramValue, timeout) }
70+
}
71+
PigeonParameter.Enum.TEMP_COMP_DISABLE -> configBooleanParam(pigeonService.tempCompensation){ pigeon, value ->
72+
pigeon.setTemperatureCompensationDisable(value, timeout)
73+
pigeonService.tempCompensation = value
74+
}
75+
PigeonParameter.Enum.YAW -> {
76+
val rpyArray = DoubleArray(3)
77+
pigeonService.active.firstOrNull()?.getYawPitchRoll(rpyArray)
78+
val default = rpyArray[0]
79+
val paramValue = param.readDouble(reader, default)
80+
pigeonService.active.forEach { it.setYaw(paramValue, timeout) }
81+
}
82+
PigeonParameter.Enum.GENERAL_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.CondStatus_1_General)){ pigeon, value ->
83+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, value, timeout)
84+
}
85+
PigeonParameter.Enum.SIX_DEG_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR)){ pigeon, value ->
86+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, value, timeout)
87+
}
88+
PigeonParameter.Enum.FUSED_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion)){ pigeon, value ->
89+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, value, timeout)
90+
}
91+
PigeonParameter.Enum.GYRO_ACCUM_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.CondStatus_11_GyroAccum)){ pigeon, value ->
92+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_11_GyroAccum, value, timeout)
93+
}
94+
PigeonParameter.Enum.GEN_COMPASS_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.CondStatus_2_GeneralCompass)){ pigeon, value ->
95+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_2_GeneralCompass, value, timeout)
96+
}
97+
PigeonParameter.Enum.GEN_ACCEL_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.CondStatus_3_GeneralAccel)){ pigeon, value ->
98+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_3_GeneralAccel, value, timeout)
99+
}
100+
PigeonParameter.Enum.SIX_QUAT_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.CondStatus_10_SixDeg_Quat)){ pigeon, value ->
101+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_10_SixDeg_Quat, value, timeout)
102+
}
103+
PigeonParameter.Enum.MAG_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.RawStatus_4_Mag)){ pigeon, value ->
104+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.RawStatus_4_Mag, value, timeout)
105+
}
106+
PigeonParameter.Enum.BIAS_GYRO_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.BiasedStatus_2_Gyro)){ pigeon, value ->
107+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.BiasedStatus_2_Gyro, value, timeout)
108+
}
109+
PigeonParameter.Enum.BIAS_MAG_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.BiasedStatus_4_Mag)){ pigeon, value ->
110+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.BiasedStatus_4_Mag, value, timeout)
111+
}
112+
PigeonParameter.Enum.BIAS_ACCEL_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame.BiasedStatus_6_Accel)){ pigeon, value ->
113+
pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.BiasedStatus_6_Accel, value, timeout)
114+
}
115+
PigeonParameter.Enum.FACTORY_DEFAULT -> configBooleanParam(false){ pigeon, value ->
116+
if(value) pigeon.configFactoryDefault(timeout)
117+
}
118+
else -> TODO("${param.enum} not implemented")
119+
}
120+
return super.execute()
121+
}
122+
123+
private fun configBooleanParam(default: Boolean, config: (PigeonIMU, Boolean) -> Unit) {
124+
val paramValue = param.readBoolean(reader, default)
125+
pigeonService.active.forEach { config(it, paramValue) }
126+
logger.debug { "set ${pigeonService.active.size} pigeon ${param.name}: $paramValue" }
127+
}
128+
129+
private fun configDoubleParam(default: Double, config: (PigeonIMU, Double) -> Unit) {
130+
val paramValue = param.readDouble(reader, default)
131+
pigeonService.active.forEach { config(it, paramValue) }
132+
logger.debug { "set ${pigeonService.active.size} pigeon ${param.name}: $paramValue" }
133+
}
134+
135+
private fun configIntParam(default: Int, config: (PigeonIMU, Int) -> Unit) {
136+
val paramValue = param.readInt(reader, default)
137+
pigeonService.active.forEach { config(it, paramValue) }
138+
logger.debug { "set ${pigeonService.active.size} pigeon ${param.name}: $paramValue" }
139+
}
140+
141+
private fun defaultFor(frame: PigeonIMU_StatusFrame): Int {
142+
return pigeonService.active.first().getStatusFramePeriod(frame)
143+
}
144+
}
Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
package org.strykeforce.thirdcoast.gyro
2+
3+
import com.ctre.phoenix.motorcontrol.can.TalonSRX
4+
import com.ctre.phoenix.sensors.PigeonIMU
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.command.prompt
10+
import org.strykeforce.thirdcoast.device.PigeonService
11+
import org.strykeforce.thirdcoast.info
12+
import org.strykeforce.thirdcoast.readIntList
13+
import org.strykeforce.thirdcoast.warn
14+
15+
class SelectPigeonCommand(
16+
parent: Command?,
17+
key: String,
18+
toml: TomlTable
19+
) : AbstractCommand(parent, key, toml) {
20+
21+
val pigeonService: PigeonService by inject()
22+
23+
override val menu: String
24+
get() = formatMenu(pigeonService.active.map(PigeonIMU::getDeviceID).joinToString())
25+
26+
override fun execute(): Command {
27+
while(true){
28+
try {
29+
var ids: List<Int>
30+
var new: Set<Int>
31+
32+
ids = reader.readIntList(this.prompt("ids"), pigeonService.active.map(PigeonIMU::getDeviceID))
33+
new = pigeonService.activate(ids)
34+
if (new.isNotEmpty())
35+
terminal.info(
36+
"reset temp compensation and compass declination for pigeon: ${new.joinToString()}"
37+
)
38+
return super.execute()
39+
} catch (e: IllegalArgumentException) {
40+
terminal.warn("Please enter a list of Pigeon ids separated by ','")
41+
}
42+
43+
}
44+
}
45+
}

src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameter.kt

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,13 @@ class TalonParameter(command: Command, toml: TomlTable, val enum: Enum) : Abstra
5151
STATUS_PULSE_WIDTH,
5252
STATUS_MOTION,
5353
STATUS_PIDF0,
54+
STATUS_MISC,
55+
STATUS_COMM,
56+
STATUS_MOTION_BUFF,
57+
STATUS_FEEDBACK1,
58+
STATUS_PIDF1,
59+
STATUS_FIRMWARE_API,
60+
STATUS_UART_GADGETEER,
5461
SOFT_LIMIT_ENABLE_FORWARD,
5562
SOFT_LIMIT_ENABLE_REVERSE,
5663
SOFT_LIMIT_THRESHOLD_FORWARD,

src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -114,6 +114,13 @@ class TalonParameterCommand(
114114
STATUS_PULSE_WIDTH -> formatMenu(defaultFor(Status_8_PulseWidth))
115115
STATUS_MOTION -> formatMenu(defaultFor(Status_10_MotionMagic))
116116
STATUS_PIDF0 -> formatMenu(defaultFor(Status_13_Base_PIDF0))
117+
STATUS_MISC -> formatMenu(defaultFor(Status_6_Misc))
118+
STATUS_COMM -> formatMenu(defaultFor(Status_7_CommStatus))
119+
STATUS_MOTION_BUFF -> formatMenu(defaultFor(Status_9_MotProfBuffer))
120+
STATUS_FEEDBACK1 -> formatMenu(defaultFor(Status_12_Feedback1))
121+
STATUS_PIDF1 -> formatMenu(defaultFor(Status_14_Turn_PIDF1))
122+
STATUS_FIRMWARE_API -> formatMenu(defaultFor(Status_15_FirmwareApiStatus))
123+
STATUS_UART_GADGETEER -> formatMenu(defaultFor(Status_11_UartGadgeteer))
117124
SOFT_LIMIT_ENABLE_FORWARD -> formatMenu(baseConfig.forwardSoftLimitEnable)
118125
SOFT_LIMIT_ENABLE_REVERSE -> formatMenu(baseConfig.reverseSoftLimitEnable)
119126
SOFT_LIMIT_THRESHOLD_FORWARD -> formatMenu(baseConfig.forwardSoftLimitThreshold)
@@ -350,6 +357,27 @@ class TalonParameterCommand(
350357
STATUS_PIDF0 -> configIntParam(defaultFor(Status_13_Base_PIDF0)) { baseTalon, value ->
351358
baseTalon.setStatusFramePeriod(Status_13_Base_PIDF0, value, timeout)
352359
}
360+
STATUS_MISC -> configIntParam(defaultFor(Status_6_Misc)){ baseTalon, value ->
361+
baseTalon.setStatusFramePeriod(Status_6_Misc, value, timeout)
362+
}
363+
STATUS_COMM -> configIntParam(defaultFor(Status_7_CommStatus)){ baseTalon, value ->
364+
baseTalon.setStatusFramePeriod(Status_7_CommStatus, value, timeout)
365+
}
366+
STATUS_MOTION_BUFF -> configIntParam(defaultFor(Status_9_MotProfBuffer)){ baseTalon, value ->
367+
baseTalon.setStatusFramePeriod(Status_9_MotProfBuffer, value, timeout)
368+
}
369+
STATUS_FEEDBACK1 -> configIntParam(defaultFor(Status_12_Feedback1)){ baseTalon, value ->
370+
baseTalon.setStatusFramePeriod(Status_12_Feedback1, value, timeout)
371+
}
372+
STATUS_PIDF1 -> configIntParam(defaultFor(Status_14_Turn_PIDF1)){ baseTalon, value ->
373+
baseTalon.setStatusFramePeriod(Status_14_Turn_PIDF1, value, timeout)
374+
}
375+
STATUS_FIRMWARE_API -> configIntParam(defaultFor(Status_15_FirmwareApiStatus)){ baseTalon, value ->
376+
baseTalon.setStatusFramePeriod(Status_15_FirmwareApiStatus, value, timeout)
377+
}
378+
STATUS_UART_GADGETEER -> configIntParam(defaultFor(Status_11_UartGadgeteer)){ baseTalon, value ->
379+
baseTalon.setStatusFramePeriod(Status_11_UartGadgeteer, value, timeout)
380+
}
353381
SOFT_LIMIT_ENABLE_FORWARD -> configBooleanParam(config.forwardSoftLimitEnable) { baseTalon, value ->
354382
baseTalon.configForwardSoftLimitEnable(value, timeout)
355383
config.forwardSoftLimitEnable = value

0 commit comments

Comments
 (0)