# 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 Color, 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 = "HAB" # <--- 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 # --------------- # 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_a = Motor(Port.A) # Top right self.motor_b = Motor(Port.B) # Top left self.motor_c = Motor(Port.C) # Bottom left self.motor_d = Motor(Port.D) # Bottom right # Sensors self.sensor_us = UltrasonicSensor(Port.E) self.sensor_ir = PUPDevice(Port.F) # IR seeker # Example action def do_action(self): # <--- Add motors and sensors as required self.motor_a.run_angle(500, 90) self.motor_b.run_angle(500, -90) self.motor_c.run_angle(500, 90) self.motor_d.run_angle(500, -90) # Stop def stop(self): # <--- Add motors and sensors as required self.motor_a.stop() self.motor_b.stop() self.motor_c.stop() self.motor_d.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.waiting = True # Send status 'alive' to Flask server def heartbeat(self): self.ble.broadcast("STATUS", "alive") # Move with motor offset def move(self, direction, speed, rotation): # Convert motor angles to radians (relative to unit circle) i.e.: 0 degrees is East angle_A = radians(45) angle_B = radians(135) angle_C = radians(225) angle_D = radians(315) # Convert direction to radians direction_angle = radians(direction) # Calculate speed for motors based on position of IR ball speed_A = cos((direction_angle - angle_A)) * speed + rotation speed_B = cos((direction_angle - angle_B)) * speed + rotation speed_C = cos((direction_angle - angle_C)) * speed + rotation speed_D = cos((direction_angle - angle_D)) * speed + rotation # Run motors at the calculated speed self.hw.motor_a.run(speed_A) self.hw.motor_b.run(speed_B) self.hw.motor_c.run(speed_C) self.hw.motor_d.run(speed_D) # 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 # Start command if action == "start": #self.hw.do_action() self.waiting = True self.ble.broadcast("EVENT", "starting") # Stop command if action == "stop": self.hw.stop() self.ble.broadcast("EVENT", "stopping") # Define functions run periodically def periodic(self): # <--- Add required code # Check distance to object distance = self.hw.sensor_us.distance() # Wait mode if self.waiting: self.hub.light.on(Color.RED) # If object detected between 50mm and 150mm, start moving if 50 < distance < 150: self.waiting = False # Switch to active mode else: # Remain stopped self.hw.stop() # Avoid_objects return # Skip movement code # Active mode else: self.hub.light.on(Color.GREEN) # If object detected again, re-enter wait mode if 50 < distance < 150: self.waiting = True self.hw.stop() # Avoid_objects return # Skip movement code # Otherwise follow the ball values = self.hw.sensor_ir.read(5) simple_direction = values[0] # 0–11 (30° steps) strength = values[1] advanced_direction = values[2] # 0–359 degrees # Convert IR clockwise to unit-circle counter-clockwise corrected_direction = (360 - advanced_direction) % 360 # Adjust speed speed = 500 - strength rotation = self.smooth_rotation(corrected_direction) # Move self.move(corrected_direction, speed, rotation) # Rotate smoothly def smooth_rotation(self, desired_heading): # Initialise function constants # Proportional gain (smoothness control) Kp = 0.5 # Lower value is smoother, higher is snappier # Deadzone to prevent jitter near 0 degrees deadzone = 4 # degrees # Reset heading to 0 degrees i.e.: 'North' self.hub.imu.reset_heading(0) current_heading = self.hub.imu.heading() # Calculate how far desired & current are apart error = desired_heading - current_heading # Set error between -180 and 180 degrees if error > 180: error -= 360 elif error < -180: error += 360 if abs(error) < deadzone: return 0 # Proportional correction rotation_temp = Kp * error # Clamp to avoid extreme rotation speeds rotation = max(min(rotation_temp, 100), -100) print("Current:", current_heading, "Desired:", desired_heading, "Error:", error, "Rotation:", rotation) return rotation # Send event 'ready' to Flask server def startup(self): print("[DEVICE] Starting up") self.ble.broadcast("EVENT", "ready") # Define main function def run(self): # Send event self.startup() # Reset heading to 0 degrees i.e.: 'North' self.hub.imu.reset_heading(0) 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()