258 lines
8.4 KiB
Python
258 lines
8.4 KiB
Python
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
|