11package org.strykeforce.thirdcoast.gyro
22
3- import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced
4- import com.ctre.phoenix.motorcontrol.can.TalonFX
5- import com.ctre.phoenix.motorcontrol.can.TalonSRX
63import com.ctre.phoenix.sensors.PigeonIMU
74import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame
85import mu.KotlinLogging
@@ -11,6 +8,7 @@ import org.koin.standalone.inject
118import org.strykeforce.thirdcoast.command.AbstractCommand
129import org.strykeforce.thirdcoast.command.Command
1310import org.strykeforce.thirdcoast.device.PigeonService
11+ import org.strykeforce.thirdcoast.gyro.PigeonParameter.Enum.*
1412
1513private val logger = KotlinLogging .logger {}
1614
@@ -21,101 +19,99 @@ class PigeonParameterCommand(
2119) : AbstractCommand(parent, key, toml) {
2220
2321 private val pigeonService: PigeonService by inject()
24- private val param = PigeonParameter .create(this , toml.getString(" param" )? : " UNKNOWN" )
22+ private val param = PigeonParameter .create(this , toml.getString(" param" ) ? : " UNKNOWN" )
2523
2624 override val menu: String
2725 get() {
2826 var xyzArray = DoubleArray (3 )
29- return when (param.enum){
27+ return when (param.enum) {
3028 PigeonParameter .Enum .ACCUM_Z_ANGLE -> {
3129 pigeonService.active.map { it.getAccumGyro(xyzArray) }
3230 formatMenu(xyzArray[2 ])
3331 }
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 -> {
32+ FUSED_HEADING -> formatMenu(pigeonService.active.map { it.fusedHeading }.joinToString())
33+ TEMP_COMP_DISABLE -> formatMenu(pigeonService.tempCompensation)
34+ YAW -> {
3735 pigeonService.active.map { it.getYawPitchRoll(xyzArray) }
3836 formatMenu(xyzArray[0 ])
3937 }
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" )
38+ GENERAL_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .CondStatus_1_General ))
39+ SIX_DEG_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .CondStatus_9_SixDeg_YPR ))
40+ FUSED_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .CondStatus_6_SensorFusion ))
41+ GYRO_ACCUM_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .CondStatus_3_GeneralAccel ))
42+ GEN_COMPASS_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .CondStatus_2_GeneralCompass ))
43+ GEN_ACCEL_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .CondStatus_3_GeneralAccel ))
44+ SIX_QUAT_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .CondStatus_10_SixDeg_Quat ))
45+ MAG_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .RawStatus_4_Mag ))
46+ BIAS_GYRO_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .BiasedStatus_2_Gyro ))
47+ BIAS_MAG_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .BiasedStatus_4_Mag ))
48+ BIAS_ACCEL_STATUS -> formatMenu(defaultFor(PigeonIMU_StatusFrame .BiasedStatus_6_Accel ))
49+ FACTORY_DEFAULT -> tomlMenu
5350 }
5451 }
5552
5653 override fun execute (): Command {
5754 val timeout = pigeonService.timeout
58- when (param.enum){
55+ when (param.enum) {
5956 PigeonParameter .Enum .ACCUM_Z_ANGLE -> {
6057 val xyzArray = DoubleArray (3 )
6158 pigeonService.active.firstOrNull()?.getAccumGyro(xyzArray)
6259 val default = xyzArray[2 ]
6360 val paramValue = param.readDouble(reader, default)
6461 pigeonService.active.forEach { it.setAccumZAngle(paramValue, timeout) }
6562 }
66- PigeonParameter . Enum . FUSED_HEADING -> {
67- val default = pigeonService.active.firstOrNull()?.fusedHeading? : 0.0
63+ FUSED_HEADING -> {
64+ val default = pigeonService.active.firstOrNull()?.fusedHeading ? : 0.0
6865 val paramValue = param.readDouble(reader, default)
6966 pigeonService.active.forEach { it.setFusedHeading(paramValue, timeout) }
7067 }
71- PigeonParameter . Enum . TEMP_COMP_DISABLE -> configBooleanParam(pigeonService.tempCompensation){ pigeon, value ->
68+ TEMP_COMP_DISABLE -> configBooleanParam(pigeonService.tempCompensation) { pigeon, value ->
7269 pigeon.setTemperatureCompensationDisable(value, timeout)
7370 pigeonService.tempCompensation = value
7471 }
75- PigeonParameter . Enum . YAW -> {
72+ YAW -> {
7673 val rpyArray = DoubleArray (3 )
7774 pigeonService.active.firstOrNull()?.getYawPitchRoll(rpyArray)
7875 val default = rpyArray[0 ]
7976 val paramValue = param.readDouble(reader, default)
8077 pigeonService.active.forEach { it.setYaw(paramValue, timeout) }
8178 }
82- PigeonParameter . Enum . GENERAL_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_1_General )){ pigeon, value ->
79+ GENERAL_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_1_General )) { pigeon, value ->
8380 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .CondStatus_1_General , value, timeout)
8481 }
85- PigeonParameter . Enum . SIX_DEG_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_9_SixDeg_YPR )){ pigeon, value ->
82+ SIX_DEG_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_9_SixDeg_YPR )) { pigeon, value ->
8683 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .CondStatus_9_SixDeg_YPR , value, timeout)
8784 }
88- PigeonParameter . Enum . FUSED_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_6_SensorFusion )){ pigeon, value ->
85+ FUSED_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_6_SensorFusion )) { pigeon, value ->
8986 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .CondStatus_6_SensorFusion , value, timeout)
9087 }
91- PigeonParameter . Enum . GYRO_ACCUM_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_11_GyroAccum )){ pigeon, value ->
88+ GYRO_ACCUM_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_11_GyroAccum )) { pigeon, value ->
9289 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .CondStatus_11_GyroAccum , value, timeout)
9390 }
94- PigeonParameter . Enum . GEN_COMPASS_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_2_GeneralCompass )){ pigeon, value ->
91+ GEN_COMPASS_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_2_GeneralCompass )) { pigeon, value ->
9592 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .CondStatus_2_GeneralCompass , value, timeout)
9693 }
97- PigeonParameter . Enum . GEN_ACCEL_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_3_GeneralAccel )){ pigeon, value ->
94+ GEN_ACCEL_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_3_GeneralAccel )) { pigeon, value ->
9895 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .CondStatus_3_GeneralAccel , value, timeout)
9996 }
100- PigeonParameter . Enum . SIX_QUAT_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_10_SixDeg_Quat )){ pigeon, value ->
97+ SIX_QUAT_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .CondStatus_10_SixDeg_Quat )) { pigeon, value ->
10198 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .CondStatus_10_SixDeg_Quat , value, timeout)
10299 }
103- PigeonParameter . Enum . MAG_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .RawStatus_4_Mag )){ pigeon, value ->
100+ MAG_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .RawStatus_4_Mag )) { pigeon, value ->
104101 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .RawStatus_4_Mag , value, timeout)
105102 }
106- PigeonParameter . Enum . BIAS_GYRO_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .BiasedStatus_2_Gyro )){ pigeon, value ->
103+ BIAS_GYRO_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .BiasedStatus_2_Gyro )) { pigeon, value ->
107104 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .BiasedStatus_2_Gyro , value, timeout)
108105 }
109- PigeonParameter . Enum . BIAS_MAG_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .BiasedStatus_4_Mag )){ pigeon, value ->
106+ BIAS_MAG_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .BiasedStatus_4_Mag )) { pigeon, value ->
110107 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .BiasedStatus_4_Mag , value, timeout)
111108 }
112- PigeonParameter . Enum . BIAS_ACCEL_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .BiasedStatus_6_Accel )){ pigeon, value ->
109+ BIAS_ACCEL_STATUS -> configIntParam(defaultFor(PigeonIMU_StatusFrame .BiasedStatus_6_Accel )) { pigeon, value ->
113110 pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame .BiasedStatus_6_Accel , value, timeout)
114111 }
115- PigeonParameter . Enum . FACTORY_DEFAULT -> configBooleanParam(false ){ pigeon, value ->
116- if (value) pigeon.configFactoryDefault(timeout)
112+ FACTORY_DEFAULT -> configBooleanParam(false ) { pigeon, value ->
113+ if (value) pigeon.configFactoryDefault(timeout)
117114 }
118- else -> TODO (" ${param.enum} not implemented" )
119115 }
120116 return super .execute()
121117 }
0 commit comments