import os import ujson from utime import sleep_us, sleep_ms from machine import Pin, PWM, ADC, time_pulse_us, deepsleep import random # LEDs STATUS = 0 #OPPONENT = 1 #LEFT_LINE = 2 #RIGHT_LINE = 3 # Directions STOP = 0 LEFT = 1 MIDDLE = 6 RIGHT = 2 SEARCH = 3 FORWARD = 4 BACKWARD = 5 #states MOVING = 0 STANDBY = 1 class Sumorobot(object): # Constructor def __init__(self, config = None): # Config file self.config = config self.state = STANDBY self.name = "Sumo-"+self.config["sumo_id"] # Ultrasonic distance sensor self.echo = Pin(self.config["ultrasonic_echo_pin"], Pin.IN) self.trigger = Pin(self.config["ultrasonic_trigger_pin"], Pin.OUT) # Servo PWM-s self.pwm_left = PWM(Pin(self.config["left_motor_pin"]), freq=50, duty=0) self.pwm_right = PWM(Pin(self.config["right_motor_pin"]), freq=50, duty=0) # Bottom status LED self.led_power = Pin(self.config["led_power_pin"], Pin.OUT) self.charging = Pin(self.config["charging_pin"]); self.playStop = Pin(self.config["paly/stop_button_pin"], Pin.IN, Pin.PULL_UP) self.led_power.value(1) self.adc_line_left = ADC(Pin(32)) self.adc_line_middle = ADC(Pin(35)) self.adc_line_right = ADC(Pin(34)) # Set reference voltage to 3.3V self.adc_line_left.atten(ADC.ATTN_11DB) self.adc_line_right.atten(ADC.ATTN_11DB) self.adc_line_middle.atten(ADC.ATTN_11DB) # To smooth out ultrasonic sensor value self.opponent_score = 0 # For terminating sleep self.terminate = False # For search mode self.search = 0 self.search_counter = 0 # Memorise previous servo speeds self.prev_speed = {LEFT: 0, RIGHT: 0} #saving line sensor valus, to read once in 50ms loop self.line_left = 0 self.line_right = 0; self.line_middle = 0; self.speedForward = 100 if self.config["motors_reverse"] == 0 else -100 self.speedReverse = -100 if self.config["motors_reverse"] == 0 else 100 # Function to get distance (cm) from the object in front of the SumoRobot def get_opponent_distance(self): # Send a pulse self.trigger.value(0) sleep_us(5) self.trigger.value(1) sleep_us(10) self.trigger.value(0) # Wait for the pulse and calculate the distance return (time_pulse_us(self.echo, 1, 30000) / 2) / 29.1 # Function to get boolean if there is something in front of the SumoRobot def is_opponent(self): # Get the opponent distance self.opponent_distance = self.get_opponent_distance() # When the opponent is close and the ping actually returned if self.opponent_distance < self.config["ultrasonic_distance"] and self.opponent_distance > 0: # When not maximum score if self.opponent_score < 5: # Increase the opponent score self.opponent_score += 1 # When no opponent was detected else: # When not lowest score if self.opponent_score > 0: # Decrease the opponent score self.opponent_score -= 1 # When the sensor saw something more than 2 times opponent = True if self.opponent_score > 2 else False # Trigger opponent LED #self.set_led(OPPONENT, opponent) return opponent # Function to update line calibration and write it to the config file def calibrate_line(self): # Read the line sensor values self.config["left_line_threshold"] = self.adc_line_left.read() self.config["right_line_threshold"] = self.adc_line_right.read() self.config["middle_line_threshold"] = self.adc_line_middle.read() # Update the config file with open("config.part", "w") as config_file: config_file.write(ujson.dumps(self.config)) os.rename("config.part", "config.json") # Function to get light inensity from the phototransistors def get_line(self, dir): # Check if the direction is valid assert dir in (LEFT, RIGHT, MIDDLE) # Return the given line sensor value if dir == LEFT: return self.adc_line_left.read() elif dir == RIGHT: return self.adc_line_right.read() elif dir == MIDDLE: return self.adc_line_middle.read() def is_line(self, dir): # Check if the direction is valid assert dir in (LEFT, RIGHT, MIDDLE) # Return the given line sensor value, storing it to variable, not to ask double in 50ms time loop if dir == LEFT: self.line_left = self.get_line(LEFT) line = abs(self.line_left - self.config["left_line_threshold"]) > 1000 #self.set_led(LEFT_LINE, line) return line elif dir == RIGHT: self.line_right = self.get_line(RIGHT) line = abs(self.line_right - self.config["right_line_threshold"]) > 1000 #self.set_led(RIGHT_LINE, line) return line elif dir == MIDDLE: self.line_middle = self.get_line(MIDDLE) line = abs(self.line_middle - self.config["middle_line_threshold"]) > 1000 return line def set_servo(self, dir, speed): # Check if the direction is valid assert dir in (LEFT, RIGHT) # Check if the speed is valid assert speed <= 100 and speed >= -100 # When the speed didn't change if speed == self.prev_speed[dir]: return # Record the new speed self.prev_speed[dir] = speed # Set the given servo speed if dir == LEFT: if speed == 0: self.pwm_left.duty(0) else: # -100 ... 100 to 33 .. 102 self.pwm_left.duty(int(33 + self.config["left_servo_tuning"] + speed * 33 / 100)) elif dir == RIGHT: if speed == 0: self.pwm_right.duty(0) else: # -100 ... 100 to 33 .. 102 self.pwm_right.duty(int(33 + self.config["right_servo_tuning"] + speed * 33 / 100)) def move(self, dir): # Check if the direction is valid assert dir in (SEARCH, STOP, RIGHT, LEFT, BACKWARD, FORWARD) # Go to the given direction if dir == STOP: self.set_state(STANDBY) else: self.set_state(MOVING) if dir == STOP: self.set_servo(LEFT, 0) self.set_servo(RIGHT, 0) elif dir == LEFT: self.set_servo(LEFT, self.speedReverse) self.set_servo(RIGHT, self.speedReverse) elif dir == RIGHT: self.set_servo(LEFT, self.speedForward) self.set_servo(RIGHT, self.speedForward) elif dir == SEARCH: # Change search mode after X seconds if self.search_counter == 50: self.search = random.randrange(0,3) self.search_counter = 0 #self.search = 0 if self.search > 2 else self.search # When in search mode if self.search == 0: # Go forward self.set_servo(LEFT, self.speedForward) self.set_servo(RIGHT, self.speedReverse) elif self.search == 1: # Go left self.set_servo(LEFT, self.speedReverse) self.set_servo(RIGHT, self.speedReverse) elif self.search == 2: self.set_servo(LEFT, self.speedForward) self.set_servo(RIGHT, self.speedForward) # Increase search counter self.search_counter += 1 elif dir == FORWARD: self.set_servo(LEFT, self.speedForward) self.set_servo(RIGHT, self.speedReverse) elif dir == BACKWARD: self.set_servo(LEFT, self.speedReverse) self.set_servo(RIGHT, self.speedForward) def sleep(self, delay): # Check for valid delay assert delay > 0 # Split the delay into 50ms chunks for j in range(0, delay, 50): # Check for forceful termination if self.terminate: # Terminate the delay return else: sleep_ms(50) def set_state(self,value): assert value in (MOVING,STANDBY) self.state = value def get_state(self): return self.state