from utime import sleep_us, sleep_ms from machine import Pin, PWM, ADC, time_pulse_us # LEDs STATUS = 0 SONAR = 1 LEFT_LINE = 2 RIGHT_LINE = 3 # Directions STOP = 0 LEFT = 1 RIGHT = 2 SEARCH = 3 FORWARD = 4 BACKWARD = 5 class Sumorobot(object): # Constructor def __init__(self, config = None): # Config file self.config = config # Sonar distance sensor self.echo = Pin(14, Pin.IN) self.trigger = Pin(27, Pin.OUT) # Servo PWM-s self.pwm = { LEFT: PWM(Pin(15), freq=50, duty=0), RIGHT: PWM(Pin(4), freq=50, duty=0) } # LED sensor feedback self.sensor_feedback = True # Bottom status LED self.status_led = Pin(self.config['status_led_pin'], Pin.OUT) # Bottom status LED is in reverse polarity self.status_led.value(1) # Sensor LEDs self.sonar_led = Pin(16, Pin.OUT) self.left_line_led = Pin(17, Pin.OUT) self.right_line_led = Pin(12, Pin.OUT) # Battery level in % self.battery_level = 0 # Battery gauge self.bat_status = 4.3 self.move_counter = 0 self.adc_battery = ADC(Pin(32)) self.bat_charge = Pin(25, Pin.IN) # The pullups for the phototransistors Pin(19, Pin.IN, Pin.PULL_UP) Pin(23, Pin.IN, Pin.PULL_UP) # The phototransistors self.last_line = LEFT self.adc_line_left = ADC(Pin(34)) self.adc_line_right = ADC(Pin(33)) # Set reference voltage to 3.3V self.adc_battery.atten(ADC.ATTN_11DB) self.adc_line_left.atten(ADC.ATTN_11DB) self.adc_line_right.atten(ADC.ATTN_11DB) # To smooth out sonar sensor value self.sonar_score = 0 # For terminating sleep self.terminate = False # For search mode self.search = False self.search_counter = 0 # Memorise previous servo speeds self.prev_speed = {LEFT: 0, RIGHT: 0} # Function to set LED states def set_led(self, led, value): # Turn the given LED on or off if led == STATUS: # Status LED is reverse polarity self.status_led.value(0 if value else 1) elif led == SONAR: self.sonar_led.value(value) elif led == LEFT_LINE: self.left_line_led.value(value) elif led == RIGHT_LINE: self.right_line_led.value(value) # Function to get battery level in percentage def get_battery_level(self): # When the SumoRobot is not moving if self.prev_speed[LEFT] == 0 and self.prev_speed[RIGHT] == 0: # Calculate battery voltage battery_voltage = round(self.config['battery_coeff'] * (self.adc_battery.read() * 3.3 / 4096), 2) # Map battery voltage to percentage temp_battery_level = 0.0 + ((100.0 - 0.0) / (4.2 - 3.2)) * (battery_voltage - 3.2) # When battery level changed more than 5 percent if abs(self.battery_level - temp_battery_level) > 5: # Update battery level self.battery_level = round(temp_battery_level) # Return the battery level in percentage return min(100, max(0, self.battery_level)) # Function to get distance (cm) from the object in front of the SumoRobot def get_sonar_value(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 round((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_sonar(self): # Get the sonar value self.sonar_value = self.get_sonar_value() # When the sonar value is small and the ping actually returned if self.sonar_value < self.config['sonar_threshold'] and self.sonar_value > 0: # When not maximum score if self.sonar_score < 5: # Increase the sonar score self.sonar_score += 1 # When no sonar ping was returned else: # When not lowest score if self.sonar_score > 0: # Decrease the sonar score self.sonar_score -= 1 # When the sensor saw something more than 2 times value = True if self.sonar_score > 2 else False # Trigger sonar LED self.set_led(SONAR, value) return value # Function to update the config file def update_config_file(self): # 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 update line calibration and write it to the config file def calibrate_line_values(self): # Read the line sensor values self.config['left_line_value'] = self.adc_line_left.read() self.config['right_line_value'] = self.adc_line_right.read() # Function to get light inensity from the phototransistors def get_line(self, line): # Check if the direction is valid assert line in (LEFT, RIGHT) # Return the given line sensor value if line == LEFT: return self.adc_line_left.read() elif line == RIGHT: return self.adc_line_right.read() def is_line(self, line): # Check if the direction is valid assert line in (LEFT, RIGHT) # Define feedback LED led = LEFT_LINE if line == LEFT else RIGHT_LINE # Define config prefix prefix = 'left' if line == LEFT else 'right' # Check for line value = abs(self.get_line(line) - self.config[prefix + '_line_value']) > self.config[prefix + '_line_threshold'] # Show LED feedback self.set_led(led, value) # Update last line direction if line was detected self.last_line = value if value else self.last_line # Return the given line sensor value return value def set_servo(self, servo, speed): # Check if the direction is valid assert servo 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[servo]: return # Save the new speed self.prev_speed[servo] = speed # Set the given servo speed if speed == 0: self.pwm[servo].duty(0) else: # Define config prefix prefix = 'left' if servo == LEFT else 'right' # -100 ... 100 to min_tuning .. max_tuning min_tuning = self.config[prefix + '_servo_min_tuning'] max_tuning = self.config[prefix + '_servo_max_tuning'] self.pwm[servo].duty(int((speed + 100) / 200 * (max_tuning - min_tuning) + min_tuning)) 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_servo(LEFT, 0) self.set_servo(RIGHT, 0) elif dir == LEFT: self.set_servo(LEFT, -100) self.set_servo(RIGHT, -100) elif dir == RIGHT: self.set_servo(LEFT, 100) self.set_servo(RIGHT, 100) elif dir == SEARCH: # Change search mode after X seconds if self.search_counter == 50: self.search = not self.search self.search_counter = 0 # When in search mode if self.search: self.move(FORWARD) elif self.last_line == RIGHT: self.move(LEFT) else: self.move(RIGHT) # Increase search counter self.search_counter += 1 elif dir == FORWARD: self.set_servo(LEFT, 100) self.set_servo(RIGHT, -100) elif dir == BACKWARD: self.set_servo(LEFT, -100) self.set_servo(RIGHT, 100) def update_sensor_feedback(self): if self.sensor_feedback: # Execute to see LED feedback for sensors self.is_sonar() self.is_line(LEFT) self.is_line(RIGHT) def get_sensor_scope(self): # TODO: implement sensor value caching return str(self.get_sonar_value()) + ',' \ + str(self.get_line(LEFT)) + ',' \ + str(self.get_line(RIGHT)) + ',' \ + str(self.bat_charge.value()) + ',' \ + str(self.get_battery_level()) def get_configuration_scope(self): return str(self.config['sumorobot_name']) + ',' \ + str(self.config['firmware_version']) + ',' \ + str(self.config['left_line_value']) + ',' \ + str(self.config['right_line_value']) + ',' \ + str(self.config['left_line_threshold']) + ',' \ + str(self.config['right_line_threshold']) + ',' \ + str(self.config['sonar_threshold']) def sleep(self, delay): # Check for valid delay assert delay > 0 # Split the delay into 50ms chunks while delay: # Check for forceful termination if self.terminate: # Terminate the delay return else: sleep_ms(50) delay -= 50