#!/usr/bin/env pybricks-micropython # Import modules import time, urandom from ucollections import namedtuple import urequests as requests from pybricks.ev3devices import GyroSensor, Motor, UltrasonicSensor from pybricks.hubs import EV3Brick from pybricks.parameters import Color, Port from pybricks.robotics import DriveBase from pybricks.tools import StopWatch, wait # Initialize program constants SERVER = "http://192.168.0.129:5000" DEVICE_NAME = "INDIE" POLL_INTERVAL = 1.0 # s HEARTBEAT_INTERVAL = 2.0 # s Action = namedtuple("Action", ["speed_drive", "steering"]) # Balancing constants AXLE_TRACK = 105 WHEEL_DIAMETER = 55.5 COUNT_GYRO_CALIBRATION_LOOP = 200 FACTOR_GYRO_OFFSET = 0.0005 PERIOD_TARGET_LOOP = 15 # ms # Behaviour constants ANGLE_ARMS = 90 ANGLE_JAW = -60 SPEED_MOTOR_ARMS = 400 SPEED_MOTOR_JAW = 150 # Distance thresholds (mm) DIST_FAR = 500 DIST_MID = 300 DIST_NEAR = 150 # Speeds (deg/s or mm/s) SPEED_DRIVE_SLOW = 80 SPEED_DRIVE_MED = 140 SPEED_ARMS = 400 SPEED_JAW = 150 # Durations (ms) ARM_FLAP_TIME = 400 JAW_BITE_TIME = 500 FORWARD_BURST_TIME = 600 # Actions BACKWARD_FAST = Action(speed_drive=-60, steering=0) BACKWARD_SLOW = Action(speed_drive=-10, steering=0) FORWARD_FAST = Action(speed_drive=120, steering=0) FORWARD_SLOW = Action(speed_drive=30, steering=0) STOP = Action(speed_drive=0, steering=0) TURN_LEFT = Action(speed_drive=0, steering=-60) TURN_RIGHT = Action(speed_drive=0, steering=60) # ------------------- # SERVER CLIENT class # ------------------- class ServerClient: # Initialize self def __init__(self, server, device): print("[SERVERCLIENT] Initializing") self.server = server self.device = device # Send event def send_event(self, kind, detail): # Populate JSON payload payload = { "device": self.device, "type": kind, "value": detail } # Try sending JSON event to /event endpoint on Flask server try: r = requests.post(self.server + "/event", json=payload, headers={"Connection": "close"} ) r.close() print("[EVENT]", payload) except Exception as e: print("[HTTP] Sending event failed with error '", e, "'") wait(200) # Receive command def get_command(self): # Try receiving JSON command from /sent_command/ endpoint on Flask server try: r = requests.get(self.server + "/sent_command/" + self.device, headers={"Connection": "close"} ) data = r.json() r.close() return data.get("command sent") except Exception as e: print("[HTTP] Receiving command failed with error '", e, "'") wait(200) return None # -------------------- # HARDWARE LAYER class # -------------------- class Hardware: # Initialize self def __init__(self): print("[HARDWARE] Initializing") # MODIFY TO SUIT REQUIRED HARDWARE self.ev3 = EV3Brick() # Robot constants self.axle_track = AXLE_TRACK # mm distance between middle of tire contact self.wheel_diameter = WHEEL_DIAMETER # mm # Motors self.motor_arms = Motor(Port.A) self.motor_left = Motor(Port.B) self.motor_right = Motor(Port.C) self.motor_jaw = Motor(Port.D) self.drive_base = DriveBase(self.motor_left, self.motor_right, self.wheel_diameter, self.axle_track) # Initialize sensors self.sensor_gyro = GyroSensor(Port.S2) self.sensor_us = UltrasonicSensor(Port.S4) # Note: we do NOT use DriveBase.drive() during balancing,to avoid fighting the balance controller. Commands are interpreted at a higher level instead. def forward(self): print("[HW] Running command forward") self.drive_base.drive(SPEED_DRIVE_SLOW, 0) def backward(self): print("[HW] Running command backward") self.drive_base.drive(-SPEED_DRIVE_SLOW, 0) def start(self): print("[HW] Running command start") #self.drive_base.drive(SPEED_DRIVE_SLOW, 0) def stop(self): print("[HW] Running command stop") self.drive_base.stop() def beep(self): print("[HW] Running command beep") # ------------------ # DEVICE LOGIC class # ------------------ class Device: # Initialize self def __init__(self, name, server): self.name = name self.server = ServerClient(server, name) self.hw = Hardware() self.last_heartbeat = 0 self.start_received = False # Behaviour state self.pending_behaviour = { "sound": None, "motion_jaw": None, "motion_arms": None } # Timers self.timer_action = StopWatch() self.timer_control_loop = StopWatch() self.timer_fall = StopWatch() self.timer_single_loop = StopWatch() # Balance def balance_once(self): # MODIFY TO SUIT REQUIRED CODE ev3 = self.hw.ev3 motor_left = self.hw.motor_left motor_right = self.hw.motor_right sensor_gyro = self.hw.sensor_gyro # Start UI when waking ev3.screen.clear() ev3.light.off() motor_left.reset_angle(0) motor_right.reset_angle(0) self.timer_fall.reset() sum_motor_position = 0 angle_wheel = 0 change_motor_position = [0, 0, 0, 0] speed_drive = 0 steering = 0 count_control_loop = 0 angle_robot_body = -0.25 # Prepare tasks action_task = self.update_action() scheduler_task = self.behaviour_scheduler() # Calibrate gyro while True: rate_gyro_minimum, rate_gyro_maximum = 440, -440 sum_gyro = 0 for _ in range(COUNT_GYRO_CALIBRATION_LOOP): sensor_gyro_value = sensor_gyro.speed() sum_gyro += sensor_gyro_value if sensor_gyro_value > rate_gyro_maximum: rate_gyro_maximum = sensor_gyro_value if sensor_gyro_value < rate_gyro_minimum: rate_gyro_minimum = sensor_gyro_value wait(5) if rate_gyro_maximum - rate_gyro_minimum < 2: break offset_gyro = sum_gyro / COUNT_GYRO_CALIBRATION_LOOP # Ready UI ev3.light.on(Color.GREEN) # Run control loop while True: self.timer_single_loop.reset() if count_control_loop == 0: period_average_control_loop = PERIOD_TARGET_LOOP / 1000 self.timer_control_loop.reset() else: period_average_control_loop = (self.timer_control_loop.time() / 1000 / count_control_loop) count_control_loop += 1 # Set body angle & rate sensor_gyro_value = sensor_gyro.speed() offset_gyro *= (1 - FACTOR_GYRO_OFFSET) offset_gyro += FACTOR_GYRO_OFFSET * sensor_gyro_value rate_robot_body = sensor_gyro_value - offset_gyro angle_robot_body += rate_robot_body * period_average_control_loop # Set wheel angle & rate angle_motor_left = motor_left.angle() angle_motor_right = motor_right.angle() sum_previous_motor = sum_motor_position sum_motor_position = angle_motor_left + angle_motor_right change = sum_motor_position - sum_previous_motor change_motor_position.insert(0, change) del change_motor_position[-1] angle_wheel += change - speed_drive * period_average_control_loop wheel_rate = sum(change_motor_position) / 4 / period_average_control_loop # Run main control output_power = (-0.01 * speed_drive + 0.8 * rate_robot_body + 15 * angle_robot_body + 0.08 * wheel_rate + 0.12 * angle_wheel) if output_power > 100: output_power = 100 if output_power < -100: output_power = -100 motor_left.dc(output_power - 0.1 * steering) motor_right.dc(output_power + 0.1 * steering) # Detect fall if abs(output_power) < 100: self.timer_fall.reset() elif self.timer_fall.time() > 1000: break # Run behaviour scheduler next(scheduler_task) # Update action action = next(action_task) if action is not None: speed_drive, steering = action wait(PERIOD_TARGET_LOOP - self.timer_single_loop.time()) # Stop motors when fallen motor_left.stop() motor_right.stop() ev3.light.on(Color.RED) wait(3000) # Run behaviour scheduler def behaviour_scheduler(self): while True: # Sound if self.pending_behaviour["sound"]: print("[SOUND]", self.pending_behaviour["sound"]) self.pending_behaviour["sound"] = None yield # Jaw motion if self.pending_behaviour["motion_jaw"] == "open_close": print("[BEHAVIOUR] Jaw open/close") m = self.hw.motor_jaw m.reset_angle(0) m.run_angle(SPEED_MOTOR_JAW, ANGLE_JAW, wait=False) while not m.control.done(): yield m.run_angle(SPEED_MOTOR_JAW, -ANGLE_JAW, wait=False) while not m.control.done(): yield self.pending_behaviour["motion_jaw"] = None yield # Arms motion if self.pending_behaviour["motion_arms"] == "up_down": print("[BEHAVIOUR] Arms up/down") m = self.hw.motor_arms m.reset_angle(0) angle_temp = ANGLE_ARMS // 2 for _ in range(2): # fewer cycles than original m.run_angle(SPEED_MOTOR_ARMS, angle_temp, wait=False) while not m.control.done(): yield m.run_angle(SPEED_MOTOR_ARMS, -ANGLE_ARMS, wait=False) while not m.control.done(): yield m.run_angle(SPEED_MOTOR_ARMS, angle_temp, wait=False) while not m.control.done(): yield self.pending_behaviour["motion_arms"] = None yield yield # Handle command sent by Flask server def handle_command(self, cmd): print("[CMD] Receiving command '", cmd, "'") # MODIFY TO SUIT REQUIRED COMMANDS if cmd == "forward": self.hw.forward() elif cmd == "backward": self.hw.backward() elif cmd == "start": self.hw.start() elif cmd == "attack": self.server.send_event("EVENT", "01") # If command is 'attack', allow code to proceed to periodic() self.start_received = True elif cmd == "stop": self.hw.stop() elif cmd == "beep": self.hw.beep() else: print("[CMD] Receiving unknown command '", cmd, "'") # Send status 'alive' to Flask server def heartbeat(self): self.server.send_event("STATUS", "alive") # Reset motors def reset_motors(self): self.hw.motor_arms.reset_angle(0) self.hw.motor_jaw.reset_angle(0) # Send event 'ready' to Flask server def startup(self): print("[DEVICE] Starting up") self.server.send_event("EVENT", "ready") # Update action def update_action(self): action_current = FORWARD_SLOW self.timer_action.reset() yield action_current while self.timer_action.time() < 800: yield action_current = STOP yield action_current while True: distance = self.hw.sensor_us.distance() action_new = STOP # Far: gentle random turn & growl if distance is not None and distance >= DIST_FAR: print("[DIST] Far") turn = urandom.choice([TURN_LEFT, TURN_RIGHT]) action_current = Action(speed_drive=FORWARD_SLOW.speed_drive, steering=turn.steering) yield action_current self.timer_action.reset() while self.timer_action.time() < 800: yield self.pending_behaviour["sound"] = "GROWL" # Mid: forward fast & jaw movement elif distance is not None and DIST_MID <= distance < DIST_FAR: print("[DIST] Mid") action_current = FORWARD_FAST yield action_current self.pending_behaviour["motion_jaw"] = "open_close" self.timer_action.reset() while self.timer_action.time() < 800: yield self.pending_behaviour["sound"] = "ATTACK" # Near: slow forward & arms, then slow backward elif distance is not None and DIST_NEAR <= distance < DIST_MID: print("[DIST] Near") action_current = FORWARD_SLOW yield action_current self.pending_behaviour["motion_arms"] = "up_down" action_current = BACKWARD_SLOW yield action_current self.timer_action.reset() while self.timer_action.time() < 800: yield self.pending_behaviour["sound"] = "WIN" else: # Very close or no reading: stop print("[DIST] Very close or None") action_current = STOP yield action_current self.timer_action.reset() while self.timer_action.time() < 200: yield self.timer_action.reset() while self.timer_action.time() < 100: yield # Define main function def run(self): # Send event self.startup() # Reset motors self.reset_motors() last_cmd = None while True: # Heartbeat & commands now = time.time() if now - self.last_heartbeat > HEARTBEAT_INTERVAL: self.heartbeat() self.last_heartbeat = now cmd = self.server.get_command() if cmd: if cmd != last_cmd: self.handle_command(cmd) # Send event command consumed to Flask server self.server.send_event("CMD_ACK", cmd) last_cmd = cmd else: last_cmd = None # Run one full balance session #self.balance_once() if self.start_received: self.balance_once() # Declare main program if __name__ == "__main__": device = Device( DEVICE_NAME, SERVER ) device.run()