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
+
+
# 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