def read_config(self):
try:
self.injector.write("config_package\n\r".encode("ascii"))
self.sleep()
self.sleep()
config_string = self.injector.read(size=1024).decode('utf-8')
except Exception as e:
self.logerror("No config found, %s", e)
self.injector.close()
self.injector = None
return
try:
m = re.search('\!\@\#\$(.+?)\!\@\#\$', config_string)
if m:
config_string = m.group(1)
self.loginfo('String found: %s', config_string)
config = json.loads(config_string)
except Exception as e:
self.loginfo("String found: %s", str(config_string))
self.logerror("Could not load settings package: %s", e)
self.injector.close()
self.injector = None
sleep(3)
return
if config:
for k, v in config.items():
ConfigManager.set_value('game|global|%s' % k, v)
def run(self):
logger.info("REMOTERF THREAD STARTED")
string = ""
while True:
field = ConfigManager.get_value("game|global|field_id")
ack_packet = "a{}{}ACK------".format(
ConfigManager.get_value("game|global|field_id"),
ConfigManager.get_value("game|global|robot_id"),
)
ack_packet = ack_packet.encode()
c = self.ser.read(11)
# ---aAXSTART -
self.ser.write(ack_packet)
try:
c = c.decode()
except:
continue
if not c:
continue
string += c
if f"{field}XSTART" in string:
logger.info("GOT START")
string = ""
ConfigManager.set_value("game|global|gameplay status",
'enabled')
if f"{field}XSTOP" in string:
logger.info("GOT STOP")
string = ""
ConfigManager.set_value("game|global|gameplay status",
'disabled')
if "PING" in string:
logger.info("got ping")
string = ""
self.ser.write(ack_packet)
if len(string) > 1000:
string = ""
def send_settings_packet():
game_config = ConfigManager.get_value('game')
if not game_config:
ConfigManager.set_value('game|global|field_id', 'A')
ConfigManager.set_value('game|global|robot_id', 'A')
ConfigManager.set_value('game|global|target goal color', 'blue')
ConfigManager.set_value('game|global|gameplay status', 'disabled')
game_options = [
("field_id", [ConfigManager.get_value("game|global|field_id"), "A", "B", "Z"]),
("robot_id", [ConfigManager.get_value("game|global|robot_id"), "A", "B"]),
("target goal color", [ConfigManager.get_value("game|global|target goal color"), "purple", "blue"]),
("gameplay status", [ConfigManager.get_value("game|global|gameplay status"), "disabled", "enabled"]),
]
settings_packet = json.dumps(dict(
action="settings-packet",
sliders=dict(color=ConfigManager.get_value("color")),
options=game_options
))
websocket.send(settings_packet)
def command(websocket):
# send logging history to ws
for buf in list(websocket_log_handler.queue):
websocket.send(buf)
def send_settings_packet():
game_config = ConfigManager.get_value('game')
if not game_config:
ConfigManager.set_value('game|global|field_id', 'A')
ConfigManager.set_value('game|global|robot_id', 'A')
ConfigManager.set_value('game|global|target goal color', 'blue')
ConfigManager.set_value('game|global|gameplay status', 'disabled')
game_options = [
("field_id", [ConfigManager.get_value("game|global|field_id"), "A", "B", "Z"]),
("robot_id", [ConfigManager.get_value("game|global|robot_id"), "A", "B"]),
("target goal color", [ConfigManager.get_value("game|global|target goal color"), "purple", "blue"]),
("gameplay status", [ConfigManager.get_value("game|global|gameplay status"), "disabled", "enabled"]),
]
settings_packet = json.dumps(dict(
action="settings-packet",
sliders=dict(color=ConfigManager.get_value("color")),
options=game_options
))
websocket.send(settings_packet)
send_settings_packet()
last_press = time()
last_press_history = []
counter = 0
rpm = 1000
while not websocket.closed:
counter += 1
websockets.add(websocket)
gevent.sleep(0.005)
msg = websocket.receive()
if not msg:
websockets.remove(websocket)
logger.error("WebSocket connection presumably closed, %d left connected" % len(websockets))
break
response = json.loads(msg)
action = response.pop("action", None)
game_package = strategy_state.package or {}
canbus_package = canbus_state.package or {}
gameplay_status = game_package.get('is_enabled', None)
target_goal_angle = round(game_package.get('target_goal_angle') or 0, 3)
canbus_rpm = canbus_package.get('last_rpm') or 0
closest_ball = game_package.get('closest_ball') or {}
dist = game_package.get('dist') or 0
if action == "gamepad":
controls = response.get("data")
# print(controls)
if controls:
toggle_gameplay = controls.get("controller0.button8", controls.get("controller0.button11", None))
if toggle_gameplay is False: # False is key up event
ConfigManager.set_value('game|global|gameplay status', 'disabled' if gameplay_status else 'enabled')
elif not gameplay_status:
# # Manual control of the robot
x = controls.get("controller0.axis0", 0) * 0.33
y = controls.get("controller0.axis1", 0) * 0.33
w = controls.get("controller0.axis3", 0) * 0.2
if controls.get("controller0.button3"):
y = 0.15
if x or y or w:
movement_publisher.publish(x=x, y=-y, az=-w)
delta = controls.get("controller0.button12", 0)
delta = delta or -controls.get("controller0.button13", 0)
if delta:
rpm = max(0, min(rpm + delta * 50, 15000))
logger.info(f"PWM+: {rpm:.0f}")
if controls.get("controller0.button0", None):
logger.info_throttle(1, f"drive_towards_target_goal: {target_goal_angle} rpm:{rpm} speed:{canbus_rpm}")
kicker_publisher.publish(rpm)
# no driving backwards when angle error
command_publisher.command(drive_towards_target_goal=dict(backtrack=False, speed_factor=0.8))
if controls.get("controller0.button5", None):
logger.info_throttle(0.3, f"kick: {target_goal_angle} rpm:{rpm} speed:{canbus_rpm}")
kicker_publisher.publish(rpm)
if controls.get("controller0.button6", None):
logger.info_throttle(1, "Drive to center")
command_publisher.command(drive_to_field_center=None)
if controls.get("controller0.button7", None):
logger.info_throttle(1, f"Flank {target_goal_angle} {closest_ball.get('dist')} {closest_ball.get('angle_deg')}")
command_publisher.command(flank=None)
if controls.get("controller0.button4", None):
logger.info_throttle(1, f"GmaeShoot {target_goal_angle:.1f} {dist:.0f} {canbus_rpm:.0f}")
command_publisher.command(kick=None)
if controls.get("controller0.button2", None):
logger.info_throttle(1, f"align_to_goal: {target_goal_angle} speed:{canbus_rpm}")
command_publisher.command(align_to_goal=dict(factor=1))
last_press_history = [*last_press_history, time() - last_press][-30:]
last_press = time()
if counter % 30 == 1:
average = sum(last_press_history) / len(last_press_history)
if not gameplay_status:
format = dict((k, v if type(v) == bool else round(v, 1)) for k, v in controls.items())
logger.info_throttle(5, f"Last press {average:.3f} ago on average, {format}")
elif action == "set_settings":
for k, v in response.items():
ConfigManager.set_value(k, v)
send_settings_packet()
elif action == "set_options":
for k, v in response.items():
ConfigManager.set_value(f"game|global|{k}", v)
send_settings_packet()
elif action == "ping":
try:
send_settings_packet()
except Exception as e:
logger.info("Socket is dead")
else:
logger.error("Unhandled action: %s", str(action))
websockets.remove(websocket)
logger.info("WebSocket connection closed, %d left connected", len(websockets))
return b""
