k-space/sumorobot-firmware
10
0
Fork 0
Code Issues Pull Requests Wiki Activity

add bootup blinking

Browse Source
This commit is contained in:
Silver Kuusik 2018-07-11 21:14:45 +02:00 committed by GitHub
parent d0159e91db
commit 5a7a4865fc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 99 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
hal.py
View File

@ -9,69 +9,67 @@ OPPONENT = 1
LEFT_LINE = 2 LEFT_LINE = 2
RIGHT_LINE = 3 RIGHT_LINE = 3
# directions # Directions
STOP = 0 STOP = 0
LEFT = 1 LEFT = 1
RIGHT = 2 RIGHT = 2
FORWARD = 3 FORWARD = 3
BACKWARD = 4 BACKWARD = 4
# open and parse the config file
config = None
with open("config.json", "r") as config_file:
config = ujson.load(config_file)
class Sumorobot(object): class Sumorobot(object):
# Constructor
def __init__(self, config = None):
# Config file
self.config = config
# ultrasonic distance sensor # Ultrasonic distance sensor
echo = Pin(14, Pin.IN) self.echo = Pin(14, Pin.IN)
trigger = Pin(27, Pin.OUT) self.trigger = Pin(27, Pin.OUT)
# Servo PWM-s # Servo PWM-s
pwm_left = PWM(Pin(15), freq=50, duty=0) self.pwm_left = PWM(Pin(15), freq=50, duty=0)
pwm_right = PWM(Pin(4), freq=50, duty=0) self.pwm_right = PWM(Pin(4), freq=50, duty=0)
# bottom LED # Bottom status LED
bottom_led = Pin(22, Pin.OUT) self.status_led = Pin(22, Pin.OUT)
# bottom LED is in reverse polarity # Bottom status LED is in reverse polarity
bottom_led.value(1) self.status_led.value(1)
# sensor LEDs # Sensor LEDs
opponent_led = Pin(16, Pin.OUT) self.opponent_led = Pin(16, Pin.OUT)
left_line_led = Pin(17, Pin.OUT) self.left_line_led = Pin(17, Pin.OUT)
right_line_led = Pin(12, Pin.OUT) self.right_line_led = Pin(12, Pin.OUT)
# battery gauge # Battery gauge
adc_battery = ADC(Pin(32)) self.adc_battery = ADC(Pin(32))
# the pullups for the phototransistors # The pullups for the phototransistors
Pin(19, Pin.IN, Pin.PULL_UP) Pin(19, Pin.IN, Pin.PULL_UP)
Pin(23, Pin.IN, Pin.PULL_UP) Pin(23, Pin.IN, Pin.PULL_UP)
# the phototransistors # The phototransistors
adc_line_left = ADC(Pin(34)) self.adc_line_left = ADC(Pin(34))
adc_line_right = ADC(Pin(33)) self.adc_line_right = ADC(Pin(33))
# Set reference voltage to 3.3V # Set reference voltage to 3.3V
adc_battery.atten(ADC.ATTN_11DB) self.adc_battery.atten(ADC.ATTN_11DB)
adc_line_left.atten(ADC.ATTN_11DB) self.adc_line_left.atten(ADC.ATTN_11DB)
adc_line_right.atten(ADC.ATTN_11DB) self.adc_line_right.atten(ADC.ATTN_11DB)
# for highlighting blockly blocks # To smooth out ultrasonic sensor value
highlight_block = None self.opponent_score = 0
# for terminating sleep # For terminating sleep
terminate = False self.terminate = False
# to smooth out ultrasonic sensor value # Memorise previous servo speeds
opponent_score = 0 self.prev_speed = {LEFT: 0, RIGHT: 0}
def __init__(self, highlight_block):
self.highlight_block = highlight_block
# Function to set LED states
def set_led(self, led, state): def set_led(self, led, state):
# set the given LED state # Set the given LED state
if led == STATUS: if led == STATUS:
self.bottom_led.value(0 if state else 1) # Status LED is reverse polarity
self.status_led.value(0 if state else 1)
elif led == OPPONENT: elif led == OPPONENT:
self.opponent_led.value(state) self.opponent_led.value(state)
elif led == LEFT_LINE: elif led == LEFT_LINE:
@ -79,123 +77,131 @@ class Sumorobot(object):
elif led == RIGHT_LINE: elif led == RIGHT_LINE:
self.right_line_led.value(state) self.right_line_led.value(state)
def get_battery_voltage(self): # Function to shortly bink status LED
return round(config["battery_coeff"] * (self.adc_battery.read() * 3.3 / 4096), 2) def toggle_led(self, timer = None):
self.status_led.value(0)
sleep_ms(10)
self.status_led.value(1)
# Function to get battery voltage
def get_battery_voltage(self):
return round(self.config["battery_coeff"] * (self.adc_battery.read() * 3.3 / 4096), 2)
# Function to get distance (cm) from the object in front of the SumoRobot
def get_opponent_distance(self): def get_opponent_distance(self):
# send a pulse # Send a pulse
self.trigger.value(0) self.trigger.value(0)
sleep_us(5) sleep_us(5)
self.trigger.value(1) self.trigger.value(1)
sleep_us(10) sleep_us(10)
self.trigger.value(0) self.trigger.value(0)
# wait for the pulse and calculate the distance # Wait for the pulse and calculate the distance
return (time_pulse_us(self.echo, 1, 30000) / 2) / 29.1 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, block_id = None): def is_opponent(self, block_id = None):
# if block_id given and blockly highlight is on # When block_id given and blockly highlight is on
if block_id and config["blockly_highlight"]: if block_id and self.config["blockly_highlight"]:
self.highlight_block(block_id) self.highlight_block(block_id)
# get the opponent distance # Get the opponent distance
self.opponent_distance = self.get_opponent_distance() self.opponent_distance = self.get_opponent_distance()
# if the opponent is close and the ping actually returned # When the opponent is close and the ping actually returned
if self.opponent_distance < config["ultrasonic_distance"] and self.opponent_distance > 0: if self.opponent_distance < self.config["ultrasonic_distance"] and self.opponent_distance > 0:
# if not maximum score # When not maximum score
if self.opponent_score < 5: if self.opponent_score < 5:
# increase the opponent score # Increase the opponent score
self.opponent_score += 1 self.opponent_score += 1
# if no opponent was detected # When no opponent was detected
else: else:
# if not lowest score # When not lowest score
if self.opponent_score > 0: if self.opponent_score > 0:
# decrease the opponent score # Decrease the opponent score
self.opponent_score -= 1 self.opponent_score -= 1
# if the sensor saw something more than 2 times # When the sensor saw something more than 2 times
opponent = True if self.opponent_score > 2 else False opponent = True if self.opponent_score > 2 else False
# trigger opponent LED # Trigger opponent LED
self.set_led(OPPONENT, opponent) self.set_led(OPPONENT, opponent)
return opponent return opponent
# Function to update line calibration and write it to the config file
def calibrate_line(self): def calibrate_line(self):
# read the line sensor values # Read the line sensor values
config["left_line_threshold"] = self.adc_line_left.read() self.config["left_line_threshold"] = self.adc_line_left.read()
config["right_line_threshold"] = self.adc_line_right.read() self.config["right_line_threshold"] = self.adc_line_right.read()
# update the config file # Update the config file
with open("config.part", "w") as config_file: with open("config.part", "w") as config_file:
config_file.write(ujson.dumps(config)) config_file.write(ujson.dumps(config))
os.rename("config.part", "config.json") os.rename("config.part", "config.json")
# Function to get light inensity from the phototransistors
def get_line(self, dir): def get_line(self, dir):
# check for valid direction # Check for valid direction
assert dir == LEFT or dir == RIGHT assert dir == LEFT or dir == RIGHT
# return the given line sensor value # Return the given line sensor value
if dir == LEFT: if dir == LEFT:
return self.adc_line_left.read() return self.adc_line_left.read()
elif dir == RIGHT: elif dir == RIGHT:
return self.adc_line_right.read() return self.adc_line_right.read()
def is_line(self, dir, block_id = None): def is_line(self, dir, block_id = None):
# check for valid direction # Check for valid direction
assert dir == LEFT or dir == RIGHT assert dir == LEFT or dir == RIGHT
# if block_id given and blockly highlight is on # When block_id given and blockly highlight is on
if block_id and config["blockly_highlight"]: if block_id and self.config["blockly_highlight"]:
self.highlight_block(block_id) self.highlight_block(block_id)
# return the given line sensor value # Return the given line sensor value
if dir == LEFT: if dir == LEFT:
line = abs(self.adc_line_left.read() - config["left_line_threshold"]) > 1000 line = abs(self.get_line(LEFT) - self.config["left_line_threshold"]) > 1000
self.set_led(LEFT_LINE, line) self.set_led(LEFT_LINE, line)
return line return line
elif dir == RIGHT: elif dir == RIGHT:
line = abs(self.adc_line_right.read() - config["right_line_threshold"]) > 1000 line = abs(self.get_line(RIGHT) - self.config["right_line_threshold"]) > 1000
self.set_led(RIGHT_LINE, line) self.set_led(RIGHT_LINE, line)
return line return line
def detach_servos(self):
self.set_servo(LEFT, 0)
self.set_servo(RIGHT, 0)
prev_speed = {LEFT: 0, RIGHT: 0}
def set_servo(self, dir, speed): def set_servo(self, dir, speed):
# check for valid direction # Check for valid direction
assert dir == LEFT or dir == RIGHT assert dir == LEFT or dir == RIGHT
# check for valid speed # Check for valid speed
assert speed <= 100 and speed >= -100 assert speed <= 100 and speed >= -100
# when the speed didn't change # When the speed didn't change
if speed == self.prev_speed[dir]: if speed == self.prev_speed[dir]:
return return
# record the new speed # Record the new speed
self.prev_speed[dir] = speed self.prev_speed[dir] = speed
# set the given servo speed # Set the given servo speed
if dir == LEFT: if dir == LEFT:
if speed == 0: if speed == 0:
self.pwm_left.duty(0) self.pwm_left.duty(0)
else: else:
self.pwm_left.duty(int(33 + config["left_servo_tuning"] + speed * 33 / 100)) # -100 ... 100 to 33 .. 102 # -100 ... 100 to 33 .. 102
self.pwm_left.duty(int(33 + self.config["left_servo_tuning"] + speed * 33 / 100))
elif dir == RIGHT: elif dir == RIGHT:
if speed == 0: if speed == 0:
self.pwm_right.duty(0) self.pwm_right.duty(0)
else: else:
self.pwm_right.duty(int(33 + config["right_servo_tuning"] + speed * 33 / 100)) # -100 ... 100 to 33 .. 102 # -100 ... 100 to 33 .. 102
self.pwm_right.duty(int(33 + self.config["right_servo_tuning"] + speed * 33 / 100))
def move(self, dir, block_id = None): def move(self, dir, block_id = None):
# check for valid direction # Check for valid direction
assert dir == STOP or dir == RIGHT or dir == LEFT or dir == BACKWARD or dir == FORWARD assert dir == STOP or dir == RIGHT or dir == LEFT or dir == BACKWARD or dir == FORWARD
# if block_id given and blockly highlight is on # When block_id given and blockly highlight is on
if block_id and config["blockly_highlight"]: if block_id and self.config["blockly_highlight"]:
self.highlight_block(block_id) self.highlight_block(block_id)
# go to the given direction # Go to the given direction
if dir == STOP: if dir == STOP:
self.set_servo(LEFT, 0) self.set_servo(LEFT, 0)
self.set_servo(RIGHT, 0) self.set_servo(RIGHT, 0)
@ -213,18 +219,18 @@ class Sumorobot(object):
self.set_servo(RIGHT, 100) self.set_servo(RIGHT, 100)
def sleep(self, delay, block_id = None): def sleep(self, delay, block_id = None):
# check for valid delay # Check for valid delay
assert delay > 0 assert delay > 0
# if block_id given and blockly highlight is on # When block_id given and blockly highlight is on
if block_id and config["blockly_highlight"]: if block_id and self.config["blockly_highlight"]:
self.highlight_block(block_id) self.highlight_block(block_id)
# split the delay into 50ms chunks # Split the delay into 50ms chunks
for j in range(0, delay, 50): for j in range(0, delay, 50):
# check for forceful termination # Check for forceful termination
if self.terminate: if self.terminate:
# terminate the delay # Terminate the delay
return return
else: else:
sleep_ms(50) sleep_ms(50)