JackM323
b537677277
Robot into its own Class instead of lose Global Variables that cause circular imports StateClass usage instead of the old RobotState.py New Input Class for Controller and randome intputs
67 lines
1.9 KiB
Python
67 lines
1.9 KiB
Python
"""
|
|
main.py - Entry point using object-oriented Robot and Input abstractions
|
|
"""
|
|
from threading import Thread
|
|
from multiprocessing import Queue
|
|
import time
|
|
|
|
import config as cfg
|
|
from Robot import Robot, HardwareBackend, PyBulletBackend
|
|
from simulation import Simulation
|
|
from EspCommunication import ESP32Communication
|
|
from ArduinoCommunication import ArduinoCommunication
|
|
from inputs.PygameController import controller_loop
|
|
|
|
|
|
def create_robot_instance() -> Robot:
|
|
if cfg.sim:
|
|
sim_instance = Simulation()
|
|
backend = PyBulletBackend(sim_instance)
|
|
else:
|
|
comm = ArduinoCommunication() if cfg.arduinoConnection else ESP32Communication()
|
|
comm.start()
|
|
backend = HardwareBackend(comm)
|
|
|
|
return Robot(backend=backend)
|
|
|
|
|
|
def robot_control_loop(robot: Robot, control_queue: Queue):
|
|
robot.reset_to_init()
|
|
time.sleep(1)
|
|
|
|
tick = 0.05
|
|
next_time = time.time()
|
|
|
|
while True:
|
|
next_time += tick
|
|
|
|
# Drain the queue to get the latest command frame
|
|
latest_cmd = None
|
|
while not control_queue.empty():
|
|
latest_cmd = control_queue.get_nowait()
|
|
|
|
if latest_cmd is not None:
|
|
robot.robot_state = latest_cmd["state"]
|
|
robot.vector_dirmov = latest_cmd["dirmov"]
|
|
|
|
# Run state machine tick
|
|
robot.tick()
|
|
|
|
sleep_time = next_time - time.time()
|
|
if sleep_time > 0:
|
|
time.sleep(sleep_time)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
controller_queue = Queue()
|
|
|
|
my_robot = create_robot_instance()
|
|
|
|
# Start Pygame input thread
|
|
input_thread = Thread(target=controller_loop, args=(controller_queue,), daemon=True)
|
|
robot_thread = Thread(target=robot_control_loop, args=(my_robot, controller_queue), daemon=True)
|
|
|
|
input_thread.start()
|
|
robot_thread.start()
|
|
|
|
robot_thread.join() |