Skip to content

Commit 1dc838d

Browse files
mwitcpalekmwitcpalek
committed
add pigeon2 options
1 parent 3eb3f4b commit 1dc838d

File tree

4 files changed

+174
-6
lines changed

4 files changed

+174
-6
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ plugins {
1111
}
1212

1313
group = "org.strykeforce"
14-
version = "24.0.5"
14+
version = "24.0.6-SNAPSHOT"
1515

1616
sourceCompatibility = JavaVersion.VERSION_17
1717
targetCompatibility = JavaVersion.VERSION_17

‎src/main/java/org/strykeforce/gyro/SF_PIGEON2.java‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
package org.strykeforce.gyro;
2+
3+
import com.ctre.phoenix6.configs.Pigeon2Configuration;
4+
import com.ctre.phoenix6.configs.Pigeon2Configurator;
5+
import com.ctre.phoenix6.hardware.Pigeon2;
6+
import edu.wpi.first.math.geometry.Rotation2d;
7+
import org.strykeforce.telemetry.TelemetryService;
8+
9+
public class SF_PIGEON2 implements Gyro {
10+
private Pigeon2 pigeon2;
11+
private Pigeon2Configurator configurator;
12+
13+
public SF_PIGEON2(int deviceId) {
14+
pigeon2 = new Pigeon2(deviceId);
15+
configurator = pigeon2.getConfigurator();
16+
}
17+
18+
public SF_PIGEON2(int deviceId, String canbus) {
19+
pigeon2 = new Pigeon2(deviceId, canbus);
20+
configurator = pigeon2.getConfigurator();
21+
}
22+
23+
@Override
24+
public void reset() {
25+
pigeon2.reset();
26+
}
27+
28+
@Override
29+
public double getAngle() {
30+
return pigeon2.getAngle();
31+
}
32+
33+
@Override
34+
public double getRate() {
35+
return pigeon2.getRate();
36+
}
37+
38+
@Override
39+
public Rotation2d getRotation2d() {
40+
return pigeon2.getRotation2d();
41+
}
42+
43+
public Pigeon2 getPigeon2() {
44+
return pigeon2;
45+
}
46+
47+
public void applyConfig(Pigeon2Configuration config) {
48+
configurator.apply(config);
49+
}
50+
51+
public Double getRoll() {
52+
return pigeon2.getRoll().getValue();
53+
}
54+
55+
public Double getPitch() {
56+
return pigeon2.getPitch().getValue();
57+
}
58+
59+
public Double getYaw() {
60+
return pigeon2.getYaw().getValue();
61+
}
62+
63+
public void registerWith(TelemetryService telemetryService) {
64+
telemetryService.register(pigeon2);
65+
}
66+
}

‎src/main/kotlin/org/strykeforce/telemetry/TelemetryService.kt‎

Lines changed: 13 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,12 +5,10 @@ package org.strykeforce.telemetry
55
import com.ctre.phoenix.motorcontrol.can.BaseTalon
66
import com.ctre.phoenix.motorcontrol.can.TalonSRX
77
import com.ctre.phoenix6.hardware.CANcoder
8+
import com.ctre.phoenix6.hardware.Pigeon2
89
import com.ctre.phoenix6.hardware.TalonFX
910
import mu.KotlinLogging
10-
import org.strykeforce.telemetry.measurable.BaseTalonMeasurable
11-
import org.strykeforce.telemetry.measurable.CancoderMeasureable
12-
import org.strykeforce.telemetry.measurable.Measurable
13-
import org.strykeforce.telemetry.measurable.TalonSRXMeasurable
11+
import org.strykeforce.telemetry.measurable.*
1412
import org.strykeforce.telemetry.talon.TalonFXFaultMeasureable
1513
import org.strykeforce.telemetry.talon.TalonFXMeasureable
1614
import org.strykeforce.thirdcoast.talon.LegacyTalonFXMeasurable
@@ -145,7 +143,7 @@ class TelemetryService(private val telemetryControllerFactory: Function<Inventor
145143
}
146144

147145
/**
148-
* Convenience method to register faults for a [com.ctre.phoenix6.hardware.CANcoder] for telemetry sending
146+
* Convenience method to register a [com.ctre.phoenix6.hardware.CANcoder] for telemetry sending
149147
*
150148
* @param cancoder the CANcoder to register for data collection
151149
* @throws IllegalStateException if TelemetryService is running
@@ -154,6 +152,16 @@ class TelemetryService(private val telemetryControllerFactory: Function<Inventor
154152
register(CancoderMeasureable(cancoder))
155153
}
156154

155+
/**
156+
* Convenience method to register a [com.ctre.phoenix6.hardware.Pigeon2] for telemetry sending
157+
*
158+
* @param pigeon2 the Pigeon2 to register for data collection
159+
* @throws IllegalStateException if TelemetryService is running
160+
*/
161+
fun register(pigeon2: Pigeon2) {
162+
register(Pigeon2Measureable(pigeon2))
163+
}
164+
157165
/**
158166
* Get an unmodifiable view of the registered items.
159167
*

‎src/main/kotlin/org/strykeforce/telemetry/measurable/Pigeon2Measureable.kt‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
package org.strykeforce.telemetry.measurable
2+
3+
import com.ctre.phoenix6.hardware.Pigeon2
4+
import jdk.jfr.Description
5+
6+
//internal const val ROLL = "ROLL"
7+
//internal const val PITCH = "PITCH"
8+
//internal const val YAW = "YAW"
9+
internal const val QUATW = "QUAT_W"
10+
internal const val QUATX = "QUAT_X"
11+
internal const val QUATY = "QUAT_Y"
12+
internal const val QUATZ = "QUAT_Z"
13+
internal const val GRAV_X = "GRAV_X"
14+
internal const val GRAV_Y = "GRAV_Y"
15+
internal const val GRAV_Z = "GRAV_Z"
16+
internal const val TEMP = "TEMP"
17+
internal const val NO_MOT_EN = "NO_MOT_EN"
18+
internal const val NO_MOT_CNT = "NO_MOT_CNT"
19+
internal const val UP_TIME = "UP_TIME"
20+
internal const val ACCUM_X = "ACCUM_X"
21+
internal const val ACCUM_Y = "ACCUM_Y"
22+
internal const val ACCUM_Z = "ACCUM_Z"
23+
internal const val ANG_VEL_X = "ANG_VEL_X"
24+
internal const val ANG_VEL_Y = "ANG_VEL_Y"
25+
internal const val ANG_VEL_Z = "ANG_VEL_Z"
26+
internal const val ACCEL_X = "ACCEL_X"
27+
internal const val ACCEL_Y = "ACCEL_Y"
28+
internal const val ACCEL_Z = "ACCEL_Z"
29+
internal const val SUPPLY_V = "SUPPLY_V"
30+
internal const val ANG_VEL_WORLD_X = "ANG_VEL_WORLD_X"
31+
internal const val ANG_VEL_WORLD_Y = "ANG_VEL_WORLD_Y"
32+
internal const val ANG_VEL_WORLD_Z = "ANG_VEL_WORLD_Z"
33+
internal const val MAG_X = "MAG_X"
34+
internal const val MAG_Y = "MAG_Y"
35+
internal const val MAG_Z = "MAG_Z"
36+
internal const val IS_PRO = "IS_PRO"
37+
internal const val RATE = "RATE"
38+
//internal const val ANGLE = "ANGLE"
39+
internal const val ROTATION2D = "ROTATION2D"
40+
internal const val ROTATION3D = "ROTATION3D"
41+
class Pigeon2Measureable @JvmOverloads constructor(
42+
private val pigeon2: Pigeon2,
43+
override val description: String = "Pigeon2 ${pigeon2.deviceID}"
44+
): Measurable {
45+
46+
override val deviceId = pigeon2.deviceID
47+
override val measures = setOf(
48+
Measure(ROLL, "Roll") {pigeon2.roll.valueAsDouble},
49+
Measure(PITCH, "Pitch") {pigeon2.pitch.valueAsDouble},
50+
Measure(YAW, "Yaw") {pigeon2.yaw.valueAsDouble},
51+
Measure(QUATW, "Quat W") {pigeon2.quatW.valueAsDouble},
52+
Measure(QUATX, "Quat X") {pigeon2.quatX.valueAsDouble},
53+
Measure(QUATY, "Quat Y") {pigeon2.quatY.valueAsDouble},
54+
Measure(QUATZ, "Quat Z") {pigeon2.quatZ.valueAsDouble},
55+
Measure(GRAV_X, "Gravity X") {pigeon2.gravityVectorX.valueAsDouble},
56+
Measure(GRAV_Y, "Gravity Y") { pigeon2.gravityVectorY.valueAsDouble},
57+
Measure(GRAV_Z, "Gravity Z") { pigeon2.gravityVectorZ.valueAsDouble},
58+
Measure(TEMP, "Temperature") { pigeon2.temperature.valueAsDouble},
59+
Measure(NO_MOT_EN, "No Motion Enabled") { pigeon2.noMotionEnabled.valueAsDouble},
60+
Measure(NO_MOT_CNT, "No Motion Count") { pigeon2.noMotionCount.valueAsDouble},
61+
Measure(UP_TIME, "Up Time") { pigeon2.upTime.valueAsDouble},
62+
Measure(ACCUM_X, "Accumulator X") { pigeon2.accumGyroX.valueAsDouble},
63+
Measure(ACCUM_Y, "Accumulator Y") {pigeon2.accumGyroY.valueAsDouble},
64+
Measure(ACCUM_Z, "Accumulator Z") { pigeon2.accumGyroZ.valueAsDouble},
65+
Measure(ANG_VEL_X, "Angular Vel X") {pigeon2.angularVelocityXDevice.valueAsDouble},
66+
Measure(ANG_VEL_Y, "Angular Vel Y") {pigeon2.angularVelocityYDevice.valueAsDouble},
67+
Measure(ANG_VEL_Z, "Angular Vel Z") {pigeon2.angularVelocityZDevice.valueAsDouble},
68+
Measure(ACCEL_X, "Acceleration X") {pigeon2.accelerationX.valueAsDouble},
69+
Measure(ACCEL_Y, "Acceleration Y") {pigeon2.accelerationY.valueAsDouble},
70+
Measure(ACCEL_Z, "Acceleration Z") {pigeon2.accelerationZ.valueAsDouble},
71+
Measure(SUPPLY_V, "Supply Voltage") {pigeon2.supplyVoltage.valueAsDouble},
72+
Measure(ANG_VEL_WORLD_X, "Angular Vel World X") {pigeon2.angularVelocityXWorld.valueAsDouble},
73+
Measure(ANG_VEL_WORLD_Y, "Angular Vel World Y") {pigeon2.angularVelocityYWorld.valueAsDouble},
74+
Measure(ANG_VEL_WORLD_Z, "Angular Vel World Z") {pigeon2.angularVelocityZWorld.valueAsDouble},
75+
Measure(MAG_X, "Mag Field X") {pigeon2.magneticFieldX.valueAsDouble},
76+
Measure(MAG_Y, "Mag Field Y") {pigeon2.magneticFieldY.valueAsDouble},
77+
Measure(MAG_Z, "Mag Field Z") {pigeon2.magneticFieldZ.valueAsDouble},
78+
Measure(IS_PRO, "Is Pro Lic") {pigeon2.isProLicensed.valueAsDouble},
79+
Measure(RATE, "Rate") {pigeon2.rate},
80+
Measure(ANGLE, "Angle") {pigeon2.angle},
81+
Measure(ROTATION2D, "Rotation2d Deg") {pigeon2.rotation2d.degrees}
82+
)
83+
84+
override fun equals(other: Any?): Boolean {
85+
if(this === other) return true
86+
if(javaClass != other?.javaClass) return false
87+
88+
other as Pigeon2
89+
if(deviceId != other.deviceID) return false
90+
return true
91+
}
92+
93+
override fun hashCode() = deviceId
94+
}

0 commit comments

Comments
 (0)