class TurnToAngle(Command):
def _setMotors(self, signal):
signal = signal if abs(
signal) > Constants.TURN_TO_ANGLE_MIN_OUTPUT else math.copysign(
Constants.TURN_TO_ANGLE_MIN_OUTPUT, signal)
Dash.putNumber("Turn To Angle Output", signal)
self.drive.setPercentOutput(signal, -signal, signal, -signal)
def __init__(self, setpoint):
"""Turn to setpoint (degrees)."""
super().__init__()
self.drive = drive.Drive()
self.odemetry = odemetry.Odemetry()
self.requires(self.drive)
self.setpoint = setpoint
src = self.odemetry.pidgyro
self.PID = PIDController(Constants.TURN_TO_ANGLE_KP,
Constants.TURN_TO_ANGLE_KI,
Constants.TURN_TO_ANGLE_KD, src,
self._setMotors)
logging.debug(
"Turn to angle constructed with angle {}".format(setpoint))
self.PID.setInputRange(-180.0, 180.0)
self.PID.setOutputRange(-Constants.TURN_TO_ANGLE_MAX_OUTPUT,
Constants.TURN_TO_ANGLE_MAX_OUTPUT)
self.PID.setContinuous(True)
self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE)
self.PID.setPIDSourceType(PIDController.PIDSourceType.kDisplacement)
def initialize(self):
self.PID.setP(Constants.TURN_TO_ANGLE_KP)
self.PID.setI(Constants.TURN_TO_ANGLE_KI)
self.PID.setD(Constants.TURN_TO_ANGLE_KD)
self.PID.setOutputRange(-Constants.TURN_TO_ANGLE_MAX_OUTPUT,
Constants.TURN_TO_ANGLE_MAX_OUTPUT)
self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE)
self.PID.setSetpoint(self.setpoint)
self.PID.enable()
def execute(self):
Dash.putNumber("Turn To Angle Error", self.PID.getError())
# logging.info(f"Turn To Angle Error {self.PID.getError()}")
def isFinished(self):
return self.PID.onTarget()
def end(self):
logging.debug("Finished turning to angle {}".format(self.setpoint))
self.PID.disable()
snaplistener.SnapListener(0).start()
def interrupted(self):
self.end()
class PIDVisionAlign(Segment):
def get_pid(self, sd):
return [
# float(sd.getNumber("KP", 0.553) * 4),
# float(sd.getNumber("KI", 0.033) * 4),
# float(sd.getNumber("KD", 0.073) * 4)
0.503 * 4,
0.033 * 4,
0.073 * 4
]
def config(self, robot):
self.pid = PIDController(*self.get_pid(robot.dashboard),
self.get_disalignment, self.output)
self.pid.setOutputRange(-1, 1)
self.pid.enable()
self.pid.setSetpoint(0)
self.turn = 0
self.robot = robot
def get_disalignment(self):
return self.robot.dashboard.getNumber("CENTROID", 0.5) - 0.5
def output(self, n):
self.turn = n
def free(self):
self.pid.free()
def update(self, robot):
err = self.pid.getError()
self.robot.chassis.arcade_drive(-0.6, self.turn)
print(self.turn)
print("err", err)
self.pid.setPID(*self.get_pid(robot.dashboard))
