def main(): # start the logger logger = logging.getLogger(__name__) handler = RotatingFileHandler('robot_log.log', "a", maxBytes=960000, backupCount=5) logger.addHandler(handler) # check for a default config file if os.path.isfile( "settings.default.json") and not os.path.isfile("settings.json"): open("settings.json", "a").close() copyfile("settings.default.json", "settings.json") # read the file with open("settings.json", "r") as f: values = jsonpickle.loads(f.read()) # get the results and save them global robot_type, m_settings, d_settings, state robot_type = values["type"] m_settings = values[robot_type] d_settings = values["drive"] # setup the state object if is_gripper(): state = GripperState(m_settings["lift_min"], m_settings["grip_min"]) # initalize i2c and piconzero piconzero.init() # Open the socket and start the listener thread netwk_mgr = NetworkManager(logger) netwk_mgr.start() # Make robot stuff robot_disabled = True watchdog = Watchdog(logger) if is_elevator(): piconzero.set_output_config(m_settings["motor_channel"], 1) # set channel 0 to PWM mode if is_gripper(): piconzero.set_output_config(m_settings["lift_servo"], 2) piconzero.set_output_config(m_settings["grip_servo"], 2) # set channel 0 and 1 to Servo mode # Initialization should be done now, start accepting packets while True: try: raw_pack = netwk_mgr.get_next_packet() if raw_pack is not None: try: pack = jsonpickle.decode( raw_pack ) # recieve packets, decode them, then de-json them except JSONDecodeError as e: print(e) logger.warning(str(e)) continue watchdog.reset() # Type-check the data if type(pack) is not Packet: print("pack is not a Packet", file=sys.stderr) continue # Process the packet if pack.type == PacketType.STATUS: # Check the contents of the packet if type(pack.data) is RobotStateData: if pack.data == RobotStateData.ENABLE: robot_disabled = False # Reinitialize the picon zero piconzero.init() if is_elevator(): piconzero.set_output_config( m_settings["motor_channel"], 1) # set channel 0 to PWM mode if is_gripper(): piconzero.set_output_config( m_settings["lift_servo"], 2) piconzero.set_output_config( m_settings["grip_servo"], 2) # set channel 0 and 1 to Servo mode continue elif pack.data == RobotStateData.DISABLE: robot_disabled = True piconzero.cleanup() continue elif pack.data == RobotStateData.E_STOP: piconzero.cleanup() break elif pack.type == PacketType.REQUEST: # Check for the request type if pack.data == RequestData.STATUS: # Send a response packet = Packet( PacketType. RESPONSE, # generate a packet saying if the robot is enabled or disabled RobotStateData.DISABLE if robot_disabled else RobotStateData.ENABLE) netwk_mgr.send_packet(jsonpickle.encode(packet)) elif pack.type == PacketType.RESPONSE: # do more stuff continue elif pack.type == PacketType.DATA: # See if the robot is disabled if robot_disabled: continue # Check and see if a list of packets was sent if type(pack.data) is list: for item in pack.data: process_data(item) else: process_data(pack) except Exception as e: logger.error(e, exc_info=True) pass # Emergency Stopped loop while True: # Disable all outputs piconzero.cleanup() # Accept a packet raw_pack = netwk_mgr.get_next_packet() if raw_pack is not None: pack = jsonpickle.decode( raw_pack) # receive packets, decode them, then de-json them # Check for a request if pack.type == PacketType.REQUEST: # Send a response, no matter the request type packet = Packet( PacketType.RESPONSE, RobotStateData.E_STOP ) # generate a packet saying that this robot is e-stopped netwk_mgr.send_packet(packet) time.sleep( .250 ) # delay for 250ms, don't want to spam the picon zero with cleanup requests pass