def getAngle(self):
"""Use the gyroscope to return the angle in radians."""
#angles.positiveAngleToMixedAngle(abs(math.fmod(units.radiansToDegrees( ), 360)))
if hal.isSimulation():
return units.degreesToRadians(
angles.positiveAngleToMixedAngle(
angles.wrapPositiveAngle(-self.gyro.getAngle()))
) + units.degreesToRadians(Constants.GYRO_OFFSET)
#return -units.degreesToRadians(self.gyro.getAngle())
else:
return units.degreesToRadians(
angles.positiveAngleToMixedAngle(
angles.wrapPositiveAngle(-self.gyro.getYawPitchRoll()[0]))
) + units.degreesToRadians(Constants.GYRO_OFFSET)
def __init__(self):
"""Initilize the Odemetry class."""
super().__init__()
self.drive = drive.Drive()
self.timestamp = 0
self.last_timestamp = 0
self.dt = 0
# Gyroscope
if hal.isSimulation():
self.gyro = analoggyro.AnalogGyro(0)
self.pidgyro = pidanaloggyro.PIDAnalogGyro(self.gyro)
else:
self.gyro = PigeonIMU(intake.Intake().l_motor)
self.pidgyro = pidpigeon.PIDPigeon(self.gyro)
self.pose = pose.Pose()
self.last_left_encoder_distance = 0
self.last_right_encoder_distance = 0
self.last_angle = 0
def __init__(self):
"""Initilize the Odemetry class."""
super().__init__()
self.drive = drive.Drive()
self.timestamp = 0
self.last_timestamp = 0
self.dt = 0
self.ir_motor = ctre.WPI_TalonSRX(Constants.IR_MOTOR_ID)
# Gyroscope
if hal.isSimulation():
self.gyro = analoggyro.AnalogGyro(0)
else:
self.gyro = PigeonIMU(self.ir_motor)
self.calibrate()
self.pose = pose.Pose()
self.last_left_encoder_distance = 0
self.last_right_encoder_distance = 0
self.last_angle = 0
def calibrate(self):
if not hal.isSimulation():
# TODO how to calibrate pigeon
pass
def reset(self):
if hal.isSimulation():
self.gyro.reset()
else:
self.gyro.setYaw(0, 0)
self.pose = pose.Pose()
def execute(self):
self.diagnostic.update()
if not hal.isSimulation(): # TODO no output on update
self.diagnostic.log()
self.diagnostic.outputToSmartDashboard()
def getAngle(self):
"""Use the gyroscope to return the angle in radians."""
if hal.isSimulation():
return -units.degreesToRadians(self.gyro.getAngle())
else:
return -units.degreesToRadians(self.gyro.getYawPitchRoll()[0])