# Import modules from pybricks.hubs import PrimeHub from pybricks.iodevices import PUPDevice from pybricks.parameters import Button, Color, Direction, Port, Side, Stop from pybricks.pupdevices import ColorSensor, ForceSensor, Motor, UltrasonicSensor from pybricks.robotics import DriveBase from pybricks.tools import multitask, run_task, StopWatch, wait from umath import cos, radians from urandom import randint # Initialise program constants SERVER = "http://192.168.0.129:5000" DEVICE_NAME = "TOI" # <--- Change device name in 3 char according to list below # Define channel map for BLE broadcast CHANNEL_MAP = { "JKG": 1, # Jurassic Kingdom gate "TOI": 2, # Toilet "MOF": 3, # Moses & feeding "HAB": 4, # Hamster ball "VOL": 5, # Volcano "AAP": 6, # Aaron & Pedro } POLL_INTERVAL = 50 # Delay to poll server in ms HEARTBEAT_INTERVAL = 2.0 # Delay to send heartbeat in s # Explosion constants SPEED_MOTOR = 720 ANGLE_EXPLODE_FRONT = 180 ANGLE_EXPLODE_BACK = -180 # US thresholds DIST_TRIGGER = 250 # first detection DIST_CONFIRM = 220 # second confirm # --------------- # BLE COMMS class # --------------- class BLEComms: # Initialise self def __init__(self, hub, device_name, broadcast_channel, observe_channels): print("[BLECOMMS] Initializing") self.hub = hub self.device_name = device_name self.broadcast_channel = broadcast_channel self.observe_channels = observe_channels # Send event def broadcast(self, kind, value): # Populate message msg = self.device_name + ":" + kind + ":" + value payload = msg[:21] # BLE advertisement limit of 21 char print("[BLE] Sending payload '", payload, "'") # Send 3 times for reliability for i in range(3): self.hub.ble.broadcast(payload) wait(POLL_INTERVAL) # Receiving event def observe(self): for ch in self.observe_channels: msg = self.hub.ble.observe(ch) if msg: print("[BLE] Receiving message '", msg, "'") return msg return None # -------------- # Hardware class # -------------- class Hardware: # Initialise self def __init__(self): # <--- Add motors and sensors as required print("[HARDWARE] Initializing") # Motors self.motor_front = Motor(Port.A) #self.motor_back = Motor(Port.B) self.motor_back = Motor(Port.C) # Sensors self.sensor_us_back = UltrasonicSensor(Port.E) # Backup ultra-sonic sensor facing back self.sensor_us = UltrasonicSensor(Port.F) # Main ultra-sonic sensor facing front # Motor list for reset() method #self.list_motors = [self.motor_front,self.motor_back,'','','',''] # Port A-F self.list_motors = [self.motor_front,'',self.motor_back,'','',''] # Port A-F # Example action def do_action(self): # <--- Add motors and sensors as required self.motor_front.run_angle(500, 90) self.motor_back.run_angle(500, -90) def stop(self): # <--- Add motors and sensors as required self.motor_front.stop() self.motor_back.stop() # ------------ # Device class # ------------ class Device: # Initialise self def __init__(self, name): self.device_name = name broadcast_channel = CHANNEL_MAP[name] # Initialise array for all channels except own observe_channels = [ ch for dev, ch in CHANNEL_MAP.items() if dev != name ] self.hub = PrimeHub( broadcast_channel=broadcast_channel, observe_channels=observe_channels # BLE observe on all channels except own ) self.ble = BLEComms( self.hub, name, broadcast_channel, observe_channels ) self.hw = Hardware() self.clock = StopWatch() self.last_heartbeat = 0 self.exploded = False self.waiting_for_confirm = False self.confirm_start_time = 0 def explode(self): self.ble.broadcast("CMD", "JKG_shhi") print("[TOILET] EXPLOSION triggered!") self.hw.motor_front.run_angle(SPEED_MOTOR, ANGLE_EXPLODE_FRONT) self.hw.motor_back.run_angle(SPEED_MOTOR, ANGLE_EXPLODE_BACK) self.exploded = True self.ble.broadcast("CMD", "TOI_stop") wait(3000) self.ble.broadcast("CMD", "JKG_wait") # Send status 'alive' to Flask server def heartbeat(self): self.ble.broadcast("STATUS", "alive") # Define functions run periodically def periodic(self): # <--- Add required code # If already exploded, do nothing if self.exploded: return dist = self.hw.sensor_us.distance() if dist is not None: print("[US] Distance:", dist) self.ble.broadcast("CMD", "TOI_open") self.ble.broadcast("CMD", "MOF_start") self.ble.broadcast("CMD", "JKG_shlo") # First detection if not self.waiting_for_confirm and dist < DIST_TRIGGER: print("[TOILET] First detection - Waiting for confirm ...") self.waiting_for_confirm = True self.confirm_start_time = self.clock.time() # Confirm detection if self.waiting_for_confirm: now = self.clock.time() if dist < DIST_CONFIRM: print("[TOILET] Confirmed — EXPLODING!") self.explode() self.waiting_for_confirm = False #self.ble.broadcast("EVENT", "TOI_open") #self.ble.broadcast("CMD", "TOI_open") elif now - self.confirm_start_time > 1000: print("[TOILET] Confirm timeout — Cancelling ...") self.waiting_for_confirm = False # Backup-confirm US detection dist_back = self.hw.sensor_us_back.distance() if dist_back is not None: print("[US BACK] Distance:", dist_back) self.ble.broadcast("CMD", "TOI_open") self.ble.broadcast("CMD", "MOF_start") self.ble.broadcast("CMD", "JKG_shlo") # First detection if not self.waiting_for_confirm and dist_back < DIST_TRIGGER: print("[TOILET] First back detection - Waiting for confirm ...") self.waiting_for_confirm = True self.confirm_start_time = self.clock.time() # Confirm detection if self.waiting_for_confirm: now_back = self.clock.time() if dist_back < DIST_CONFIRM: print("[TOILET] Confirmed — EXPLODING!") self.explode() self.waiting_for_confirm = False #self.ble.broadcast("EVENT", "TOI_open") #self.ble.broadcast("STATUS", "exploding") elif now_back - self.confirm_start_time > 1000: print("[TOILET] Confirm timeout — Cancelling ...") self.waiting_for_confirm = False # Handle BLE message handler def on_ble_message(self, msg): try: src, kind, value = msg.split(":", 2) except: return # Ignore empty or own broadcasts if not src or src == self.device_name: return print(f"[EVENT] Receiving message from device {src}: {kind}:{value}") if "_" in value: device, action = value.split("_", 1) else: device = value action = None if device == self.device_name and kind == "CMD": # <--- Add required commands & return messages if action == "start": self.ble.broadcast("EVENT", "start") elif action == "stop": motors_stop = [0,0,"","","",""] # Port A-F self.reset(motors_stop) self.exploded = False self.waiting_for_confirm = False self.ble.broadcast("EVENT", "stop") # Example: Gate tells Fossil Dig to start #if action == "start": # self.hw.do_action() # self.ble.broadcast("EVENT", "digging") # Reset Toilet motors def reset(self, motors): # Initialize counter counter_motor = 0 # Loop through motors array for angle in motors: # If a value is found in array if angle != '': print('Resetting motor', self.hw.list_motors[counter_motor], 'at', angle, 'degrees ...') # Look up motor, reset position to angle self.hw.list_motors[counter_motor].run_target(SPEED_MOTOR, angle) # Increment counter counter_motor += 1 # Send event 'ready' to Flask server def startup(self): print("[DEVICE] Starting up") self.ble.broadcast("EVENT", "ready") # Define main function def run(self): # Reset Toilet motors, set angle for all active motors #motors_start = [0,0,'','','',''] # Port A-F motors_start = [0,'',0,'','',''] # Port A-F # Send event self.startup() self.reset(motors_start) while True: # Send status now = self.clock.time() if now - self.last_heartbeat > HEARTBEAT_INTERVAL: self.heartbeat() self.last_heartbeat = now # Listen for BLE broadcast msg = self.ble.observe() if msg: self.on_ble_message(msg) self.periodic() wait(POLL_INTERVAL) # Declare main program if __name__ == "__main__": device = Device( name=DEVICE_NAME ) device.run()