diff --git a/Makefile b/Makefile index f5c64e7..c76f925 100755 --- a/Makefile +++ b/Makefile @@ -1,12 +1,28 @@ #!/bin/bash #SERIAL_PORT=/dev/ttyUSB0 -SERIAL_PORT=/dev/tty.usbserial-1410 +SERIAL_PORT=/dev/tty.usbserial-1420 #SERIAL_PORT=/dev/tty.SLAB_USBtoUART -#SERIAL_PORT=/dev/tty.wchusbserial1410 -all: flash +all: flash delay config update reset + +delay: + sleep 3 + +reset: + esptool.py -p $(SERIAL_PORT) --after hard_reset read_mac + +update: + ampy -d 0.5 -p $(SERIAL_PORT) put hal.py + ampy -d 0.5 -p $(SERIAL_PORT) put main.py + ampy -d 0.5 -p $(SERIAL_PORT) put boot.py + +config: + ampy -d 0.5 -p $(SERIAL_PORT) put config.json flash: esptool.py -p $(SERIAL_PORT) -b 460800 erase_flash - esptool.py -p $(SERIAL_PORT) -b 460800 write_flash --flash_mode dio 0x1000 esp32-*.bin + esptool.py -p $(SERIAL_PORT) -b 460800 write_flash --flash_mode dio 0x1000 esp32*.bin + +serial: + picocom --baud 115200 $(SERIAL_PORT) diff --git a/README.md b/README.md index 82c5523..812a524 100755 --- a/README.md +++ b/README.md @@ -2,12 +2,16 @@ The software that is running on the SumoRobots +Code + # Instructions * Change the SERIAL_PORT in the Makefile +* Add your WiFi networks to the config.json file * Install [Python](https://www.python.org/downloads/) -* Install [esptool](https://github.com/espressif/esptool) (to flash SumoFirmware to the SumoRobot) -* Download the [SumoFirmware](https://github.com/robokoding/sumorobot-firmware/releases) to this directory -* Upload the SumoFirmware to your SumoRobot (open a terminal and type: make all) +* Install [esptool](https://github.com/espressif/esptool) (to flash MicroPython to the ESP32) +* Install [ampy](https://github.com/adafruit/ampy) (for uploading files) +* Download [the MicroPython binary](http://micropython.org/download#esp32) to this directory +* Upload the MicroPython binary and the SumoRobot firmware to your ESP32 (open a terminal and type: make all) # Support If you find our work useful, please consider donating : ) diff --git a/boot.py b/boot.py new file mode 100755 index 0000000..b06a0c4 --- /dev/null +++ b/boot.py @@ -0,0 +1,5 @@ +from utime import sleep_ms + +# Give time to cancel boot script +print("Press Ctrl-C to stop boot script...") +sleep_ms(500) diff --git a/config.json b/config.json new file mode 100755 index 0000000..a64fdd4 --- /dev/null +++ b/config.json @@ -0,0 +1,17 @@ +{ + "status_led_pin": 5, + "battery_coeff": 2.25, + "sumorobot_name": "SumoRobot", + "firmware_timestamp": "2019.11.17 16:23:00", + "firmware_version": "1.0.0", + "left_servo_min_tuning": 1, + "left_servo_max_tuning": 100, + "right_servo_min_tuning": 1, + "right_servo_max_tuning": 100, + "sonar_threshold": 40, + "boot_code": "code.py", + "left_line_value": 1000, + "right_line_value": 1000, + "left_line_threshold": 1000, + "right_line_threshold": 1000 +} diff --git a/hal.py b/hal.py new file mode 100755 index 0000000..bc293e3 --- /dev/null +++ b/hal.py @@ -0,0 +1,282 @@ +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 diff --git a/main.py b/main.py new file mode 100755 index 0000000..3cdf175 --- /dev/null +++ b/main.py @@ -0,0 +1,173 @@ +import os +import ujson +import struct +import _thread +import ubluetooth +from machine import Timer +from micropython import const + +from hal import * +# Loading libraries takes ca 400ms + +# BLE events +_IRQ_CENTRAL_CONNECT = const(1) +_IRQ_CENTRAL_DISCONNECT = const(2) +_IRQ_GATTS_READ_REQUEST = const(4) + +# Open and parse the config file +with open("config.json", "r") as config_file: + config = ujson.load(config_file) + +# Initialize the SumoRobot object +sumorobot = Sumorobot(config) + +# Advertise BLE name (SumoRobot name) +def advertise_ble_name(name): + ble_name = bytes(name, 'ascii') + ble_name = bytearray((len(ble_name) + 1, 0x09)) + ble_name + ble.gap_advertise(100, bytearray('\x02\x01\x02') + ble_name) + +def update_battery_level(timer): + if conn_handle is not None: + battery_level = sumorobot.get_battery_level() + ble.gatts_notify(conn_handle, battery, bytes([battery_level])) + +# The code processing thread +def process(): + global prev_bat_level, python_code + + while True: + # Leave time to process other code + sleep_ms(50) + # Execute to see LED feedback for sensors + sumorobot.update_sensor_feedback() + + # When no code to execute + if python_code == b'': + continue + + # Try to execute the Python code + try: + python_code = compile(python_code, "snippet", "exec") + exec(python_code) + except: + print("main.py: the code sent had errors") + finally: + print("main.py: finized execution") + # Erase the code + python_code = b'' + # Stop the robot + sumorobot.move(STOP) + # Cancel code termination + sumorobot.terminate = False + +# The BLE handler thread +def ble_handler(event, data): + global conn_handle, python_code, temp_python_code + + if event is _IRQ_CENTRAL_CONNECT: + conn_handle, _, _, = data + # Turn ON the status LED + sumorobot.set_led(STATUS, True) + update_battery_level(None) + elif event is _IRQ_CENTRAL_DISCONNECT: + conn_handle = None + # Turn OFF status LED + sumorobot.set_led(STATUS, False) + # Advertise with name + advertise_ble_name(sumorobot.config['sumorobot_name']) + elif event is _IRQ_GATTS_READ_REQUEST: + # Read the command + cmd = ble.gatts_read(rx) + + if b'' in cmd: + python_code = b'' + sumorobot.move(STOP) + sumorobot.terminate = True + elif b'' in cmd: + python_code = b'' + sumorobot.move(FORWARD) + elif b'' in cmd: + python_code = b'' + sumorobot.move(BACKWARD) + elif b'' in cmd: + python_code = b'' + sumorobot.move(LEFT) + elif b'' in cmd: + python_code = b'' + sumorobot.move(RIGHT) + elif b'' in cmd: + print(sumorobot.get_sensor_scope()) + ble.gatts_notify(conn_handle, tx, sumorobot.get_sensor_scope()) + elif b'' in cmd: + temp_python_code = b'\n' + elif b'' in cmd: + python_code = temp_python_code + temp_python_code = b'' + elif temp_python_code != b'': + temp_python_code += cmd + else: + temp_python_code = b'' + print('main.py: unknown cmd=', cmd) + +conn_handle = None +temp_python_code = b'' +python_code = b'' + +# When user code (code.py) exists +if 'code.py' in os.listdir(): + print('main.py: trying to load code.py') + # Try to load the user code + try: + with open("code.py", "r") as code: + python_code = compile(code.read(), "snippet", "exec") + except: + print("main.py: code.py compilation failed") + +# Start BLE +ble = ubluetooth.BLE() +ble.active(True) + +# Register the BLE hander +ble.irq(ble_handler) + +# BLE info serivce +INFO_SERVICE_UUID = ubluetooth.UUID(0x180a) +MODEL_CHARACTERISTIC = (ubluetooth.UUID(0x2a24), ubluetooth.FLAG_READ,) +FIRMWARE_CHARACTERISTIC = (ubluetooth.UUID(0x2a26), ubluetooth.FLAG_READ,) +MANUFACTURER_CHARACTERISTIC = (ubluetooth.UUID(0x2a29), ubluetooth.FLAG_READ,) +INFO_SERVICE = (INFO_SERVICE_UUID, (MODEL_CHARACTERISTIC, FIRMWARE_CHARACTERISTIC, MANUFACTURER_CHARACTERISTIC,),) + +# BLE battery service +BATTERY_SERVICE_UUID = ubluetooth.UUID(0x180f) +BATTERY_CHARACTERISTIC = (ubluetooth.UUID(0x2a19), ubluetooth.FLAG_READ | ubluetooth.FLAG_NOTIFY,) +BATTERY_SERVICE = (BATTERY_SERVICE_UUID, (BATTERY_CHARACTERISTIC,),) + +# BLE UART service +UART_SERVICE_UUID = ubluetooth.UUID('6E400001-B5A3-F393-E0A9-E50E24DCCA9E') +RX_CHARACTERISTIC = (ubluetooth.UUID('6E400002-B5A3-F393-E0A9-E50E24DCCA9E'), ubluetooth.FLAG_WRITE,) +TX_CHARACTERISTIC = (ubluetooth.UUID('6E400003-B5A3-F393-E0A9-E50E24DCCA9E'), ubluetooth.FLAG_READ | ubluetooth.FLAG_NOTIFY,) +UART_SERVICE = (UART_SERVICE_UUID, (TX_CHARACTERISTIC, RX_CHARACTERISTIC,),) + +# Register BLE services +SERVICES = (INFO_SERVICE, BATTERY_SERVICE, UART_SERVICE,) +((model, firmware, manufacturer,), (battery,), (tx, rx,),) = ble.gatts_register_services(SERVICES) + +# Set BLE info service values +ble.gatts_write(model, "SumoRobot") +ble.gatts_write(manufacturer, "RoboKoding LTD") +ble.gatts_write(firmware, sumorobot.config['firmware_version']) + +# Start BLE advertising with name +advertise_ble_name(sumorobot.config['sumorobot_name']) + +# Start the code processing thread +_thread.start_new_thread(process, ()) + +# Start BLE battery percentage update timer +battery_timer = Timer(Timer.PERIODIC) +battery_timer.init(period=3000, callback=update_battery_level) + +# Clean up +import gc +gc.collect() diff --git a/platformio.ini b/platformio.ini deleted file mode 100644 index 3458800..0000000 --- a/platformio.ini +++ /dev/null @@ -1,17 +0,0 @@ -;PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -[env:lolin32] -platform = espressif32 -board = lolin32 -framework = arduino -upload_speed = 460800 -monitor_speed = 115200 -lib_deps = NewPing \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 3b30529..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,396 +0,0 @@ -/* - This the code that runs on the SumoRobots -*/ -// Include BLE libraries -#include -#include -#include -#include -#include -// Include other libraries -#include -#include -#include -#include -#include - -#define DEBUG true - -#define VERSION "0.8.0" -#define VERSION_TIMESTAMP "2019.08.13 08:00" - -// See the following for generating UUIDs: -// https://www.uuidgenerator.net/ -#define NUS_SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // NUS service UUID -#define NUS_CHARACTERISTIC_RX_UUID "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" -#define NUS_CHARACTERISTIC_TX_UUID "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" - -// Cleate BLE variables -BLEServer * bleServer = NULL; -bool deviceConnected = false; -bool oldDeviceConnected = false; -BLECharacteristic * nusTxCharacteristic; -BLECharacteristic * batteryLevelCharacteristic; - -// Create preferences persistence -Preferences preferences; - -// Create timers -Ticker sonarTimer; -Ticker batteryTimer; -Ticker connectionLedTimer; - -// Battery stuff -float batteryVoltage; -bool robotMoving = false; -uint8_t batteryLevel = 0; -uint8_t tempBatteryLevel = 0; - -// Sonar stuff -uint8_t sonarValue; -NewPing sonar(27, 14, 200); -uint8_t sonarThreshold = 40; - -// Line stuff -uint16_t leftLineValue; -uint16_t rightLineValue; -uint16_t leftLineValueField = 0; -uint16_t rightLineValueField = 0; -uint16_t leftLineThreshold = 1000; -uint16_t rightLineThreshold = 1000; - -// Other sensor stuff -uint8_t sensorValues[6]; -bool ledFeedbackEnabled = true; - -// Move command names -std::string cmdStop("stop"); -std::string cmdLeft("left"); -std::string cmdRight("right"); -std::string cmdForward("forward"); -std::string cmdBackward("backward"); -// Other command names -std::string cmdLed("led"); -std::string cmdLine("line"); -std::string cmdName("name"); -std::string cmdSonar("sonar"); -std::string cmdServo("servo"); -std::string cmdLedFeedback("ledf"); - -void setLed(char led, char value) { - // Convert the value to a HIGH or LOW - bool state = value == '1' ? HIGH : LOW; - if (led == 'c') { - // Connection status LED is opposite value - digitalWrite(5, !state); - } - else if (led == 's') { - digitalWrite(16, state); - } - else if (led == 'r') { - digitalWrite(12, state); - } - else if (led == 'l') { - digitalWrite(17, state); - } -} - -void setServo(char servo, int8_t speed) { - Serial.println(speed); - if (servo == 'l') { - ledcWrite(1, map(speed, -100, 100, 1, 100)); - } - else if (servo == 'r') { - ledcWrite(2, map(speed, -100, 100, 1, 30)); - } -} - -void updateSensorFeedback() { - if (sonarValue <= sonarThreshold) { - digitalWrite(16, HIGH); - } - else { - digitalWrite(16, LOW); - } - if (abs(leftLineValue - leftLineValueField) > leftLineThreshold) { - digitalWrite(17, HIGH); - } - else { - digitalWrite(17, LOW); - } - if (abs(rightLineValue - rightLineValueField) > rightLineThreshold) { - digitalWrite(12, HIGH); - } - else { - digitalWrite(12, LOW); - } -} - -void updateSonarValue() { - // Update the sensor values - sonarValue = sonar.ping_cm(); - // When we didn't receive a ping back - // set to max distance - if (sonarValue == 0) sonarValue = 255; - leftLineValue = analogRead(34); - rightLineValue = analogRead(33); - if (ledFeedbackEnabled) updateSensorFeedback(); - sensorValues[0] = sonarValue; - sensorValues[1] = leftLineValue >> 8; - sensorValues[2] = leftLineValue; - sensorValues[3] = rightLineValue >> 8; - sensorValues[4] = rightLineValue; - sensorValues[5] = digitalRead(25); - // When BLE is connected - if (deviceConnected) { - // Notify the new sensor values - nusTxCharacteristic->setValue(sensorValues, 6); - nusTxCharacteristic->notify(); - } -} - -void updateBatteryLevel() { - // Don't update battery level when robot is moving - // the servo motors lower the battery voltage - // TODO: wait still a little more after moving - // for the voltage to settle - if (robotMoving) { - return; - } - - // Calculate the battery voltage - batteryVoltage = 2.12 * (analogRead(32) * 3.3 / 4096); - // Calculate battery percentage - tempBatteryLevel = 0.0 + ((100.0 - 0.0) / (4.2 - 3.2)) * (batteryVoltage - 3.2); - // When battery level changed more than 3% - if (abs(batteryLevel - tempBatteryLevel) > 3) { - // Update battery level - batteryLevel = tempBatteryLevel; - } - // Notify the new battery level - batteryLevelCharacteristic->setValue(&batteryLevel, 1); - batteryLevelCharacteristic->notify(); -#if DEBUG - Serial.print(batteryVoltage); - Serial.print(" : "); - Serial.println(batteryLevel); -#endif -} - -void blinkConnectionLed() { - digitalWrite(5, LOW); - delay(20); - digitalWrite(5, HIGH); -} - -// BLE connect and disconnect callbacks -class MyServerCallbacks: public BLEServerCallbacks { - void onConnect(BLEServer * pServer) { - deviceConnected = true; - }; - - void onDisconnect(BLEServer * pServer) { - deviceConnected = false; - } -}; - -// BLE NUS received callback -class MyCallbacks: public BLECharacteristicCallbacks { - void onWrite(BLECharacteristic * nusRxCharacteristic) { - // Get the received command over BLE - std::string cmd = nusRxCharacteristic->getValue(); -#if DEBUG - Serial.println(cmd.c_str()); -#endif - if (cmd.length() > 0) { - //int speed = atoi(rxValue.c_str()); - //Serial.println(speed); - //ledcWrite(1, speed); // left 1 ... 100 - //ledcWrite(2, speed); // right 1 ... 30 - // Specify command - if (cmd == cmdForward) { - robotMoving = true; - ledcWrite(1, 100); - ledcWrite(2, 1); - } - else if (cmd == cmdBackward) { - robotMoving = true; - ledcWrite(1, 1); - ledcWrite(2, 30); - } - else if (cmd == cmdLeft) { - robotMoving = true; - ledcWrite(1, 1); - ledcWrite(2, 1); - } - else if (cmd == cmdRight) { - robotMoving = true; - ledcWrite(1, 100); - ledcWrite(2, 30); - } - else if (cmd == cmdStop) { - robotMoving = false; - ledcWrite(1, 0); - ledcWrite(2, 0); - } - else if (cmd == cmdLedFeedback) { - ledFeedbackEnabled = !ledFeedbackEnabled; - } - else if (cmd.find(cmdLed) != std::string::npos) { - setLed(cmd.at(3), cmd.at(4)); - } - else if (cmd.find(cmdLine) != std::string::npos) { - // Get the threshold value - leftLineThreshold = atoi(cmd.substr(4, cmd.length() - 4).c_str()); - rightLineThreshold = leftLineThreshold; - // Remember value on the field (white or black) - leftLineValueField = analogRead(34); - rightLineValueField = analogRead(33); - // Save the threshold value in the persistence - preferences.begin("sumorobot", false); - preferences.putUInt("line_threshold", leftLineThreshold); - preferences.end(); - } - else if (cmd.find(cmdSonar) != std::string::npos) { - sonarThreshold = atoi(cmd.substr(5, cmd.length() - 5).c_str()); - // Save the threshold value in the persistence - preferences.begin("sumorobot", false); - preferences.putUInt("sonar_threshold", sonarThreshold); - preferences.end(); - } - else if (cmd.find(cmdServo) != std::string::npos) { - setServo(cmd.at(5), atoi(cmd.substr(6, cmd.length() - 6).c_str())); - } - else if (cmd.find(cmdName) != std::string::npos) { - preferences.begin("sumorobot", false); - preferences.putString("name", cmd.substr(4, cmd.length() - 4).c_str()); - preferences.end(); - } - } - } -}; - -void setup() { -#if DEBUG - Serial.begin(115200); -#endif - - // Start preferences persistence - preferences.begin("sumorobot", false); - - // Create the BLE device - Serial.println(preferences.getString("name", "SumoRobot").c_str()); - BLEDevice::init(preferences.getString("name", "SumoRobot").c_str()); - - preferences.end(); - - // Create the BLE server - bleServer = BLEDevice::createServer(); - bleServer->setCallbacks(new MyServerCallbacks()); - - // Create device info service and characteristic - BLEService * deviceInfoService = bleServer->createService(BLEUUID((uint16_t) 0x180a)); - BLECharacteristic * modelCharacteristic = deviceInfoService->createCharacteristic( - (uint16_t) 0x2A24, BLECharacteristic::PROPERTY_READ); - BLECharacteristic * firmwareCharacteristic = deviceInfoService->createCharacteristic( - (uint16_t) 0x2A26, BLECharacteristic::PROPERTY_READ); - BLECharacteristic * manufacturerCharacteristic = deviceInfoService->createCharacteristic( - (uint16_t) 0x2a29, BLECharacteristic::PROPERTY_READ); - manufacturerCharacteristic->setValue("RoboKoding LTD"); - modelCharacteristic->setValue("SumoRobot"); - firmwareCharacteristic->setValue(VERSION); - - // Create battery service - BLEService * batteryService = bleServer->createService(BLEUUID((uint16_t) 0x180f)); - // Mandatory battery level characteristic with notification and presence descriptor - BLE2904* batteryLevelDescriptor = new BLE2904(); - batteryLevelDescriptor->setFormat(BLE2904::FORMAT_UINT8); - batteryLevelDescriptor->setNamespace(1); - batteryLevelDescriptor->setUnit(0x27ad); - // Create battery level characteristics - batteryLevelCharacteristic = batteryService->createCharacteristic( - (uint16_t) 0x2a19, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY); - batteryLevelCharacteristic->addDescriptor(batteryLevelDescriptor); - batteryLevelCharacteristic->addDescriptor(new BLE2902()); - - // Create the BLE NUS service - BLEService * nusService = bleServer->createService(NUS_SERVICE_UUID); - - // Create a BLE NUS transmit characteristic - nusTxCharacteristic = nusService->createCharacteristic( - NUS_CHARACTERISTIC_TX_UUID, BLECharacteristic::PROPERTY_NOTIFY); - nusTxCharacteristic->addDescriptor(new BLE2902()); - - // Create a BLE NUS receive characteristics - BLECharacteristic * nusRxCharacteristic = nusService->createCharacteristic( - NUS_CHARACTERISTIC_RX_UUID, BLECharacteristic::PROPERTY_WRITE); - nusRxCharacteristic->setCallbacks(new MyCallbacks()); - - // Start the services - deviceInfoService->start(); - batteryService->start(); - nusService->start(); - - // Start advertising - bleServer->getAdvertising()->start(); - -#if DEBUG - Serial.println("Waiting a client connection to notify..."); -#endif - - // Setup BLE connection status LED - pinMode(5, OUTPUT); - connectionLedTimer.attach_ms(2000, blinkConnectionLed); - - // Setup the left servo PWM - ledcSetup(1, 50, 10); - ledcAttachPin(15, 1); - - // Setup the right servo PWM - ledcSetup(2, 50, 8); - ledcAttachPin(4, 2); - - // Phototransistor pull-ups - pinMode(19, INPUT_PULLUP); - pinMode(23, INPUT_PULLUP); - - // Setup battery charge detection pin - pinMode(25, INPUT); - - // Setup sensor feedback LED pins - pinMode(16, OUTPUT); - pinMode(17, OUTPUT); - pinMode(12, OUTPUT); - - // Setup ADC for reading phototransistors and battery - analogSetAttenuation(ADC_11db); - adcAttachPin(32); - adcAttachPin(33); - adcAttachPin(34); - - // Setup sonar timer to update it's value - sonarTimer.attach_ms(50, updateSonarValue); - - // Setup battery level timer to update it's value - batteryTimer.attach(5, updateBatteryLevel); - updateBatteryLevel(); -} - -void loop() { - // When BLE got disconnected - if (!deviceConnected && oldDeviceConnected) { - delay(500); // Give the bluetooth stack the chance to get things ready - bleServer->startAdvertising(); // Restart advertising -#if DEBUG - Serial.println("start advertising"); -#endif - oldDeviceConnected = deviceConnected; - connectionLedTimer.attach_ms(2000, blinkConnectionLed); - } - // When BLE got connected - if (deviceConnected && !oldDeviceConnected) { - oldDeviceConnected = deviceConnected; - connectionLedTimer.detach(); - digitalWrite(5, LOW); - } -} \ No newline at end of file