#!/usr/bin/env pybricks-micropython # Build based on GyroBoy robot code and building instructions: # https://github.com/pybricks/pybricks-projects/blob/master/sets/mindstorms-ev3/education-core/gyro_boy/main.py # https://assets.education.lego.com/v3/assets/blt293eea581807678a/blt9b683d3a8c4c4078/5f8801eba0ee6b216678e013/ev3-model-core-set-gyro-boy.pdf?locale=en-us # Main changes include: # - Adding 'jaw' motor to represent T-Rex jaw # - Moving ultrasonic sensor from side to top to represent T-Rex eyes # - Removing colour sensor as not required given movement by ultrasonic sensor # - Adding arms movement based on distance # - Adding Wi-Fi 2-way connectivity to get started from Jeep and have it play sounds using its Bluetooth mic # - Adding dinosaur sounds from https://www.dinosaurfact.net/Sounds.php and display images (in .png format) created from stock. # Import modules import math, socket, sys, _thread, time, urandom from pybricks.hubs import EV3Brick from pybricks.ev3devices import ColorSensor, GyroSensor, InfraredSensor, Motor, TouchSensor, UltrasonicSensor from pybricks.media.ev3dev import Font, ImageFile, SoundFile from pybricks.parameters import Button, Color, Direction, Port, Stop from pybricks.robotics import DriveBase from pybricks.tools import DataLog, StopWatch, wait from ucollections import namedtuple # Initialize brick Rexie = EV3Brick() # Initialize robot constants axle_track = 105 # mm distance between middle of tire contact wheel_diameter = 55.5 # mm font_big = Font(size=24) font_small = Font(size=8) Rexie.screen.set_font(font_small) # Initialize motors motor_arms = Motor(Port.A) motor_left = Motor(Port.B) motor_right = Motor(Port.C) motor_jaw = Motor(Port.D) drive_base = DriveBase(motor_left, motor_right, wheel_diameter, axle_track) # Initialize sensors #sensor_colour = ColorSensor(Port.S1) sensor_gyro = GyroSensor(Port.S2) #sensor_ir = InfraredSensor(Port.S4) sensor_touch = TouchSensor(Port.S3) sensor_us = UltrasonicSensor(Port.S4) # Declare program variables Action = namedtuple('Action', ['speed_drive', 'steering']) not_connected = True # Timers timer_action = StopWatch() timer_control_loop = StopWatch() timer_fall = StopWatch() timer_single_loop = StopWatch() # Behaviours pending_behaviour = { "sound": None, "motion_jaw": None, "motion_arms": None, "started_at": 0 } # Initialize program constants ANGLE_ARMS = 90 # deg ANGLE_JAW = -60 # deg COUNT_GYRO_CALIBRATION_LOOP = 200 FACTOR_GYRO_OFFSET = 0.0005 SPEED_MOTOR_ARMS = 600 # deg/s SPEED_MOTOR_JAW = 200 # deg/s PERIOD_TARGET_LOOP = 15 # ms # Predefined actions BACKWARD_FAST = Action(speed_drive=-75, steering=0) BACKWARD_SLOW = Action(speed_drive=-10, steering=0) FORWARD_FAST = Action(speed_drive=150, steering=0) FORWARD_SLOW = Action(speed_drive=40, steering=0) STOP = Action(speed_drive=0, steering=0) TURN_LEFT = Action(speed_drive=0, steering=-70) TURN_RIGHT = Action(speed_drive=0, steering=70) # Declare functions def behaviour_scheduler(): while True: #if isinstance(pending_behaviour["sound"], dict): if pending_behaviour["sound"] != "": sound = pending_behaviour["sound"] #print("Sending", sound, "request") #send_message(sound["name"]) send_message(sound) #pending_behaviour["sound"] = None pending_behaviour["sound"] = "" if pending_behaviour["motion_jaw"] == "open_close": behaviour = pending_behaviour["motion_jaw"] print("Executing", behaviour, "action") motor_jaw.reset_angle(0) motor_jaw.run_angle(SPEED_MOTOR_JAW, ANGLE_JAW, wait=False) while not motor_jaw.control.done(): yield motor_jaw.run_angle(SPEED_MOTOR_JAW, -ANGLE_JAW, wait=False) while not motor_jaw.control.done(): yield pending_behaviour["motion_jaw"] = None if pending_behaviour["motion_arms"] == "up_down": behaviour = pending_behaviour["motion_arms"] print("Executing", behaviour, "action") motor_arms.reset_angle(0) ANGLE_TEMP = ANGLE_ARMS / 2 for _ in range(3): motor_arms.run_angle(SPEED_MOTOR_ARMS, ANGLE_TEMP, wait=False) while not motor_arms.control.done(): yield motor_arms.run_angle(SPEED_MOTOR_ARMS, -ANGLE_ARMS, wait=False) while not motor_arms.control.done(): yield motor_arms.run_angle(SPEED_MOTOR_ARMS, ANGLE_TEMP, wait=False) while not motor_arms.control.done(): yield pending_behaviour["motion_arms"] = None yield # Send message to Jeep def send_message(sound_name): s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) try: s.connect(("jeep", 12346)) # port must match Jeep’s listener s.sendall(sound_name.encode()) print("Message", sound_name, "sent to Jeep successfully.") except Exception as e: print("Connection to Jeep unsuccessful! Error:", e) finally: s.close() # If falling over, stop arms, jaw & wheel motors def stop_action(): motor_arms.run_target(SPEED_MOTOR_ARMS, 0) motor_jaw.run_target(SPEED_MOTOR_JAW, 0) motor_left.stop() motor_right.stop() # Update action def update_action(): # Declare function variables # Drive forward for 1 seconds to leave stand, then stop. action_current = FORWARD_SLOW timer_action.reset() yield action_current while timer_action.time() < 1000: yield action_current = STOP yield action_current # Check ultrasonic sensor to determine action while True: distance = sensor_us.distance() action_new = STOP if action_new is not None: timer_action.reset() while timer_action.time() < 100: yield if action_new.steering != 0: action_current = Action(speed_drive=action_current.speed_drive, steering=action_new.steering) else: action_current = action_new yield action_current #If distance more than 500 mm, turn left or right for 1 seconds & set 'Rexie growl' sound if distance >= 500: print('>= 500') turn = urandom.choice([TURN_LEFT, TURN_RIGHT]) action_current = Action(speed_drive=FORWARD_SLOW.speed_drive, steering=turn.steering) yield action_current timer_action.reset() while timer_action.time() < 1000: yield #pending_behaviour["sound"] = {"name": "GROWL", "step": 0} pending_behaviour["sound"] = "GROWL" # If distance less than 500 mm, move forward fast while moving jaw & set 'Rexie attack' sound elif (distance < 500) and (distance >= 250): print('>=250 & <500') action_current = FORWARD_FAST yield action_current # Open then close jaw pending_behaviour["motion_jaw"] = "open_close" timer_action.reset() while timer_action.time() < 1000: yield #pending_behaviour["sound"] = {"name": "ATTACK", "step": 0} pending_behaviour["sound"] = "ATTACK" # If distance less than 250 mm, move forward slowly while moving arms, then move backward fast & set 'Rexie win' sound elif (distance < 250) and (distance >= 100): print('>=100 & <250') action_current = FORWARD_SLOW yield action_current # Move arms half up, down & half up pending_behaviour["motion_arms"] = "up_down" action_current = BACKWARD_FAST yield action_current timer_action.reset() while timer_action.time() < 1000: yield #pending_behaviour["sound"] = {"name": "WIN", "step": 0} pending_behaviour["sound"] = "WIN" else: print('Distance < 100') # Add delay to not read sensor continuously timer_action.reset() while timer_action.time() < 100: yield # Declare main program motor_arms.reset_angle(0) motor_jaw.reset_angle(0) # Set up comms to listen for START command host = '0.0.0.0' # Rexie port = 12345 r = socket.socket(socket.AF_INET, socket.SOCK_STREAM) r.bind((host, port)) r.listen(5) # Allow multiple connections print("Waiting for connection from Jeep...") while not_connected: conn, addr = r.accept() msg = conn.recv(1024).decode() if msg == "START": print("Message START received from Jeep successfully.") # Close this connection, but keep listening conn.close() not_connected = False while True: # When waking, set JP image & light off Rexie.screen.load_image("Jurassic_Park.png") Rexie.light.off() # Declare program variables motor_left.reset_angle(0) motor_right.reset_angle(0) timer_fall.reset() sum_motor_position = 0 angle_wheel = 0 change_motor_position = [0, 0, 0, 0] speed_drive, steering = 0, 0 count_control_loop = 0 angle_robot_body = -0.25 # Prepare update_action() for later use action_task = update_action() # Calibrate gyro offset 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 # When ready, set Rexie image, 'Rexie growl' sound & green light Rexie.screen.load_image("Rexie.png") #Rexie.speaker.play_file("Rexie_Growl.wav") #pending_behaviour["sound"] = {"name": "GROWL", "step": 0} send_message("GROWL") Rexie.light.on(Color.GREEN) scheduler_task = behaviour_scheduler() # Balance robot while True: timer_single_loop.reset() # Calculate average control loop period if count_control_loop == 0: # Assign a value to avoid dividing by zero later period_average_control_loop = PERIOD_TARGET_LOOP / 1000 timer_control_loop.reset() else: period_average_control_loop = (timer_control_loop.time() / 1000 / count_control_loop) count_control_loop += 1 # Calculate robot body angle and speed 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 # Calculate wheel angle and speed 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 # Calculate main control feedback 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 # Drive motors motor_left.dc(output_power - 0.1 * steering) motor_right.dc(output_power + 0.1 * steering) # Check robot fell down. if abs(output_power) < 100: timer_fall.reset() elif timer_fall.time() > 1000: break scheduler = next(scheduler_task) # Run update_action() until next yield statement action = next(action_task) if action is not None: speed_drive, steering = action # Set loop time to at least PERIOD_TARGET_LOOP wait(PERIOD_TARGET_LOOP - timer_single_loop.time()) # Stop all motors stop_action() # When fallen over, set JP image, 'Rexie growl' sound & red light Rexie.screen.load_image("Jurassic_Park.png") #Rexie.speaker.play_file("Rexie_Growl.wav") #pending_behaviour["sound"] = {"name": "GROWL", "step": 0} send_message("GROWL") Rexie.light.on(Color.RED) # Wait for a few seconds before trying to balance again wait(3000)