├── ble_help_functions.h ├── ble_help_functions.cpp ├── wedo_color_definitions.h ├── library.properties ├── backlog.md ├── keywords.txt ├── examples ├── button_motor │ └── button_motor.ino ├── sensor_motor │ └── sensor_motor.ino ├── two_sensors │ └── two_sensors.ino └── wifi_control │ └── wifi_control.ino ├── esp32_ble_wedo.h ├── ble_functions.h ├── README.md ├── esp32_ble_wedo.cpp └── ble_functions.cpp /ble_help_functions.h: -------------------------------------------------------------------------------- 1 | // This file is no longer needed with NimBLE-Arduino 2 | // All helper functions are now built into the NimBLE library 3 | // Kept for backwards compatibility but not used 4 | -------------------------------------------------------------------------------- /ble_help_functions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ble_help_func.cpp 3 | * 4 | * This file is no longer needed with NimBLE-Arduino 5 | * All helper functions are now built into the NimBLE library 6 | * Kept for backwards compatibility but not used 7 | */ 8 | -------------------------------------------------------------------------------- /wedo_color_definitions.h: -------------------------------------------------------------------------------- 1 | /* 2 | Index colors for the RGB wedo2.0 LED 3 | */ 4 | 5 | #define LEGO_COLOR_BLACK 0 6 | #define LEGO_COLOR_PINK 1 7 | #define LEGO_COLOR_PURPLE 2 8 | #define LEGO_COLOR_BLUE 3 9 | #define LEGO_COLOR_CYAN 4 10 | #define LEGO_COLOR_LIGHTGREEN 5 11 | #define LEGO_COLOR_GREEN 6 12 | #define LEGO_COLOR_YELLOW 7 13 | #define LEGO_COLOR_ORANGE 8 14 | #define LEGO_COLOR_RED 9 15 | #define LEGO_COLOR_WHITE 10 16 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=WEDO 2.0 BLE for ESP32 2 | version=2.0.0 3 | author=Geert Roumen 4 | maintainer=Geert Roumen 5 | sentence=A library that supports to use BLE to connect and control the wedo2.0 6 | paragraph=It has both support for sensors (tilt and detect) and actuators (motor, sound, led). Now using NimBLE-Arduino for improved performance and stability. 7 | category=Communication 8 | url=https://github.com/lemio/esp32_ble_wedo 9 | architectures=esp32 10 | includes=esp32_ble_wedo.h 11 | depends=NimBLE-Arduino 12 | -------------------------------------------------------------------------------- /backlog.md: -------------------------------------------------------------------------------- 1 | ## Getting output devices working 2 | - motor 3 | - LED 4 | - piezo 5 | 6 | ## Getting sensor working 7 | - notify system works (with the button on the wedo) 8 | - putting the commands directly on the nRF connect works 9 | - info from https://github.com/vheun/wedo2/blob/master/index.js 10 | - register for notify to 1560 11 | - set 1563 to 0x01 02 02 23 00 01 00 00 00 00 01 12 | - you get data in the range 02-02-00 -> 02-02-0A 13 | - printing the exact sequence in the Serial monitor 14 | - Found error, only prints the first 5 uint8_t's of the array 15 | - Probably some pointer issue 16 | - Also explains the piezo frequency issues 17 | - Try to do some sizeof's in the program 18 | - Solved the issue by sending a size with every function that receieves an array 19 | || swopping input and output in the search for characteristic function (already solved the issue) 20 | - passing a callbackHandler to the BLE stack (for notifications) 21 | 22 | - detect Sensor 23 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | Wedo KEYWORD1 2 | 3 | connect KEYWORD2 4 | connected KEYWORD2 5 | ready KEYWORD2 6 | 7 | #All the write functions 8 | 9 | writeOutputCommand KEYWORD2 10 | writeInputCommand KEYWORD2 11 | writeMotor KEYWORD2 12 | writeIndexColor KEYWORD2 13 | writeSound KEYWORD2 14 | writeRGB KEYWORD2 15 | writePortDefinition KEYWORD2 16 | 17 | #All the set functions 18 | 19 | setRGBMode KEYWORD2 20 | setIndexMode KEYWORD2 21 | setTiltSensor KEYWORD2 22 | setDetectSensor KEYWORD2 23 | 24 | #All the LEGO colors 25 | 26 | LEGO_COLOR_BLACK LITERAL1 27 | LEGO_COLOR_PINK LITERAL1 28 | LEGO_COLOR_PURPLE LITERAL1 29 | LEGO_COLOR_BLUE LITERAL1 30 | LEGO_COLOR_CYAN LITERAL1 31 | LEGO_COLOR_LIGHTGREEN LITERAL1 32 | LEGO_COLOR_GREEN LITERAL1 33 | LEGO_COLOR_YELLOW LITERAL1 34 | LEGO_COLOR_ORANGE LITERAL1 35 | LEGO_COLOR_RED LITERAL1 36 | LEGO_COLOR_WHITE LITERAL1 37 | 38 | #All the other constants 39 | 40 | ID_MOTOR LITERAL1 41 | ID_TILT_SENSOR LITERAL1 42 | ID_DETECT_SENSOR LITERAL1 43 | 44 | RANGE_RAW LITERAL1 45 | RANGE_10 LITERAL1 46 | RANGE_100 LITERAL1 47 | -------------------------------------------------------------------------------- /examples/button_motor/button_motor.ino: -------------------------------------------------------------------------------- 1 | /* 2 | @Author Geert Roumen 3 | @Date 5-6-2017 4 | @Hardware 5 | https://www.analoglamb.com/product/esp32-development-board/ 6 | @library 7 | https://github.com/lemio/esp32_ble_wedo 8 | @Status 9 | Working (but probably sub-optimal/stable) 10 | @licence 11 | https://creativecommons.org/licenses/by-sa/4.0/ 12 | 13 | if you look in front of the WEDO ports, 14 | so the back of the wedo, on port 1 there 15 | is a LEGO wedo2.0 motor connected 16 | _________________ 17 | | port2 | port1 | 18 | |________|________| 19 | | | 20 | | | 21 | |_________________| 22 | */ 23 | 24 | #include 25 | 26 | Wedo myWedo("test"); 27 | 28 | void setup() { 29 | // put your setup code here, to run once: 30 | Serial.begin(115200); 31 | myWedo.connect(); 32 | //Wait untill the wedo is connected to the ESP32 33 | while (!myWedo.connected()){ 34 | Serial.print("."); 35 | delay(100); 36 | } 37 | } 38 | 39 | void loop() { 40 | // Handle automatic reconnection if connection is lost 41 | myWedo.handleConnection(); 42 | 43 | //read the value from the button (on pin 0) 44 | boolean value = !digitalRead(0); 45 | //invert that value (HIGH->false and LOW->true), 46 | //so that if the button is pressed (LOW) 47 | //the motor and LED will shine 48 | 49 | //convert the boolean to a value that makes sense 50 | //for the LED (white -> 10 in this case) 51 | myWedo.writeIndexColor(value*LEGO_COLOR_WHITE); 52 | //write a value to the motor on port 1 53 | myWedo.writeMotor(1,value*100); 54 | } 55 | -------------------------------------------------------------------------------- /examples/sensor_motor/sensor_motor.ino: -------------------------------------------------------------------------------- 1 | /* 2 | @Author Geert Roumen 3 | @Date 5-6-2017 4 | @Hardware 5 | https://www.analoglamb.com/product/esp32-development-board/ 6 | @library 7 | https://github.com/lemio/esp32_ble_wedo 8 | @Status 9 | Working (but probably sub-optimal/stable) 10 | @licence 11 | https://creativecommons.org/licenses/by-sa/4.0/ 12 | 13 | if you look in front of the WEDO ports, 14 | so the back of the wedo, on port 1 there 15 | is a LEGO wedo2.0 motor connected 16 | at port 2 there should be a infrared 17 | detect sensor 18 | _________________ 19 | | port2 | port1 | 20 | |________|________| 21 | | | 22 | | | 23 | |_________________| 24 | */ 25 | 26 | #include 27 | 28 | //Make myWedo object 29 | Wedo myWedo("test"); 30 | //Add a global variable detectSensorValue 31 | int detectSensorValue = 0; 32 | //Make the handleInput function (try to avoid using wedo 33 | //functions in this one) and keep it short 34 | void handleInput(int8_t* value,int size){ 35 | detectSensorValue = value[0]; 36 | } 37 | 38 | void setup() { 39 | // put your setup code here, to run once: 40 | Serial.begin(115200); 41 | myWedo.connect(); 42 | //Wait untill the wedo is connected to the ESP32 43 | while (!myWedo.connected()){ 44 | Serial.print("."); 45 | delay(100); 46 | } 47 | delay(3000); 48 | myWedo.setDetectSensor(2,handleInput); 49 | } 50 | 51 | 52 | void loop() { 53 | delay(100); 54 | myWedo.writeIndexColor(detectSensorValue/10); 55 | delay(100); 56 | myWedo.writeMotor(1,100-detectSensorValue); 57 | } 58 | -------------------------------------------------------------------------------- /esp32_ble_wedo.h: -------------------------------------------------------------------------------- 1 | /* 2 | Morse.h - Library for flashing Morse code. 3 | Created by David A. Mellis, November 2, 2007. 4 | Released into the public domain. 5 | */ 6 | #ifndef Wedo_h 7 | #define Wedo_h 8 | #define LOG_LOCAL_LEVEL ESP_LOG_VERBOSE 9 | 10 | 11 | 12 | #include "Arduino.h" 13 | #include "wedo_color_definitions.h" 14 | #include "ble_functions.h" 15 | 16 | #define ID_MOTOR 1 17 | #define ID_TILT_SENSOR 34 18 | //0x23 19 | #define ID_DETECT_SENSOR 35 20 | //0x24 21 | #define RANGE_10 0 22 | #define RANGE_100 1 23 | #define RANGE_RAW 2 24 | static void _WEDOnotificationHandler(uint8_t* data,int size); 25 | #define WEDO_PORTS 2 26 | static uint8_t devices[WEDO_PORTS] = {0,0}; 27 | typedef void (*inputHandlerFunction)(int8_t*,int); 28 | static void (*portHandlers[WEDO_PORTS])(int8_t*,int); 29 | 30 | //static void (*port2Handler)(uint8_t*,int); 31 | 32 | class Wedo 33 | { 34 | public: 35 | Wedo(const char*);//,void (*f)(int)); 36 | int connect(); 37 | boolean connected(); 38 | boolean ready(); 39 | void handleConnection(); // Call this in loop() to handle automatic reconnection 40 | int writeOutputCommand(uint8_t* command,int size); 41 | int writeInputCommand(uint8_t* command,int size); 42 | void writeMotor(uint8_t wedo_port,int wedo_speed); 43 | void writeIndexColor(uint8_t color); 44 | void writeSound(unsigned int frequency, unsigned int length); 45 | void writeRGB(uint8_t red, uint8_t green, uint8_t blue); 46 | void setRGBMode(); 47 | void setIndexMode(); 48 | void setTiltSensor(uint8_t port,inputHandlerFunction portHandler); 49 | void setDetectSensor(uint8_t port,inputHandlerFunction portHandler); 50 | void writePortDefinition(uint8_t port, uint8_t type, uint8_t mode, uint8_t format); 51 | void addNotificationHandler(void (*f)(uint8_t*,int)); 52 | }; 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /examples/two_sensors/two_sensors.ino: -------------------------------------------------------------------------------- 1 | /* 2 | @Author Geert Roumen 3 | @Date 5-6-2017 4 | @Hardware 5 | https://www.analoglamb.com/product/esp32-development-board/ 6 | @library 7 | https://github.com/lemio/esp32_ble_wedo 8 | @Status 9 | Working (but probably sub-optimal/stable) 10 | @licence 11 | https://creativecommons.org/licenses/by-sa/4.0/ 12 | 13 | if you look in front of the WEDO ports, 14 | so the back of the wedo, on port 1 there 15 | is a LEGO detect sensor connected 16 | at port 2 there should be a tilt sensor 17 | _________________ 18 | | port2 | port1 | 19 | |________|________| 20 | | | 21 | | | 22 | |_________________| 23 | */ 24 | 25 | #include 26 | //Make myWedo object 27 | Wedo myWedo("test"); 28 | //Add a global variable detectSensorValue 29 | int detectSensorValue = 0; 30 | //Make the handleInput functions (try to avoid using wedo 31 | //functions in this one) and keep it short 32 | void handleTiltSensor(int8_t* value,int size){ 33 | printf("\n\n \tx: %i y: %i \n\n",value[0],value[1]); 34 | } 35 | void handleDetectSensor(int8_t* value, int size){ 36 | printf("\n\n \tRIGHT: %i \n\n",value[0]); 37 | detectSensorValue = value[0]; 38 | } 39 | void setup() { 40 | // put your setup code here, to run once: 41 | Serial.begin(115200); 42 | myWedo.connect(); 43 | //Wait untill the wedo is connected to the ESP32 44 | while (!myWedo.connected()){ 45 | Serial.print("."); 46 | delay(100); 47 | } 48 | delay(1000); 49 | myWedo.setTiltSensor(2,handleTiltSensor); 50 | myWedo.setDetectSensor(1,handleDetectSensor); 51 | myWedo.setRGBMode(); 52 | } 53 | 54 | 55 | void loop() { 56 | // Handle automatic reconnection if connection is lost 57 | myWedo.handleConnection(); 58 | 59 | if (detectSensorValue != 0){ 60 | myWedo.writeSound(detectSensorValue*10,200); 61 | myWedo.writeRGB(detectSensorValue*2.5,0,255-detectSensorValue*2.5); 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /ble_functions.h: -------------------------------------------------------------------------------- 1 | #ifndef ble_functions_h 2 | #define ble_functions_h 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | //#define KEYBLE_UUID_APPL_SRVC { 0x23,0xd1,0xbc,0xea,0x5f,0x78,0x23,0x15,0xde,0xef, 0x12, 0x12,0x23, 0x15 ,0x00,0x00} This is the nordic button service 12 | /* 13 | Define the UUID's to listen to 14 | -> Service is the main LEGO i/o service 15 | -> MOTOR_UUID is the characteristic for the output commands 16 | */ 17 | 18 | #define WEDO_INPUT 0 19 | #define WEDO_OUTPUT 1 20 | 21 | // WEDO 2.0 Service and Characteristic UUIDs 22 | // Note: The advertising service UUID is 00001523-... but the actual service is 00004F0E-... 23 | #define WEDO_SERVICE_UUID "00004f0e-1212-efde-1523-785feabcd123" // Main LEGO I/O service 24 | #define WEDO_ADVERTISING_UUID "00001523-1212-efde-1523-785feabcd123" // Used in advertising 25 | #define WEDO_SENSOR_VALUE_UUID "00001560-1212-efde-1523-785feabcd123" 26 | #define WEDO_OUTPUT_UUID "00001565-1212-efde-1523-785feabcd123" 27 | #define WEDO_INPUT_UUID "00001563-1212-efde-1523-785feabcd123" 28 | #define WEDO_BATTERY_UUID "00001526-1212-efde-1523-785feabcd123" 29 | 30 | // Forward declarations 31 | class WedoClientCallbacks; 32 | class WedoScanCallbacks; 33 | 34 | // NimBLE client and service pointers 35 | static NimBLEClient* pWedoClient = nullptr; 36 | static NimBLERemoteService* pWedoService = nullptr; 37 | static NimBLERemoteCharacteristic* pOutputCharacteristic = nullptr; 38 | static NimBLERemoteCharacteristic* pInputCharacteristic = nullptr; 39 | static NimBLERemoteCharacteristic* pSensorCharacteristic = nullptr; 40 | 41 | static const NimBLEAdvertisedDevice* pTargetDevice = nullptr; 42 | static bool doConnect = false; 43 | static bool connected = false; 44 | static bool recieved = true; 45 | static uint32_t scanTimeMs = 5000; 46 | 47 | static void (*globalHandler)(uint8_t*,int) = nullptr; 48 | static const char* wedo_name = nullptr; 49 | 50 | // Callback classes 51 | class WedoClientCallbacks : public NimBLEClientCallbacks { 52 | void onConnect(NimBLEClient* pClient) override; 53 | void onDisconnect(NimBLEClient* pClient, int reason) override; 54 | }; 55 | 56 | class WedoScanCallbacks : public NimBLEScanCallbacks { 57 | void onResult(const NimBLEAdvertisedDevice* advertisedDevice) override; 58 | void onScanEnd(const NimBLEScanResults& results, int reason) override; 59 | }; 60 | 61 | // Function declarations 62 | void gattc_client_test(void); 63 | void writeBLECommand(int type, uint8_t* command, int size); 64 | void addBLEhandler(void (*f)(uint8_t*,int)); 65 | void setName(const char* name); 66 | int getBLEReady(); 67 | int getBLEConnected(); 68 | bool connectToWedoServer(); 69 | void notifyCallback(NimBLERemoteCharacteristic* pRemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify); 70 | void handleBLEConnection(); // New function to handle background reconnection 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # esp32_ble_wedo 2 | A library to control LEGO wedo with the ESP32 through Bluetooth low energy 3 | 4 | ## Version 2.0.0 - Now using NimBLE-Arduino! 🎉 5 | 6 | This library has been completely refactored to use the modern **NimBLE-Arduino** library for improved performance, stability, and reduced memory usage. The public API remains 100% compatible with previous versions. 7 | 8 | ### Requirements 9 | 10 | - **ESP32** board 11 | - **NimBLE-Arduino** library (automatically installed via Arduino Library Manager or add to platformio.ini) 12 | 13 | ### Installation 14 | 15 | #### Arduino IDE 16 | 1. Install NimBLE-Arduino from Library Manager (search for "NimBLE-Arduino" by h2zero) 17 | 2. Install this library from Library Manager or download from GitHub 18 | 19 | #### PlatformIO 20 | Add to your `platformio.ini`: 21 | ```ini 22 | lib_deps = 23 | h2zero/NimBLE-Arduino @ ^1.4.0 24 | lemio/esp32_ble_wedo @ ^2.0.0 25 | ``` 26 | 27 | ## API Reference 28 | 29 | ### myWedo(char* name) 30 | 31 | Connect to a WEDO2.0 that is advertising with the defined name. 32 | 33 | ### myWedo.connect() 34 | 35 | Start connecting to the WEDO2.0 (do this after the wifi is initialized, if you're using wifi) 36 | 37 | ### myWedo.writeMotor(uint8_t wedo_port,int wedo_speed) 38 | 39 | Writes a certain speed (-100,100) to the specified port. 40 | 41 | If you look in front of the WEDO ports; 42 | the back of the wedo, this is the port 43 | overview 44 | 
45 |  _________________
46 | |  port2 | port1  |
47 | |________|________|
48 | |                 |
49 | |                 |
50 | |_________________|
51 |
52 | 53 | ### myWedo.writeIndexColor(uint8_t color) 54 | 55 | Sets the color of the RGB led on the wedo, you can choose from the list below 56 | 
57 | #define LEGO_COLOR_BLACK 0
58 | #define LEGO_COLOR_PINK 1
59 | #define LEGO_COLOR_PURPLE 2
60 | #define LEGO_COLOR_BLUE 3
61 | #define LEGO_COLOR_CYAN 4
62 | #define LEGO_COLOR_LIGHTGREEN 5
63 | #define LEGO_COLOR_GREEN 6
64 | #define LEGO_COLOR_YELLOW 7
65 | #define LEGO_COLOR_ORANGE 8
66 | #define LEGO_COLOR_RED 9
67 | #define LEGO_COLOR_WHITE 10
68 |
69 | ### myWedo.writeSound(unsigned int frequency, unsigned int length) 70 | 71 | Let's the piezo in the WEDO make some noise, I'm not sure if the freqency and length are set correctly 72 | 73 | ### myWedo.writeOutputCommand(uint8_t* command) 74 | 75 | Sends a direct output command to the WEDO2.0 76 | 77 | ## Examples 78 | 79 | * wifi_control.ino (it let's you set the direction of the motor connected to the wedo). 80 | * button_motor.ino (it let's you controll the motor with the build in button on the ESP). (Nice start if you want to make a remote for you WEDO creation) 81 | 82 | ## Prior art 83 | 84 | * [Official WEDO2.0 SDK from LEGO ](https://education.lego.com/en-us/support/wedo-2/developer-kits) 85 | * [Nodejs implementation for Wedo2.0](https://github.com/vheun/wedo2) 86 | * [Controlling a motor in linux with USB dongle](http://www.ev3dev.org/docs/tutorials/controlling-wedo2-motor/) 87 | * [Sniffing data from WEDO2.0](http://ofalcao.pt/blog/2016/wedo-2-0-reverse-engineering) 88 | * [Controlling a motor ](http://ofalcao.pt/blog/2016/controlling-wedo-2-0-motor-from-linux) 89 | * [Controlling RGB LED in python](http://ofalcao.pt/blog/2016/wedo-2-0-colors-with-python) 90 | * [Making apps for WEDO2.0 in app-inventor](http://ofalcao.pt/blog/2016/lego-wedo-2-0-with-mit-app-inventor) 91 | * [NimBLE-Arduino](https://github.com/h2zero/NimBLE-Arduino) - The BLE library powering this project 92 | * [nRF Connect App](https://www.nordicsemi.com/Products/Development-tools/nRF-Connect-for-mobile) -------------------------------------------------------------------------------- /esp32_ble_wedo.cpp: -------------------------------------------------------------------------------- 1 | #include "esp32_ble_wedo.h" 2 | 3 | static void _WEDOnotificationHandler(uint8_t* data, int size){ 4 | if (size < 3) { 5 | printf("Invalid data size: %d\n", size); 6 | return; 7 | } 8 | 9 | uint8_t value = data[2]; 10 | uint8_t port = data[1]; 11 | 12 | printf("value: %d ; port: %d\n", data[2], data[1]); 13 | 14 | if (port > 0 && port <= WEDO_PORTS){ 15 | if (devices[port-1] > 0){ 16 | printf("Sent command to the handler\n"); 17 | if (devices[port-1] == ID_DETECT_SENSOR) 18 | { 19 | //send only one value (0-100) 20 | int8_t callback_data[] = {(int8_t)data[2]}; 21 | portHandlers[port-1](callback_data, 1); 22 | } 23 | else if (devices[port-1] == ID_TILT_SENSOR) 24 | { 25 | //send two values (-45 -> 45) 26 | if (size >= 4) { 27 | int8_t callback_data[] = {(int8_t)data[2], (int8_t)data[3]}; 28 | portHandlers[port-1](callback_data, 2); 29 | } 30 | } 31 | } else { 32 | printf("Can't handle the message - perhaps you didn't use setDetectSensor()\n"); 33 | } 34 | } 35 | } 36 | 37 | Wedo::Wedo(const char* name) 38 | { 39 | setName(name); 40 | addBLEhandler(_WEDOnotificationHandler); 41 | } 42 | 43 | int Wedo::connect(){ 44 | // Initialize BT with NimBLE 45 | gattc_client_test(); 46 | return 1; 47 | } 48 | 49 | int Wedo::writeOutputCommand(uint8_t* command, int size) 50 | { 51 | /* 52 | Write an output command to the output characteristic of the WEDO 53 | */ 54 | // Make a safety buffer so there won't be any loss of messages due to sending two messages at the same time 55 | // It waits till the last message was sent with a maximum of 100ms 56 | for(int i = 0; i < 20 && !ready(); i++){ 57 | delay(5); 58 | } 59 | writeBLECommand(WEDO_OUTPUT, command, size); 60 | return 1; 61 | } 62 | 63 | int Wedo::writeInputCommand(uint8_t input_command[], int size) 64 | { 65 | /* 66 | Write an input command to the input characteristic of the WEDO 67 | */ 68 | // Make a safety buffer so there won't be any loss of messages due to sending two messages at the same time 69 | // It waits till the last message was sent with a maximum of 100ms 70 | for(int i = 0; i < 20 && !ready(); i++){ 71 | delay(5); 72 | } 73 | printf("writeInputCommand with len: %d \n", size); 74 | writeBLECommand(WEDO_INPUT, input_command, size); 75 | return 1; 76 | } 77 | 78 | void Wedo::writeMotor(uint8_t wedo_port, int wedo_speed) 79 | { 80 | // From http://www.ev3dev.org/docs/tutorials/controlling-wedo2-motor/ 81 | // conversion from int (both pos and neg) to unsigned 8 bit int 82 | uint8_t speed_byte = wedo_speed; 83 | uint8_t command[] = {wedo_port, 0x01, 0x01, speed_byte}; 84 | writeOutputCommand(command, sizeof(command)); 85 | } 86 | 87 | void Wedo::writeIndexColor(uint8_t color){ 88 | // From http://ofalcao.pt/blog/2016/wedo-2-0-colors-with-python 89 | uint8_t command[] = {0x06, 0x04, 0x01, color}; 90 | writeOutputCommand(command, sizeof(command)); 91 | } 92 | 93 | void Wedo::writeRGB(uint8_t red, uint8_t green, uint8_t blue){ 94 | uint8_t command[] = {0x06, 0x04, 0x03, red, green, blue}; 95 | writeOutputCommand(command, sizeof(command)); 96 | } 97 | 98 | void Wedo::writeSound(unsigned int frequency, unsigned int length){ 99 | // From https://github.com/vheun/wedo2/blob/master/index.js (setSound) 100 | uint8_t command[] = { 101 | 0x05, 102 | 0x02, 103 | 0x04, 104 | uint8_t((frequency >> (8*0)) & 0xff), 105 | uint8_t((frequency >> (8*1)) & 0xff), 106 | uint8_t((length >> (8*0)) & 0xff), 107 | uint8_t((length >> (8*1)) & 0xff) 108 | }; 109 | writeOutputCommand(command, sizeof(command)); 110 | } 111 | 112 | boolean Wedo::connected(){ 113 | return getBLEConnected(); 114 | } 115 | 116 | boolean Wedo::ready(){ 117 | return getBLEReady(); 118 | } 119 | 120 | void Wedo::handleConnection(){ 121 | // Call the background connection handler 122 | handleBLEConnection(); 123 | } 124 | 125 | void Wedo::setRGBMode(){ 126 | writePortDefinition(0x06, 0x17, 0x01, 0x02); 127 | } 128 | 129 | void Wedo::setIndexMode(){ 130 | writePortDefinition(0x06, 0x17, 0x00, 0x00); 131 | } 132 | 133 | void Wedo::setDetectSensor(uint8_t port, inputHandlerFunction portHandler){ 134 | writePortDefinition(port, ID_DETECT_SENSOR, 0, RANGE_100); 135 | devices[port-1] = ID_DETECT_SENSOR; 136 | portHandlers[port-1] = portHandler; 137 | } 138 | 139 | void Wedo::setTiltSensor(uint8_t port, inputHandlerFunction portHandler){ 140 | writePortDefinition(port, ID_TILT_SENSOR, 0, RANGE_100); 141 | devices[port-1] = ID_TILT_SENSOR; 142 | portHandlers[port-1] = portHandler; 143 | } 144 | 145 | void Wedo::writePortDefinition(uint8_t port, uint8_t type, uint8_t mode, uint8_t format){ 146 | uint8_t command[] = {0x01, 0x02, port, type, mode, 0x01, 0x00, 0x00, 0x00, format, 0x01}; 147 | printf("writePortDefinition with len: %d \n", sizeof(command)); 148 | writeInputCommand(command, sizeof(command)); 149 | } 150 | 151 | void Wedo::addNotificationHandler(void (*f)(uint8_t*, int)){ 152 | // Overrule the standard notification handler 153 | addBLEhandler(f); 154 | } 155 | -------------------------------------------------------------------------------- /examples/wifi_control/wifi_control.ino: -------------------------------------------------------------------------------- 1 | /* 2 | WiFi Web Server LED Blink 3 | 4 | A simple web server that lets you blink an LED via the web. 5 | This sketch will print the IP address of your WiFi Shield (once connected) 6 | to the Serial monitor. From there, you can open that address in a web browser 7 | to turn on and off the LED on pin 5. 8 | 9 | If the IP address of your shield is yourAddress: 10 | http://yourAddress/H turns the LED on 11 | http://yourAddress/L turns it off 12 | 13 | This example is written for a network using WPA encryption. For 14 | WEP or WPA, change the Wifi.begin() call accordingly. 15 | 16 | Circuit: 17 | * WiFi shield attached 18 | * LED attached to pin 5 19 | 20 | created for arduino 25 Nov 2012 21 | by Tom Igoe 22 | 23 | ported for sparkfun esp32 24 | 31.01.2017 by Jan Hendrik Berlin 25 | 26 | changed fot the lego WEDO2.0 exmaple by Geert Roumen 27 | 28 | @Hardware 29 | https://www.analoglamb.com/product/esp32-development-board/ 30 | @library 31 | https://github.com/lemio/esp32_ble_wedo 32 | @Status 33 | Working (but probably sub-optimal/stable) 34 | @licence 35 | https://creativecommons.org/licenses/by-sa/4.0/ 36 | 37 | if you look in front of the WEDO ports, 38 | so the back of the wedo, on port 1 there 39 | is a LEGO wedo2.0 motor connected 40 | _________________ 41 | | port2 | port1 | 42 | |________|________| 43 | | | 44 | | | 45 | |_________________| 46 | 47 | */ 48 | #include 49 | 50 | #include 51 | Wedo myWedo("test"); 52 | 53 | const char* ssid = "The Shostakovich Network"; 54 | const char* password = "MySecretPassword"; 55 | 56 | WiFiServer server(80); 57 | 58 | void setup() 59 | { 60 | Serial.begin(115200); 61 | pinMode(5, OUTPUT); // set the LED pin mode 62 | 63 | 64 | 65 | delay(10); 66 | 67 | // We start by connecting to a WiFi network 68 | 69 | Serial.println(); 70 | Serial.println(); 71 | Serial.print("Connecting to "); 72 | Serial.println(ssid); 73 | 74 | WiFi.begin(ssid, password); 75 | 76 | while (WiFi.status() != WL_CONNECTED) { 77 | delay(500); 78 | Serial.print("."); 79 | } 80 | 81 | Serial.println(""); 82 | Serial.println("WiFi connected"); 83 | Serial.println("IP address: "); 84 | Serial.println(WiFi.localIP()); 85 | 86 | server.begin(); 87 | 88 | myWedo.connect(); 89 | while (!myWedo.connected()){ 90 | Serial.print("."); 91 | delay(100); 92 | } 93 | 94 | } 95 | 96 | int value = 0; 97 | 98 | void loop(){ 99 | WiFiClient client = server.available(); // listen for incoming clients 100 | 101 | if (client) { // if you get a client, 102 | Serial.println("new client"); // print a message out the serial port 103 | String currentLine = ""; // make a String to hold incoming data from the client 104 | while (client.connected()) { // loop while the client's connected 105 | if (client.available()) { // if there's bytes to read from the client, 106 | char c = client.read(); // read a byte, then 107 | Serial.write(c); // print it out the serial monitor 108 | if (c == '\n') { // if the byte is a newline character 109 | 110 | // if the current line is blank, you got two newline characters in a row. 111 | // that's the end of the client HTTP request, so send a response: 112 | if (currentLine.length() == 0) { 113 | // HTTP headers always start with a response code (e.g. HTTP/1.1 200 OK) 114 | // and a content-type so the client knows what's coming, then a blank line: 115 | client.println("HTTP/1.1 200 OK"); 116 | client.println("Content-type:text/html"); 117 | client.println(); 118 | 119 | // the content of the HTTP response follows the header: 120 | client.print("Click here turn the motor forward on
"); 121 | client.print("Click here turn the stop the motor
"); 122 | client.print("Click here turn the motor backwards
"); 123 | // The HTTP response ends with another blank line: 124 | client.println(); 125 | // break out of the while loop: 126 | break; 127 | } else { // if you got a newline, then clear currentLine: 128 | currentLine = ""; 129 | } 130 | } else if (c != '\r') { // if you got anything else but a carriage return character, 131 | currentLine += c; // add it to the end of the currentLine 132 | } 133 | 134 | // Check to see if the client request was "GET /H" or "GET /L": 135 | if (currentLine.endsWith("GET /F")) { 136 | myWedo.writeMotor(1,100);//digitalWrite(5, HIGH); // GET /H turns the LED on 137 | } 138 | if (currentLine.endsWith("GET /S")) { 139 | myWedo.writeMotor(1,0);//digitalWrite(5, HIGH); // GET /H turns the LED on 140 | } 141 | if (currentLine.endsWith("GET /B")) { 142 | myWedo.writeMotor(1,-100);//digitalWrite(5, LOW); // GET /L turns the LED off 143 | } 144 | } 145 | } 146 | // close the connection: 147 | client.stop(); 148 | Serial.println("client disonnected"); 149 | } 150 | } 151 | -------------------------------------------------------------------------------- /ble_functions.cpp: -------------------------------------------------------------------------------- 1 | #include "ble_functions.h" 2 | 3 | #define GATTC_TAG "WEDO_BLE" 4 | 5 | // Callback instances 6 | static WedoClientCallbacks clientCallbacks; 7 | static WedoScanCallbacks scanCallbacks; 8 | 9 | // Notification callback handler 10 | void notifyCallback(NimBLERemoteCharacteristic* pRemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) { 11 | Serial.printf("Notification from %s: ", pRemoteCharacteristic->getUUID().toString().c_str()); 12 | Serial.printf("Length: %d, Data: 0x", length); 13 | for (size_t i = 0; i < length; i++) { 14 | Serial.printf("%02x", pData[i]); 15 | } 16 | Serial.printf("\n"); 17 | 18 | // Call the user-defined handler if set 19 | if (globalHandler != nullptr) { 20 | globalHandler(pData, length); 21 | } 22 | } 23 | 24 | // Client callback implementations 25 | void WedoClientCallbacks::onConnect(NimBLEClient* pClient) { 26 | Serial.printf("Connected to WEDO device\n"); 27 | connected = true; 28 | } 29 | 30 | void WedoClientCallbacks::onDisconnect(NimBLEClient* pClient, int reason) { 31 | Serial.printf("Disconnected from WEDO, reason = %d - Starting scan\n", reason); 32 | connected = false; 33 | doConnect = false; 34 | pWedoClient = nullptr; 35 | pWedoService = nullptr; 36 | pOutputCharacteristic = nullptr; 37 | pInputCharacteristic = nullptr; 38 | pSensorCharacteristic = nullptr; 39 | 40 | // Restart scanning after a short delay 41 | delay(1000); 42 | if (NimBLEDevice::getScan()->isScanning()) { 43 | NimBLEDevice::getScan()->stop(); 44 | } 45 | NimBLEDevice::getScan()->start(scanTimeMs, false, true); 46 | Serial.printf("Reconnection scan started\n"); 47 | } 48 | 49 | // Scan callback implementations 50 | void WedoScanCallbacks::onResult(const NimBLEAdvertisedDevice* advertisedDevice) { 51 | Serial.printf("Advertised Device: %s\n", advertisedDevice->toString().c_str()); 52 | 53 | // Check if this is the WEDO device - it advertises with UUID 00001523 but service is 00004F0E 54 | if (advertisedDevice->isAdvertisingService(NimBLEUUID(WEDO_ADVERTISING_UUID))) { 55 | Serial.printf("Found WEDO Advertising Service\n"); 56 | 57 | // Check if the name matches (if a name filter is set) 58 | if (wedo_name != nullptr) { 59 | if (advertisedDevice->haveName()) { 60 | std::string deviceName = advertisedDevice->getName(); 61 | if (deviceName == wedo_name) { 62 | Serial.printf("Found our WEDO device: %s\n", wedo_name); 63 | NimBLEDevice::getScan()->stop(); 64 | pTargetDevice = advertisedDevice; 65 | doConnect = true; 66 | } 67 | } 68 | } else { 69 | // No name filter, connect to any WEDO device 70 | Serial.printf("Found a WEDO device\n"); 71 | NimBLEDevice::getScan()->stop(); 72 | pTargetDevice = advertisedDevice; 73 | doConnect = true; 74 | } 75 | } 76 | } 77 | 78 | void WedoScanCallbacks::onScanEnd(const NimBLEScanResults& results, int reason) { 79 | Serial.printf("Scan Ended, reason: %d, device count: %d\n", reason, results.getCount()); 80 | 81 | // Only restart if we haven't found a device to connect to 82 | if (!doConnect && !connected) { 83 | Serial.printf("Restarting scan...\n"); 84 | NimBLEDevice::getScan()->start(scanTimeMs, false, true); 85 | } 86 | } 87 | 88 | // Connect to WEDO server 89 | bool connectToWedoServer() { 90 | if (pTargetDevice == nullptr) { 91 | Serial.printf("No target device set\n"); 92 | return false; 93 | } 94 | 95 | // Check if we have a client we can reuse 96 | if (NimBLEDevice::getCreatedClientCount()) { 97 | pWedoClient = NimBLEDevice::getClientByPeerAddress(pTargetDevice->getAddress()); 98 | if (pWedoClient) { 99 | if (!pWedoClient->connect(pTargetDevice, false)) { 100 | Serial.printf("Reconnect failed\n"); 101 | return false; 102 | } 103 | Serial.printf("Reconnected client\n"); 104 | } else { 105 | pWedoClient = NimBLEDevice::getDisconnectedClient(); 106 | } 107 | } 108 | 109 | // Create a new client if needed 110 | if (!pWedoClient) { 111 | if (NimBLEDevice::getCreatedClientCount() >= NIMBLE_MAX_CONNECTIONS) { 112 | Serial.printf("Max clients reached\n"); 113 | return false; 114 | } 115 | 116 | pWedoClient = NimBLEDevice::createClient(); 117 | Serial.printf("New client created\n"); 118 | 119 | pWedoClient->setClientCallbacks(&clientCallbacks, false); 120 | pWedoClient->setConnectionParams(12, 12, 0, 150); 121 | pWedoClient->setConnectTimeout(5 * 1000); 122 | 123 | if (!pWedoClient->connect(pTargetDevice)) { 124 | NimBLEDevice::deleteClient(pWedoClient); 125 | pWedoClient = nullptr; 126 | Serial.printf("Failed to connect\n"); 127 | return false; 128 | } 129 | } 130 | 131 | if (!pWedoClient->isConnected()) { 132 | if (!pWedoClient->connect(pTargetDevice)) { 133 | Serial.printf("Failed to connect\n"); 134 | return false; 135 | } 136 | } 137 | 138 | Serial.printf("Connected to: %s RSSI: %d\n", 139 | pWedoClient->getPeerAddress().toString().c_str(), 140 | pWedoClient->getRssi()); 141 | 142 | // Get the WEDO service 143 | pWedoService = pWedoClient->getService(WEDO_SERVICE_UUID); 144 | if (pWedoService == nullptr) { 145 | Serial.printf("Failed to find WEDO service\n"); 146 | pWedoClient->disconnect(); 147 | return false; 148 | } 149 | 150 | Serial.printf("Found WEDO service\n"); 151 | 152 | // Get the characteristics 153 | pSensorCharacteristic = pWedoService->getCharacteristic(WEDO_SENSOR_VALUE_UUID); 154 | pOutputCharacteristic = pWedoService->getCharacteristic(WEDO_OUTPUT_UUID); 155 | pInputCharacteristic = pWedoService->getCharacteristic(WEDO_INPUT_UUID); 156 | 157 | if (pSensorCharacteristic == nullptr || pOutputCharacteristic == nullptr || pInputCharacteristic == nullptr) { 158 | Serial.printf("Failed to find required characteristics\n"); 159 | Serial.printf("Sensor: %s, Output: %s, Input: %s\n", 160 | pSensorCharacteristic ? "OK" : "FAIL", 161 | pOutputCharacteristic ? "OK" : "FAIL", 162 | pInputCharacteristic ? "OK" : "FAIL"); 163 | pWedoClient->disconnect(); 164 | return false; 165 | } 166 | 167 | Serial.printf("Found all required characteristics\n"); 168 | 169 | // Subscribe to notifications on the sensor characteristic 170 | if (pSensorCharacteristic->canNotify()) { 171 | if (!pSensorCharacteristic->subscribe(true, notifyCallback)) { 172 | Serial.printf("Failed to subscribe to notifications\n"); 173 | pWedoClient->disconnect(); 174 | return false; 175 | } 176 | Serial.printf("Subscribed to sensor notifications\n"); 177 | } 178 | 179 | connected = true; 180 | Serial.printf("WEDO device ready!\n"); 181 | return true; 182 | } 183 | 184 | // Initialize BLE and start scanning 185 | void gattc_client_test(void) { 186 | Serial.printf("Initializing NimBLE Client\n"); 187 | 188 | // Initialize NimBLE 189 | NimBLEDevice::init("ESP32-WEDO"); 190 | 191 | // Optional: Set transmit power 192 | NimBLEDevice::setPower(ESP_PWR_LVL_P9); /** +9dBm */ 193 | 194 | // Get the scan object 195 | NimBLEScan* pScan = NimBLEDevice::getScan(); 196 | 197 | // Set scan callbacks 198 | pScan->setScanCallbacks(&scanCallbacks, false); 199 | 200 | // Set scan parameters 201 | pScan->setInterval(100); 202 | pScan->setWindow(100); 203 | pScan->setActiveScan(true); 204 | 205 | // Start scanning 206 | pScan->start(scanTimeMs, false, true); 207 | Serial.printf("Scanning for WEDO devices...\n"); 208 | 209 | // Non-blocking connection handling - will be called in loop 210 | // Check periodically if we need to connect 211 | unsigned long startTime = millis(); 212 | while (!connected && (millis() - startTime < 30000)) { // 30 second timeout 213 | if (doConnect) { 214 | doConnect = false; 215 | if (connectToWedoServer()) { 216 | Serial.printf("Successfully connected to WEDO\n"); 217 | break; 218 | } else { 219 | Serial.printf("Failed to connect, continuing scan\n"); 220 | delay(1000); 221 | if (!NimBLEDevice::getScan()->isScanning()) { 222 | NimBLEDevice::getScan()->start(scanTimeMs, false, true); 223 | } 224 | } 225 | } 226 | delay(100); 227 | } 228 | 229 | if (!connected) { 230 | Serial.printf("Warning: Initial connection attempt timed out. Scan continues in background.\n"); 231 | } 232 | } 233 | 234 | // Handle background connection attempts 235 | void handleBLEConnection() { 236 | if (!connected && doConnect) { 237 | doConnect = false; 238 | if (connectToWedoServer()) { 239 | Serial.printf("Successfully reconnected to WEDO\n"); 240 | } else { 241 | Serial.printf("Reconnection attempt failed\n"); 242 | // Restart scanning if not already scanning 243 | if (!NimBLEDevice::getScan()->isScanning()) { 244 | NimBLEDevice::getScan()->start(scanTimeMs, false, true); 245 | } 246 | } 247 | } 248 | } 249 | 250 | // Write a command to the WEDO device 251 | void writeBLECommand(int type, uint8_t* command, int size) { 252 | if (!connected || pWedoClient == nullptr) { 253 | Serial.printf("Not connected to WEDO device - attempting reconnection\n"); 254 | 255 | // Try to trigger reconnection if not already scanning 256 | if (!NimBLEDevice::getScan()->isScanning() && !doConnect) { 257 | Serial.printf("Starting reconnection scan\n"); 258 | NimBLEDevice::getScan()->start(scanTimeMs, false, true); 259 | } 260 | 261 | recieved = true; 262 | return; 263 | } 264 | 265 | recieved = false; 266 | 267 | NimBLERemoteCharacteristic* pChar = nullptr; 268 | 269 | if (type == WEDO_INPUT) { 270 | pChar = pInputCharacteristic; 271 | } else if (type == WEDO_OUTPUT) { 272 | pChar = pOutputCharacteristic; 273 | } 274 | 275 | if (pChar == nullptr) { 276 | Serial.printf("Characteristic not available\n"); 277 | recieved = true; 278 | return; 279 | } 280 | 281 | if (pChar->canWrite()) { 282 | if (pChar->writeValue(command, size, true)) { 283 | // Serial.printf("Successfully wrote command\n"); 284 | recieved = true; 285 | } else { 286 | Serial.printf("Failed to write command\n"); 287 | recieved = true; 288 | } 289 | } else { 290 | Serial.printf("Characteristic not writable\n"); 291 | recieved = true; 292 | } 293 | } 294 | 295 | // Set the device name to scan for 296 | void setName(const char* name) { 297 | wedo_name = name; 298 | recieved = true; 299 | } 300 | 301 | // Check if the device is ready to receive commands 302 | int getBLEReady() { 303 | return recieved; 304 | } 305 | 306 | // Check if the device is connected 307 | int getBLEConnected() { 308 | return connected; 309 | } 310 | 311 | // Add a notification handler 312 | void addBLEhandler(void (*f)(uint8_t*, int)) { 313 | globalHandler = f; 314 | } 315 | --------------------------------------------------------------------------------