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+ }
0 commit comments