# Import modules from pybricks.hubs import PrimeHub #from pybricks.iodevices import PUPDevice #from pybricks.parameters import Button, Color, Direction, Port, Side, Stop from pybricks.parameters import Direction, Port #from pybricks.pupdevices import ColorSensor, ForceSensor, Motor, UltrasonicSensor from pybricks.pupdevices import Motor, UltrasonicSensor #from pybricks.robotics import DriveBase #from pybricks.tools import multitask, run_task, StopWatch, wait from pybricks.tools import StopWatch, wait #from umath import cos, radians from urandom import randint # Initialise program constants SERVER = "http://192.168.0.129:5000" DEVICE_NAME = "JKG" # <--- 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 # Angle to open gate, to turn light axle, to rotate trees, motor speed in deg/s, time to wait in ms, list of motors ANGLE_GATE = -90 ANGLE_LIGHTS = 360 ANGLE_ROTATE = 30 SPEED_MOTOR = 720 WAIT_TIME = 100 # --------------- # 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_gate_left = Motor(Port.A, Direction.COUNTERCLOCKWISE) self.motor_gate_right = Motor(Port.B, Direction.CLOCKWISE) self.motor_lights = Motor(Port.C, Direction.CLOCKWISE) #self.motor_tree_left = Motor(Port.D, Direction.CLOCKWISE) self.motor_tree_right = Motor(Port.D, Direction.CLOCKWISE) #self.motor_tree_right = Motor(Port.E, Direction.CLOCKWISE) self.motor_tree_left = Motor(Port.E, Direction.CLOCKWISE) # Sensors self.sensor_us = UltrasonicSensor(Port.F) # Motor list for reset() method #self.list_motors = [self.motor_gate_left,self.motor_gate_right,self.motor_lights,self.motor_tree_left,self.motor_tree_right,''] # Port A-F self.list_motors = [self.motor_gate_left,self.motor_gate_right,self.motor_lights,self.motor_tree_right,self.motor_tree_left,''] # Port A-F # Example action def do_action(self): # <--- Add motors and sensors as required self.motor_gate_left.run_angle(500, 90) self.motor_gate_right.run_angle(500, -90) def stop(self): # <--- Add motors and sensors as required self.motor_gate_left.stop() self.motor_gate_right.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.angle_gate = ANGLE_GATE self.counter_us = 0 self.frequency = "low" #self.wait_time = WAIT_TIME # Send status 'alive' to Flask server def heartbeat(self): self.ble.broadcast("STATUS", "alive") # Start lights at frequency of change def light_up(self, frequency): print('Turning lights on at', frequency, 'frequency ...') # If frequency low, choose high wait time in ms if frequency == 'low': wait_lights = randint(200,500) # If frequency high, choose low wait time in ms else: wait_lights = randint(100,200) # Run gate lights self.hw.motor_lights.run_angle(SPEED_MOTOR, ANGLE_LIGHTS) # Gate lights wait(wait_lights) self.hw.motor_lights.hold() # Gate lights wait(WAIT_TIME) # Shake trees at frequency of change def shake_trees(self, frequency): print('Shaking trees at', frequency, 'frequency ...') # If frequency low, choose high wait time in ms if frequency == 'low': wait_trees = randint(200,500) # If frequency high, choose low wait time in ms else: wait_trees = randint(100,200) # Rotate tree motors self.hw.motor_tree_left.run_angle(SPEED_MOTOR, ANGLE_ROTATE, wait=False) # Left tree motor self.hw.motor_tree_left.run_angle(SPEED_MOTOR, ANGLE_ROTATE) # Right tree motor wait(wait_trees) ANGLE_ROTATE *= -1 # Define functions run periodically def periodic(self): # <--- Add required code # Run lights always self.light_up(self.frequency) # Use ultrasonic TWICE to open and close gate if self.counter_us == 0: dist = self.hw.sensor_us.distance() if dist is not None and dist < 275: #self.counter_us = 1 self.counter_us += 1 print("[US] Maddie opened gate ...") wait(WAIT_TIME * 10) # Tell Dig/Nest to prepare #self.ble.broadcast("CMD", "JKG_wave") # Open gate self.open_close_gate("open") return if self.counter_us == 1: dist = self.hw.sensor_us.distance() if dist is not None and dist < 275: #self.counter_us = 2 self.counter_us += 1 print("[US] Jeep closed gate ...") wait(WAIT_TIME * 10) # Tell Dig/Nest to prepare #self.ble.broadcast("CMD", "JKG_wave") # Open gate self.open_close_gate("close") return # After gate opens, all actions come from BLE # BLE messages handled in on_ble_message() method # 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 == "open": self.open_close_gate("open") elif action == "close": self.open_close_gate("close") #elif action == "wave": # self.ble.broadcast("EVENT", "wave") #elif action == "rumble": # self.frequency = "high" # self.ble.broadcast("EVENT", "rumble") elif action == "JKG_shlo": self.shake_trees(self.frequency) elif action == "JKG_shhi": self.frequency = "high" self.shake_trees(self.frequency) elif action == "stop": motors_stop = [0,0,0,0,0,""] self.reset(motors_stop) self.ble.broadcast("EVENT", "stop") # Example: Gate tells Fossil Dig to start #if action == "start": # self.hw.do_action() # self.ble.broadcast("EVENT", "digging") # Open or close gate def open_close_gate(self, action): speed = SPEED_MOTOR / 5 if action == 'open': print('Opening gate ...') self.ble.broadcast("EVENT", "JKG_open") self.hw.motor_gate_left.run_angle(speed, self.angle_gate, wait=False) self.hw.motor_gate_right.run_angle(speed, self.angle_gate) #self.angle_gate = self.angle_gate * -1 self.angle_gate *= -1 elif action == 'close': print('Closing gate ...') self.ble.broadcast("EVENT", "JKG_close") self.hw.motor_gate_left.run_angle(speed, self.angle_gate, wait=False) self.hw.motor_gate_right.run_angle(speed, self.angle_gate) #self.angle_gate = self.angle_gate * -1 self.angle_gate *= -1 # Reset gate & light 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 gate, light, trees motors, set angle for all active motors motors_start = [0,0,270,0,0,''] # Port A-F #motors_stop = [0,0,0,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()