75 lines
1.6 KiB
Python
75 lines
1.6 KiB
Python
import math
|
|
from phoenix6.hardware.pigeon2 import Pigeon2
|
|
import navx
|
|
|
|
|
|
class Pigeon:
|
|
gyro: Pigeon2
|
|
|
|
def setup(self):
|
|
self.gyro.reset()
|
|
|
|
def execute(self):
|
|
pass
|
|
|
|
def yaw(self) -> float:
|
|
return self.gyro.get_yaw().value
|
|
|
|
def roll(self) -> float:
|
|
return self.gyro.get_roll().value
|
|
|
|
def pitch(self) -> float:
|
|
return self.gyro.get_pitch().value
|
|
|
|
def rotation2d(self):
|
|
return self.gyro.getRotation2d()
|
|
|
|
def degrees_per_second_ccw(self) -> float:
|
|
return self.gyro.get_angular_velocity_z_world().value
|
|
|
|
def radians_per_second_ccw(self) -> float:
|
|
return math.radians(self.degrees_per_second_ccw())
|
|
|
|
def set_yaw(self, angle):
|
|
self.gyro.set_yaw(angle)
|
|
|
|
|
|
class NavX:
|
|
gyro: navx.AHRS # Either navx.AHRS.create_spi() or navx.AHRS.create_i2c()
|
|
|
|
def setup(self):
|
|
self.gyro.reset()
|
|
|
|
def execute(self):
|
|
pass
|
|
|
|
def yaw(self) -> float:
|
|
return self.gyro.getYaw()
|
|
|
|
def roll(self) -> float:
|
|
return self.gyro.getRoll()
|
|
|
|
def pitch(self) -> float:
|
|
return self.gyro.getPitch()
|
|
|
|
def is_calibrating(self) -> bool:
|
|
return self.gyro.isCalibrating()
|
|
|
|
def is_connected(self) -> bool:
|
|
return self.gyro.isConnected()
|
|
|
|
def rotation2d(self):
|
|
return self.gyro.getRotation2d()
|
|
|
|
def rotation3d(self):
|
|
return self.gyro.getRotation3d()
|
|
|
|
def reset(self) -> None:
|
|
self.gyro.reset()
|
|
|
|
def fused_heading(self) -> float:
|
|
return self.gyro.getFusedHeading()
|
|
|
|
def zero_yaw(self) -> None:
|
|
self.gyro.zeroYaw()
|