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))
