Skip to content
Snippets Groups Projects
Commit 0b4fbbe2 authored by Václav Jelínek's avatar Václav Jelínek
Browse files

Add ESP option to switch between BT and WiFi

parent d7432d30
No related branches found
No related tags found
1 merge request!7Merge old hardware code with new hardware code
Hide whitespace changes
Inline Side-by-side
ESP/uart/uart.ino
+ 267
− 71
View file @ 0b4fbbe2
#include <BTAdvertisedDevice.h>
#include <BTScan.h>
#include <BTAddress.h>
#include <BluetoothSerial.h>
#include <WiFi.h>
......@@ -20,6 +17,10 @@
#define COM_RESET 0b10101111
#define COM_ACK 0b10110000
#define COM_NACK 0b10110001
#define COM_BT_PAIR 0b0001000
#define COM_BT_CANCEL 0b0001001
#define COM_BT_SUCCESS 0b0001011
#define COM_WIFI 0b0001010
#define ACK 0
#define NACK 1
......@@ -28,20 +29,48 @@
#define CMD_PIN 4
#define LED_PIN 2
#define STOP 0
#define UP 1
#define DOWN 2
#define LEFT 3
#define RIGHT 4
BluetoothSerial SerialBT;
//const char *pin = "1234";
IPAddress local_ip(192, 168, 1, 1);
IPAddress gateway(192, 168, 1, 1);
IPAddress subnet(255, 255, 255, 0);
WiFiServer server(80);
WiFiClient client;
char default_name[] = "OPEN-CUBE";
uint8_t message_rx[MAX_PAYLOAD + 3];
uint8_t message_tx[MAX_PAYLOAD + 3];
char esp_name[MAX_PAYLOAD];
int message_idx = 0;
uint8_t ch;
int cmd_pin = 0;
int cmd_pin_prev = 0;
bool cmd_mode = true;
bool bt_on = false;
bool bt_on = true;
bool startup = true;
uint32_t bt_pin = 0;
bool bt_success = false;
boolean confirmRequestPending = false;
//wifi
String request;
String s_stop;
String s_up;
String s_down;
String s_left;
String s_right;
int dir;
int counter;
uint8_t calculate_message_checksum(const uint8_t *buffer, size_t length) {
uint8_t csum = 0xFF;
for (size_t i = 0; i < length; i++) {
......@@ -64,6 +93,26 @@ void send_short_cmd(uint8_t command) {
Serial.write(message_tx, 3);
}
bool check_commands(uint8_t command) {
switch (command) {
case COM_NAME:
case COM_PASSWORD:
case COM_BT_PIN:
case COM_RESET:
case COM_ACK:
case COM_NACK:
case COM_BT_PAIR:
case COM_BT_CANCEL:
case COM_WIFI:
case COM_BT_SUCCESS:
return true;
break;
default:
return false;
}
}
void BTConfirmRequestCallback(uint32_t numVal)
{
confirmRequestPending = true;
......@@ -72,100 +121,247 @@ void BTConfirmRequestCallback(uint32_t numVal)
void BTAuthCompleteCallback(boolean success)
{
bt_success = success;
confirmRequestPending = false;
if (success)
}
void html () {
client.println("HTTP/1.1 200 OK");
client.println("Content-Type: text/html");
client.println("Connection: close");
client.println();
client.println("<!DOCTYPE HTML>");
client.println("<html>");
client.println("<head>");
client.println("<meta name=\"viewport\" content=\"width=device-width, initial-scale=1\">");
client.println("<link rel=\"icon\" href=\"data:,\">");
client.println("<style>");
client.println("html { font-family: Roboto; display: inline-block; margin: 0px auto; text-align: center;}");
client.println(".button {background-color: #4CAF50; border: none; color: white; width: 100px; height:100px; padding: 0px; text-align: center; text-decoration: none; display: inline-block; font-size: 16px; margin: 4px 2px; cursor: pointer;");
client.println("text-decoration: none; font-size: 25px; margin: 2px; cursor: pointer;}");
client.println(".button_ON {background-color: #6AFB92l; color: black; border: 6px solid #4CAF50;}");
client.println(".button_OFF {background-color: white; color: black; border: 6px solid #f44336;}");
client.println("</style>");
client.println("</head>");
client.println("<body>");
client.println("<h2>Open-Cube segway control</h2>");
s_stop = "OFF";
s_up = "OFF";
s_down = "OFF";
s_left = "OFF";
s_right = "OFF";
if (dir == STOP)
{
send_short_cmd(COM_ACK);
s_stop = "ON";
}
else
if (dir == UP)
{
send_short_cmd(COM_NACK);
s_up = "ON";
}
if (dir == DOWN)
{
s_down = "ON";
}
if (dir == LEFT)
{
s_left = "ON";
}
if (dir == RIGHT)
{
s_right = "ON";
}
client.printf("<p><a href=\"/UP\n\"><button class=\"button button_%s\">&uarr;</button></a></p>", s_up);
client.printf("<p><a href=\"/LEFT\n\"><button class=\"button button_%s\">&larr;</button></a><a href=\"/STOP\n\"><button class=\"button button_%s\">STOP</button></a><a href=\"/RIGHT\n\"><button class=\"button button_%s\">&rarr;</button></a></p>", s_left, s_stop, s_right);
client.printf("<p><a href=\"/DOWN\n\"><button class=\"button button_%s\">&darr;</button></a></p>", s_down);
client.println("</body>");
client.println("</html>");
}
void setup() {
Serial.begin(115200);
pinMode(CMD_PIN, INPUT);
pinMode(LED_PIN, OUTPUT);
cmd_pin = digitalRead(CMD_PIN);
cmd_pin_prev = cmd_pin;
//SerialBT.setPin(pin);
memset(esp_name, '\0', sizeof(esp_name));
memcpy(esp_name, &default_name, 9);
SerialBT.enableSSP();
SerialBT.onConfirmRequest(BTConfirmRequestCallback);
SerialBT.onAuthComplete(BTAuthCompleteCallback);
SerialBT.begin("OPEN-CUBEn"); //Bluetooth device name
SerialBT.begin(esp_name); //Bluetooth device name
dir = STOP;
counter = 0;
}
void loop() {
cmd_pin = digitalRead(CMD_PIN);
digitalWrite(LED_PIN, cmd_pin);
if (SerialBT.available() && !cmd_pin) {
ch = SerialBT.read();
SerialBT.write(ch);
Serial.write(ch);
if (cmd_pin && cmd_pin_prev == 0) {
send_short_cmd(COM_ACK);
message_idx = 0;
}
if (cmd_pin == 0 && cmd_pin_prev) {
send_short_cmd(COM_ACK);
message_idx = 0;
}
cmd_pin_prev = cmd_pin;
if (cmd_pin) {
if (Serial.available()) {
ch = Serial.read();
if (cmd_pin) {
message_rx[message_idx] = ch;
if (message_idx == 0) {
if (check_commands(message_rx[0])) {
message_idx++;
}
} else if (message_idx == 1) {
message_idx++;
} else if (message_idx == message_rx[1] + 2) {
message_idx = 0;
if (calculate_message_checksum(message_rx, message_rx[1] + 3) == 0) {
switch (message_rx[0]) {
case COM_NAME:
send_short_cmd(COM_ACK);
memset(esp_name, '\0', sizeof(esp_name));
memcpy(esp_name, &message_rx[2], message_rx[1]);
if (bt_on) {
SerialBT.end();
SerialBT.begin(esp_name); //Bluetooth device name
} else {
server.end();
WiFi.softAP(esp_name, "12345678");
server.begin();
}
while (Serial.available()) {
ch = Serial.read();
if (cmd_pin) {
message_rx[message_idx] = ch;
if (message_idx == 0) {
message_idx++;
} else if (message_idx == 1) {
message_idx++;
} else if (message_idx == message_rx[1] + 2) {
message_idx = 0;
if (calculate_message_checksum(message_rx, message_rx[1] + 3) == 0) {
//SerialBT.write(97);
switch (message_rx[0]) {
case COM_NAME:
send_short_cmd(COM_ACK);
message_rx[2 + message_rx[1]] = 0;
SerialBT.begin(message_rx[2]); //Bluetooth device name
break;
case COM_PASSWORD:
send_short_cmd(COM_ACK);
break;
case COM_RESET:
send_short_cmd(COM_ACK);
SerialBT.end();
break;
case COM_ACK:
if (confirmRequestPending) {
SerialBT.confirmReply(true);
}
break;
case COM_NACK:
if (confirmRequestPending) {
SerialBT.confirmReply(false);
}
break;
case COM_BT_PIN:
SerialBT.write(111);
if (confirmRequestPending) {
send_bt_pin();
} else {
send_short_cmd(COM_NACK);
}
break;
default:
break;
break;
case COM_PASSWORD:
send_short_cmd(COM_ACK);
break;
case COM_RESET:
send_short_cmd(COM_ACK);
bt_on = true;
server.end();
SerialBT.end();
SerialBT.begin(esp_name); //Bluetooth device name
break;
case COM_ACK:
break;
case COM_NACK:
break;
case COM_BT_PIN:
if (confirmRequestPending) {
send_bt_pin();
} else {
send_short_cmd(COM_NACK);
}
break;
case COM_BT_PAIR:
if (confirmRequestPending) {
SerialBT.confirmReply(true);
send_short_cmd(COM_ACK);
}
break;
case COM_BT_CANCEL:
if (confirmRequestPending) {
SerialBT.confirmReply(false);
send_short_cmd(COM_ACK);
}
break;
case COM_BT_SUCCESS:
if (bt_success) {
send_short_cmd(COM_ACK);
} else {
send_short_cmd(COM_NACK);
}
break;
case COM_WIFI:
bt_on = false;
SerialBT.end();
WiFi.softAP(esp_name, "12345678");
WiFi.softAPConfig(local_ip, gateway, subnet);
IPAddress myIP = WiFi.softAPIP();
server.begin();
break;
}
//SerialBT.write(10);
} else {
//SerialBT.write(110);
send_short_cmd(COM_NACK);
}
SerialBT.write(10);
} else {
//SerialBT.write(110);
send_short_cmd(COM_NACK);
//SerialBT.write(ch);
message_idx++;
}
} else {
}
}
} else {
if (bt_on) {
if (SerialBT.available()) {
ch = SerialBT.read();
SerialBT.write(ch);
message_idx++;
Serial.write(ch);
}
} else { //data mode
SerialBT.write(ch);
if (Serial.available()) {
ch = Serial.read();
SerialBT.write(ch);
}
} else {
client = server.available();
if (!client)
{
return;
}
while (client.connected())
{
if (client.available())
{
char c = client.read();
request += c;
if (c == '\n')
{
if (request.indexOf("GET /STOP") != -1)
{
dir = STOP;
}
if (request.indexOf("GET /UP") != -1)
{
dir = UP;
}
if (request.indexOf("GET /DOWN") != -1)
{
dir = DOWN;
}
if (request.indexOf("GET /LEFT") != -1)
{
dir = LEFT;
}
if (request.indexOf("GET /RIGHT") != -1)
{
dir = RIGHT;
}
html();
break;
}
}
}
request = "";
client.stop();
}
}
}
lib/cube/esp_config.py
+ 61
− 35
View file @ 0b4fbbe2
......@@ -5,22 +5,27 @@ import struct
from lib.hw_defs.pins import INTERNAL_UART_HW_ID, INTERNAL_UART_TX_PIN, INTERNAL_UART_RX_PIN
MAX_PAYLOAD = const(255)
class Esp:
MAX_PAYLOAD = const(255)
COM_RESET = const(0b10101111)
COM_ACK = const(0b10110000)
COM_NACK = const(0b10110001)
COM_NAME = const(0b00000011)
COM_PASSWORD = const(0b0000010)
COM_BT_PIN = const(0b0000111)
COM_RESET = const(0b10101111)
COM_ACK = const(0b10110000)
COM_NACK = const(0b10110001)
COM_NAME = const(0b00000011)
COM_PASSWORD = const(0b0000010)
COM_BT_PIN = const(0b0000111)
COM_BT_PAIR = const(0b0001000)
COM_BT_CANCEL = const(0b0001001)
COM_BT_SUCCESS = const(0b0001011)
COM_WIFI = const(0b0001010)
ACK = const(0)
NACK = const(1)
TIMEOUT = const(2)
NACK_ATTEMPTS = const(5)
ACK = const(0)
NACK = const(1)
TIMEOUT = const(2)
NACK_ATTEMPTS = const(5)
class Esp:
def __init__(self, baud_rate=115200):
self.uart = UART(INTERNAL_UART_HW_ID, baudrate=baud_rate, bits=8, parity=None, stop=1,
tx=Pin(INTERNAL_UART_TX_PIN), rx=Pin(INTERNAL_UART_RX_PIN), flow=0, invert=0)
......@@ -34,7 +39,19 @@ class Esp:
self.payload = bytes()
self.new_message = False
self.esp_running = False
self.not_responding = 0
def timeout(self):
self.not_responding += 1
if self.not_responding > 5:
self.esp_running = False
self.not_responding = 0
else:
self.flush()
def running(self):
return self.esp_running
def flush(self):
while self.uart.any():
self.uart.read()
......@@ -57,27 +74,28 @@ class Esp:
while utime.ticks_diff(utime.ticks_ms(), time) < timeout:
utime.sleep_us(100)
ack = self.ack_received()
if ack != TIMEOUT:
if ack != Esp.TIMEOUT:
return ack
return TIMEOUT
return Esp.TIMEOUT
def ack_received(self):
if self.new_message:
if self.header == COM_ACK:
if self.header == Esp.COM_ACK:
self.new_message = False
return ACK
if self.header == COM_NACK:
return Esp.ACK
if self.header == Esp.COM_NACK:
self.new_message = False
return NACK
return Esp.NACK
else:
return TIMEOUT
return Esp.TIMEOUT
def reset(self, timeout=2000):
self.flush()
time = utime.ticks_ms()
while utime.ticks_diff(utime.ticks_ms(), time) < timeout:
self.send_reset()
utime.sleep_ms(50)
if self.ack_received() == ACK:
if self.ack_received() == Esp.ACK:
self.esp_running = True
return True
self.esp_running = False
......@@ -98,41 +116,49 @@ class Esp:
def req_bt_pin(self, timeout=100):
time = utime.ticks_ms()
self.send_message(self.i2b(COM_BT_PIN)+self.i2b(0))
self.send_message(self.i2b(Esp.COM_BT_PIN)+self.i2b(0))
while utime.ticks_diff(utime.ticks_ms(), time) < timeout:
utime.sleep_us(100)
if self.ack_received() == NACK:
utime.sleep_ms(1)
if self.ack_received() == Esp.NACK:
return None
if self.new_message and self.header == COM_BT_PIN:
if self.new_message and self.header == Esp.COM_BT_PIN:
self.new_message = False
return struct.unpack('<I', self.payload)[0]
return None
return -1
def send_reset(self):
self.send_message(self.i2b(COM_RESET)+self.i2b(0))
self.send_message(self.i2b(Esp.COM_RESET)+self.i2b(0))
def send_ack(self):
self.send_message(self.i2b(COM_ACK)+self.i2b(0))
self.send_message(self.i2b(Esp.COM_ACK)+self.i2b(0))
def send_nack(self):
self.send_message(self.i2b(COM_NACK)+self.i2b(0))
self.send_message(self.i2b(Esp.COM_NACK)+self.i2b(0))
def set_name(self, name: str, timeout=500):
return self.repeat_str(COM_NAME, name, timeout)
return self.repeat_str(Esp.COM_NAME, name, timeout)
def set_password(self, password: str, timeout=500):
return self.repeat_str(COM_PASSWORD, password, timeout)
def repeat_str(self, command: int, str: str, timeout=100):
for i in range(NACK_ATTEMPTS):
return self.repeat_str(Esp.COM_PASSWORD, password, timeout)
def repeat_cmd(self, command: int, timeout=50):
for i in range(Esp.NACK_ATTEMPTS):
self.send_message(self.i2b(command)+self.i2b(0))
receive = self.wait_for_ack(timeout)
if receive != Esp.TIMEOUT:
return receive
return Esp.TIMEOUT
def repeat_str(self, command: int, str: str, timeout=50):
for i in range(Esp.NACK_ATTEMPTS):
self.send_str(command, str)
if self.wait_for_ack(timeout) == ACK:
if self.wait_for_ack(timeout) == Esp.ACK:
return True
return False
def send_str(self, command: int, str: str):
str_len = len(str)
if str_len > MAX_PAYLOAD:
if str_len > Esp.MAX_PAYLOAD:
return
message = self.i2b(command)
message += self.i2b(str_len)
......
main/main.py
+ 85
− 43
View file @ 0b4fbbe2
......@@ -31,7 +31,6 @@ def main():
robot.display.text(program_list[current_program][0][:-3], 0, 55, 1)
else:
robot.display.text("<brick is empty>", 0, 55, 1)
robot.display.show()
# Show program error on display
def display_show_error(robot):
......@@ -46,13 +45,24 @@ def main():
robot.display.text('on', 40, 30, 1)
robot.display.show()
def display_show_bt_pin(robot, pin):
robot.display.fill(0)
robot.display.text('Pair with', 35, 20, 1)
robot.display.text('device', 35, 30, 1)
robot.display.text('{:6d}'.format(pin), 35, 40, 1)
robot.display.show()
def display_show_bt_pin(robot, pin, connecting):
robot.display.rect(6,6,116,52, 1)
robot.display.rect(7,7,114,50, 1)
robot.display.fill_rect(8,8,112,48, 0)
if connecting:
robot.display.centered_text('connecting', 25, 1)
else:
robot.display.centered_text('pair with', 10, 1)
##robot.display.text('device', 10, 30, 1)
robot.display.centered_text(f'{pin:06d}', 20, 1)
robot.display.centered_text('?', 30, 1)
robot.display.text('no', 10, 44, 1)
robot.display.text('ok', 100,44, 1)
def esp_command(robot):
robot.buttons.set_pin(8, False)
utime.sleep(0.05)
# Analyze pressed buttons for menu movement
def buttons(robot, button_values, robotState, menu_move, menu_debounce):
ok_pressed = 0
......@@ -76,17 +86,35 @@ def main():
menu_debounce = 0
return menu_move, menu_debounce, ok_pressed
display_show_startup(robot)
def send_esp_req(robot):
esp_command(robot)
pin = robot.esp.req_bt_pin()
robot.esp.flush()
robot.buttons.set_pin(8, True)
print("Pin: ", pin)
if pin is not None and pin != -1:
bt_pair = True
else:
bt_pair = False
if pin == -1:
robot.esp.flush()
robot.esp.timeout()
return bt_pair, pin
# Initialize variables
robotState = 0
menu_move = 0
menu_debounce = 0
current_program = 0
counter = 0
rejected_pin = 0
bt_pair = False
button_values = robot.buttons.pressed()
robot.esp_command = True
bt_connecting = False
display_show_startup(robot)
# Get a list of user programs
program_list = []
try:
......@@ -105,47 +133,59 @@ def main():
os.mkdir("./programs")
program_count = len(program_list)
robot.esp_command = True
robot.buttons.set_pin(8, False)
robot.buttons.set_pin(14, False)
print("Reset: ", robot.esp.reset())
print("Name: ", robot.esp.set_name("Open-Cube5"))
print("Password: ",robot.esp.set_password("1234"))
robot.buttons.set_pin(14, True)
robot.buttons.set_pin(8, True)
#robot.esp_command = False
#robot.buttons.set_pin(8, False)
# Loop that shows display, controls menu and starts user programs
while(True):
robot.buttons.set_pin(8, False)
utime.sleep_us(1000)
pin = robot.esp.req_bt_pin()
print("Pin: ", pin)
robot.buttons.set_pin(8, True)
if pin is not None:
bt_pair = True
else:
bt_pair = False
if not robot.esp.running() and counter > 0 and counter % 20 == 0:
robot.buttons.set_pin(14, False)
esp_command(robot)
reset = robot.esp.reset(100)
print("Reset: ", reset)
robot.esp.flush()
robot.buttons.set_pin(14, True)
if reset:
print("Name: ", robot.esp.set_name("Open-Cube5",300))
robot.esp.flush()
robot.buttons.set_pin(8, True)
bat_voltage = robot.battery.voltage() # Get current battery voltage
button_values = robot.buttons.pressed() # Get buttons state
if robot.esp.running() and counter % 20 == 0:
bt_pair, pin = send_esp_req(robot)
if pin != None and pin == rejected_pin:
bt_pair = False
if not bt_pair:
bt_connecting = False
if not bt_pair:
menu_move, menu_debounce, ok_pressed = buttons(robot, button_values, robotState,
menu_move, menu_debounce)
display_show_menu(robot, bat_voltage, program_list, current_program)
if bt_pair:
display_show_bt_pin(robot, pin)
if button_values[Button.OK]:
robot.buttons.set_pin(8, False)
robot.esp.send_ack()
display_show_bt_pin(robot, pin, bt_connecting)
if button_values[Button.RIGHT]:
esp_command(robot)
print("pair", robot.esp.repeat_cmd(robot.esp.COM_BT_PAIR, 300)==robot.esp.ACK)
robot.esp.flush()
robot.buttons.set_pin(8, True)
bt_connecting = True
elif button_values[Button.LEFT]:
robot.buttons.set_pin(8, False)
robot.esp.send_nack()
rejected_pin = pin
esp_command(robot)
print("cancel", robot.esp.repeat_cmd(robot.esp.COM_BT_CANCEL, 300)==robot.esp.ACK)
robot.esp.flush()
robot.buttons.set_pin(8, True)
robot.display.show()
counter += 1
utime.sleep(0.05)
continue
else:
display_show_menu(robot, bat_voltage, program_list, current_program)
menu_move, menu_debounce, ok_pressed = buttons(robot, button_values, robotState,
menu_move, menu_debounce)
robot.display.show()
# Check if buttons were pressed and move menu accordingly
if menu_move != 0 and menu_debounce == 0 and program_count > 0:
current_program += menu_move
......@@ -179,8 +219,10 @@ def main():
# Collect unreachable objects (this deinitializes EV3 motors & sensors)
gc.collect()
robotState = 0
robot.esp.esp_running = False
print("Program finished\n")
counter += 1
utime.sleep(0.05)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment