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
