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