class Robot(object): cycle_time = 0.01 enabled = False odometry = Odometry(cycle_time) def __init__(self, gui): self.gui = gui self.drive = Drive(self, gui) def initialize(self): sensors.initSensors() def startLoop(self): if (not self.enabled): self.gui.log_message("Starting Main Robot Loop") self.enabled = True self.initialize() self.drive.initDrive() self.odometry.reset() self.drive.setEnableIntersection(self.gui.getIntersectionEnabled()) self.drive.setEnableEntering(self.gui.getEnteringEnabled()) self.main_thread = threading.Thread(target=self.loop) self.main_thread.start() def stopLoop(self): if (self.enabled): self.gui.log_message("Stopping Main Robot Loop") self.enabled = False def loop(self): loop_counter = 0 self.current = RigidTransform2d(Translation2d(0, 0), Rotation2d.fromDegrees(0)) while (self.enabled): loop_counter += 1 #self.gui.log_message("robot main") # Able to decrease sensor update frequency if (loop_counter % 1 == 0): sensors.updateSensors() self.odometry.updateOdometry() self.drive.updateDrive() if (loop_counter % 1 == 0): self.current = self.odometry.getFieldToVehicle() self.gui.log_pos(self.current) self.gui.log_sonics(sensors.getLeftWallDistance(), sensors.getForwardWallDistance(), sensors.getRightWallDistance()) self.gui.log_mag(sensors.getMagneticMagnitude()) self.gui.log_ir(0.0, 0.0) self.gui.log_state(self.drive.state) if (loop_counter >= 1000): loop_counter = 0 time.sleep(self.cycle_time) sensors.shutdown()