def initialize(self):
self.kp = Constants.TURN_TO_ANGLE_KP
self.ki = Constants.TURN_TO_ANGLE_KI
self.kd = Constants.TURN_TO_ANGLE_KD
self.kf = Constants.TURN_TO_ANGLE_KF
self.error_tolerance = units.degreesToRadians(
Constants.TURN_TO_ANGLE_TOLERANCE)
self.timeout = Constants.TURN_TO_ANGLE_TIMEOUT
if self.relative:
self.setpoint += units.degreesToRadians(self.odemetry.getAngle())
self.controller = pidf.PIDF(self.setpoint, self.kp, self.ki, self.kd,
self.kf, True, -180, 180)
self.timer.reset()
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 computeCoefficients(self):
"""Compute the coefficients of the curve equations. This must be called inorder to make interpolations."""
scale = 2 * math.hypot(self.end.pos.x - self.start.pos.x,
self.end.pos.y - self.start.pos.y)
self.dx0 = scale * math.cos(units.degreesToRadians(-self.start.angle))
self.dx1 = scale * math.cos(units.degreesToRadians(-self.end.angle))
self.ax = self.dx0 + self.dx1 + 2 * self.start.pos.x - 2 * self.end.pos.x
self.bx = -2 * self.dx0 - self.dx1 - 3 * self.start.pos.x + 3 * self.end.pos.x
self.cx = self.dx0
self.dx = self.start.pos.x
self.dy0 = scale * math.sin(units.degreesToRadians(-self.start.angle))
self.dy1 = scale * math.sin(units.degreesToRadians(-self.end.angle))
self.ay = self.dy0 + self.dy1 + 2 * self.start.pos.y - 2 * self.end.pos.y
self.by = -2 * self.dy0 - self.dy1 - 3 * self.start.pos.y + 3 * self.end.pos.y
self.cy = self.dy0
self.dy = self.start.pos.y
def __init__(self, setpoint, relative=False):
super().__init__()
self.drive = drive.Drive()
self.odemetry = odemetry.Odemetry()
self.requires(self.drive)
self.setpoint = units.degreesToRadians(setpoint)
self.relative = relative
self.timer = wpilib.Timer()
self.timestamp = 0
self.last_timestamp = 0
self.cur_error = 0
def __init__(self, setpoint, relative=False):
super().__init__()
self.drive = drive.Drive()
self.odemetry = odemetry.Odemetry()
self.requires(self.drive)
self.setpoint = units.degreesToRadians(setpoint)
self.kp = Constants.TURN_TO_ANGLE_KP
self.ki = Constants.TURN_TO_ANGLE_KI
self.kd = Constants.TURN_TO_ANGLE_KD
self.error_tolerance = Constants.TURN_TO_ANGLE_TOLERANCE
self.timeout = Constants.TURN_TO_ANGLE_TIMEOUT
self.controller = pid.PID(
self.setpoint, self.kp, self.ki, self.kd, True, -180, 180)
self.timestamp = 0
self.last_timestamp = 0
self.cur_error = 0
self.relative = relative
self.timer = wpilib.Timer()
self.timeout = 1000
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])
def initialize(self):
if self.relative:
self.setpoint += units.degreesToRadians(self.odemetry.getAngle())
self.controller.setpoint = self.setpoint