├── README.md ├── Config.h ├── analogWrite.h ├── Kinematics.ino ├── LED.h ├── OTA.ino ├── LegoRemote.ino ├── Config.ino ├── Lego.ino ├── LED.ino ├── WiFi.ino ├── analogWrite.cpp └── BLE.ino /README.md: -------------------------------------------------------------------------------- 1 | # LegoRemote 2 | 3 | For a project description see http://www.g3gg0.de/wordpress/programming/legoremote/ 4 | -------------------------------------------------------------------------------- /Config.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef __CONFIG_H__ 3 | #define __CONFIG_H__ 4 | 5 | typedef struct 6 | { 7 | uint64_t magic1; 8 | uint32_t version; 9 | 10 | bool invert[2]; 11 | bool swap; 12 | bool kinematics; 13 | uint8_t light_intensity; 14 | 15 | /* footer */ 16 | uint64_t magic2; 17 | } config_t; 18 | 19 | #define CONFIG_VERSION 2 20 | 21 | extern config_t current_config; 22 | extern bool config_modified; 23 | 24 | #endif 25 | 26 | -------------------------------------------------------------------------------- /analogWrite.h: -------------------------------------------------------------------------------- 1 | #ifndef _ESP32_ANALOG_WRITE_ 2 | #define _ESP32_ANALOG_WRITE_ 3 | 4 | #include 5 | 6 | typedef struct analog_write_channel 7 | { 8 | int8_t pin; 9 | double frequency; 10 | uint8_t resolution; 11 | } analog_write_channel_t; 12 | 13 | int analogWriteChannel(uint8_t pin); 14 | 15 | void analogWriteFrequency(double frequency); 16 | void analogWriteFrequency(uint8_t pin, double frequency); 17 | 18 | void analogWriteResolution(uint8_t resolution); 19 | void analogWriteResolution(uint8_t pin, uint8_t resolution); 20 | 21 | void analogWrite(uint8_t pin, uint32_t value, uint32_t valueMax = 255); 22 | 23 | #endif 24 | 25 | -------------------------------------------------------------------------------- /Kinematics.ino: -------------------------------------------------------------------------------- 1 | 2 | void kin_norm(float *x, float *y) 3 | { 4 | float norm_value_x = fabsf(*x); 5 | float norm_value_y = fabsf(*y); 6 | 7 | if(norm_value_x > 1) 8 | { 9 | *x /= norm_value_x; 10 | *y /= norm_value_x; 11 | } 12 | if(norm_value_y > 1) 13 | { 14 | *x /= norm_value_y; 15 | *y /= norm_value_y; 16 | } 17 | } 18 | 19 | void kin_set(float x, float y) 20 | { 21 | float val_a = 0; 22 | float val_b = 0; 23 | 24 | /* y is the forward speed, being the base value of both motors a and b */ 25 | val_a = y; 26 | val_b = y; 27 | 28 | /* x adds to motor a and subtracts from motor b, causing a rotation */ 29 | val_a += x; 30 | val_b -= x; 31 | 32 | /* now normalize to 1 if larger */ 33 | kin_norm(&val_a, &val_b); 34 | 35 | /* set outputs, swapping etc will be done in that lower layer */ 36 | lego_motor(0, val_a); 37 | lego_motor(1, val_b); 38 | } 39 | 40 | -------------------------------------------------------------------------------- /LED.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _LED_H_ 3 | #define _LED_H_ 4 | 5 | #define LED_ANIM_LOOP { 254, 0 } 6 | #define LED_ANIM_END { 253, 0 } 7 | #define LED_ANIM_NEXT(anim) { 255, (uint32_t) anim } 8 | 9 | 10 | typedef struct 11 | { 12 | uint8_t pct; 13 | uint32_t delay; 14 | } led_anim_t; 15 | 16 | typedef struct 17 | { 18 | uint32_t pin; 19 | uint32_t pos; 20 | uint32_t last_time; 21 | led_anim_t *anim; 22 | } led_anim_state_t; 23 | 24 | 25 | extern led_anim_t led_anim_ble_connected[]; 26 | 27 | extern led_anim_t led_anim_shortblink[]; 28 | extern led_anim_t led_anim_slowshortblink[]; 29 | extern led_anim_t led_anim_shortdoubleblinkloop[]; 30 | extern led_anim_t led_anim_shortdoubleblink[]; 31 | extern led_anim_t led_anim_emergency[]; 32 | extern led_anim_t led_anim_idle[]; 33 | extern led_anim_t led_anim_none[]; 34 | extern led_anim_t led_anim_mediumblink[]; 35 | extern led_anim_t led_anim_fastblink[]; 36 | 37 | void led_anim_loop(led_anim_state_t *state); 38 | void led_anim_start(led_anim_state_t *state, led_anim_t *anim, bool force = false); 39 | void led_anim_red(led_anim_t *anim); 40 | void led_anim_blue(led_anim_t *anim); 41 | 42 | #endif 43 | 44 | -------------------------------------------------------------------------------- /OTA.ino: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | led_anim_t ota_anim_enabled[] = { { 10, 20 }, { 100, 20 }, { 10, 20 }, { 0, 50 }, { 10, 20 }, { 100, 20 }, { 10, 20 }, { 0, 750 }, LED_ANIM_LOOP }; 5 | led_anim_t ota_anim_active[] = { { 10, 20 }, { 100, 20 }, { 10, 20 }, { 0, 20 }, LED_ANIM_LOOP }; 6 | 7 | bool ota_started = false; 8 | 9 | void ota_setup() 10 | { 11 | if(ota_started) 12 | { 13 | return; 14 | } 15 | 16 | Serial.printf("[OTA] Starting...\n"); 17 | ArduinoOTA.setHostname("LegoRemote"); 18 | 19 | ArduinoOTA 20 | .onStart([]() { 21 | Serial.printf("[OTA] Start download...\n"); 22 | ota_active = true; 23 | led_anim_blue(ota_anim_active); 24 | led_anim_red(ota_anim_active); 25 | }) 26 | .onEnd([]() { 27 | Serial.printf("[OTA] Finished download...\n"); 28 | ota_active = false; 29 | led_anim_blue(led_anim_none); 30 | led_anim_red(led_anim_none); 31 | }) 32 | .onProgress([](unsigned int progress, unsigned int total) { 33 | analogWrite(LED_STATUS, progress * 255 / total, 255); 34 | }) 35 | .onError([](ota_error_t error) { 36 | led_anim_blue(led_anim_emergency); 37 | led_anim_red(led_anim_emergency); 38 | Serial.printf("Error[%u]: ", error); 39 | if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed"); 40 | else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed"); 41 | else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed"); 42 | else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed"); 43 | else if (error == OTA_END_ERROR) Serial.println("End Failed"); 44 | }); 45 | 46 | ota_started = true; 47 | ArduinoOTA.begin(); 48 | 49 | led_anim_blue(ota_anim_enabled); 50 | } 51 | 52 | bool ota_loop() 53 | { 54 | if(ota_started) 55 | { 56 | ArduinoOTA.handle(); 57 | } 58 | 59 | return ota_active; 60 | } 61 | 62 | -------------------------------------------------------------------------------- /LegoRemote.ino: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * To be built with partition scheme "Minimal SPIFFS" 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "LED.h" 13 | 14 | bool ota_active = false; 15 | 16 | 17 | #define LED_ACTIVITY 13 18 | #define LED_STATUS 15 19 | 20 | 21 | void setup() 22 | { 23 | Serial.begin(115200); 24 | Serial.printf("\n\n\n"); 25 | 26 | Serial.printf("[i] SDK: '%s'\n", ESP.getSdkVersion()); 27 | Serial.printf("[i] CPU Speed: %d MHz\n", ESP.getCpuFreqMHz()); 28 | Serial.printf("[i] Chip Id: %06X\n", ESP.getEfuseMac()); 29 | Serial.printf("[i] Flash Mode: %08X\n", ESP.getFlashChipMode()); 30 | Serial.printf("[i] Flash Size: %08X\n", ESP.getFlashChipSize()); 31 | Serial.printf("[i] Flash Speed: %d MHz\n", ESP.getFlashChipSpeed() / 1000000); 32 | Serial.printf("[i] Heap %d/%d\n", ESP.getFreeHeap(), ESP.getHeapSize()); 33 | Serial.printf("[i] SPIRam %d/%d\n", ESP.getFreePsram(), ESP.getPsramSize()); 34 | Serial.printf("\n"); 35 | Serial.printf("[i] Starting\n"); 36 | 37 | Serial.printf("[i] Setup LEDs\n"); 38 | led_setup(); 39 | Serial.printf("[i] Setup LEGO outputs\n"); 40 | lego_init(); 41 | Serial.printf("[i] Setup SPIFFS\n"); 42 | if(!SPIFFS.begin(true)) 43 | { 44 | Serial.println("[E] SPIFFS Mount Failed"); 45 | } 46 | config_setup(); 47 | 48 | Serial.printf("[i] Setup BLE\n"); 49 | ble_setup(); 50 | 51 | Serial.println("Setup done"); 52 | } 53 | 54 | void loop() 55 | { 56 | bool hasWork = false; 57 | 58 | hasWork |= lego_loop(); 59 | hasWork |= config_loop(); 60 | hasWork |= ble_loop(); 61 | hasWork |= wifi_loop(); 62 | hasWork |= led_loop(); 63 | hasWork |= ota_loop(); 64 | 65 | if(!hasWork) 66 | { 67 | delay(20); 68 | } 69 | else 70 | { 71 | delay(5); 72 | } 73 | } 74 | 75 | -------------------------------------------------------------------------------- /Config.ino: -------------------------------------------------------------------------------- 1 | 2 | #include "Config.h" 3 | 4 | config_t current_config; 5 | bool config_modified = false; 6 | 7 | 8 | void config_setup() 9 | { 10 | File file = SPIFFS.open("/config.bin", "r"); 11 | 12 | if(!file) 13 | { 14 | Serial.println("[E] failed to open file for reading"); 15 | } 16 | else 17 | { 18 | file.read((uint8_t *)¤t_config, sizeof(current_config)); 19 | file.close(); 20 | Serial.println("[i] read status from SPIFFS"); 21 | } 22 | 23 | if(current_config.magic1 != 0xDEADBEEF || current_config.magic2 != 0x55AA55AA || current_config.version != CONFIG_VERSION) 24 | { 25 | Serial.println("[i] incorrect magics or version, reinit status"); 26 | 27 | memset((void *)¤t_config, 0x00, sizeof(current_config)); 28 | 29 | current_config.magic1 = 0xDEADBEEF; 30 | current_config.magic2 = 0x55AA55AA; 31 | current_config.version = CONFIG_VERSION; 32 | 33 | /* set default config */ 34 | current_config.swap = false; 35 | current_config.kinematics = true; 36 | current_config.invert[0] = false; 37 | current_config.invert[1] = false; 38 | current_config.light_intensity = 32; 39 | 40 | config_modified = true; 41 | } 42 | } 43 | 44 | bool config_loop() 45 | { 46 | int curTime = millis(); 47 | static int nextTimeSave = 0x7FFFFFFF; 48 | 49 | if(config_modified) 50 | { 51 | Serial.println("[i] config modified"); 52 | nextTimeSave = curTime + 5000; 53 | config_modified = false; 54 | } 55 | 56 | if(nextTimeSave <= curTime) 57 | { 58 | File file = SPIFFS.open("/config.bin", "w"); 59 | if(!file) 60 | { 61 | Serial.println("[E] failed to open file for writing"); 62 | } 63 | else 64 | { 65 | file.write((const uint8_t *)¤t_config, sizeof(current_config)); 66 | file.close(); 67 | Serial.println("[i] saved config"); 68 | } 69 | nextTimeSave = 0x7FFFFFFF; 70 | led_anim_red(led_anim_shortdoubleblink); 71 | } 72 | } 73 | 74 | 75 | -------------------------------------------------------------------------------- /Lego.ino: -------------------------------------------------------------------------------- 1 | 2 | #include "analogWrite.h" 3 | #include "LED.h" 4 | 5 | #define LEGO_LED 27 6 | #define LEGO_NSLEEP 26 7 | #define LEGO_NFAULT 25 8 | #define LEGO_AIN2 33 9 | #define LEGO_AIN1 32 10 | #define LEGO_BIN2 12 11 | #define LEGO_BIN1 14 12 | 13 | bool lego_fault = false; 14 | 15 | uint8_t lego_in1[] = { LEGO_AIN1, LEGO_BIN1 }; 16 | uint8_t lego_in2[] = { LEGO_AIN2, LEGO_BIN2 }; 17 | 18 | 19 | void lego_motor_disable() 20 | { 21 | digitalWrite(LEGO_NSLEEP, LOW); 22 | analogWrite(LEGO_AIN1, 0, 255); 23 | analogWrite(LEGO_AIN2, 0, 255); 24 | analogWrite(LEGO_BIN1, 0, 255); 25 | analogWrite(LEGO_BIN2, 0, 255); 26 | } 27 | 28 | void lego_led(uint8_t state) 29 | { 30 | analogWrite(LEGO_LED, state, 255); 31 | } 32 | 33 | void lego_init() 34 | { 35 | lego_motor_disable(); 36 | 37 | analogWriteFrequency(30000); 38 | analogWriteResolution(8); 39 | 40 | pinMode(LEGO_LED, OUTPUT); 41 | digitalWrite(LEGO_LED, HIGH); 42 | 43 | pinMode(LEGO_NSLEEP, OUTPUT); 44 | pinMode(LEGO_NFAULT, INPUT_PULLUP); 45 | pinMode(LEGO_AIN1, OUTPUT); 46 | pinMode(LEGO_AIN2, OUTPUT); 47 | pinMode(LEGO_BIN1, OUTPUT); 48 | pinMode(LEGO_BIN2, OUTPUT); 49 | 50 | lego_motor(0, 0); 51 | lego_motor(1, 0); 52 | lego_led(0); 53 | 54 | delay(10); 55 | digitalWrite(LEGO_NSLEEP, HIGH); 56 | delay(10); 57 | 58 | led_anim_red(led_anim_none); 59 | } 60 | 61 | void lego_motor(uint8_t motor, uint8_t power, bool forward = true) 62 | { 63 | /* swap motor if configured */ 64 | if(current_config.swap) 65 | { 66 | motor = motor ? 0 : 1; 67 | } 68 | 69 | /* invert direction */ 70 | forward ^= current_config.invert[motor]; 71 | 72 | if(power == 0) 73 | { 74 | analogWrite(lego_in1[motor], 0, 255); 75 | analogWrite(lego_in2[motor], 0, 255); 76 | } 77 | else 78 | { 79 | uint8_t pwm = min(255.0f, sgn(power) * 0.2f * 255.0f + power * 0.8f); 80 | 81 | if(forward) 82 | { 83 | analogWrite(lego_in1[motor], 255 - pwm, 255); 84 | analogWrite(lego_in2[motor], 255, 255); 85 | } 86 | else 87 | { 88 | analogWrite(lego_in1[motor], 255, 255); 89 | analogWrite(lego_in2[motor], 255 - pwm, 255); 90 | } 91 | } 92 | } 93 | 94 | void lego_motor(uint8_t motor, float power) 95 | { 96 | lego_motor(motor, min(1.0f,fabsf(power)) * 255, power >= 0); 97 | } 98 | 99 | void lego_motor_a(uint8_t motor, float power) 100 | { 101 | lego_motor(0, power); 102 | } 103 | 104 | void lego_motor_b(uint8_t motor, float power) 105 | { 106 | lego_motor(1, power); 107 | } 108 | 109 | bool lego_loop() 110 | { 111 | if(digitalRead(LEGO_NFAULT) == LOW) 112 | { 113 | Serial.println("[E] DRV8833 reports nFAULT == LOW, disabling"); 114 | lego_motor_disable(); 115 | lego_fault = true; 116 | led_anim_red(led_anim_emergency); 117 | } 118 | 119 | return false; 120 | } 121 | 122 | -------------------------------------------------------------------------------- /LED.ino: -------------------------------------------------------------------------------- 1 | 2 | #include "analogWrite.h" 3 | #include "LED.h" 4 | 5 | extern bool lego_fault; 6 | led_anim_state_t led_anim_state_red = { LED_STATUS, 0, 0, NULL }; 7 | led_anim_state_t led_anim_state_blue = { LED_ACTIVITY, 0, 0, NULL }; 8 | 9 | led_anim_t led_anim_shortblink[] = { { 100, 100 }, { 0, 2000 }, LED_ANIM_LOOP }; 10 | led_anim_t led_anim_slowshortblink[] = { { 10, 20 }, { 100, 20 }, { 10, 20 }, { 0, 5000 }, LED_ANIM_LOOP }; 11 | led_anim_t led_anim_shortdoubleblinkloop[] = { { 10, 20 }, { 100, 20 }, { 10, 20 }, { 0, 100 }, { 10, 20 }, { 100, 20 }, { 10, 20 }, { 0, 100 }, { 0, 750 }, LED_ANIM_LOOP }; 12 | led_anim_t led_anim_shortdoubleblink[] = { { 10, 20 }, { 100, 20 }, { 10, 20 }, { 0, 100 }, { 10, 20 }, { 100, 20 }, { 10, 20 }, { 0, 100 }, { 0, 750 }, LED_ANIM_END }; 13 | led_anim_t led_anim_emergency[] = { { 100, 20 }, { 0, 100 }, { 100, 20 }, { 0, 100 }, LED_ANIM_LOOP }; 14 | led_anim_t led_anim_idle[] = { { 1, 20 }, { 0, 5000 }, LED_ANIM_LOOP }; 15 | led_anim_t led_anim_mediumblink[] = { { 1, 200 }, { 0, 200 }, LED_ANIM_LOOP }; 16 | led_anim_t led_anim_fastblink[] = { { 1, 50 }, { 0, 50 }, LED_ANIM_LOOP }; 17 | led_anim_t led_anim_none[] = { { 0, 500 }, LED_ANIM_LOOP }; 18 | 19 | led_anim_t led_anim_ble_connected[] = { { 100, 200 }, { 0, 20 }, { 100, 200 }, { 0, 20 }, { 100, 20 }, { 0, 20 }, { 100, 20 }, LED_ANIM_NEXT(led_anim_slowshortblink) }; 20 | 21 | 22 | void led_anim_loop(led_anim_state_t *state) 23 | { 24 | if(!state || !state->anim) 25 | { 26 | return; 27 | } 28 | if(state->anim[state->pos].pct == 255) 29 | { 30 | led_anim_start(state, (led_anim_t *)state->anim[state->pos].delay, true); 31 | return; 32 | } 33 | else if(state->anim[state->pos].pct == 253) 34 | { 35 | return; 36 | } 37 | else if(state->anim[state->pos].pct > 100) 38 | { 39 | state->pos = 0; 40 | } 41 | 42 | analogWrite(state->pin, state->anim[state->pos].pct, 100); 43 | 44 | if(state->last_time + state->anim[state->pos].delay < millis()) 45 | { 46 | state->last_time = millis(); 47 | state->pos++; 48 | } 49 | } 50 | 51 | void led_anim_start(led_anim_state_t *state, led_anim_t *anim, bool force) 52 | { 53 | if(state->anim == anim && !force) 54 | { 55 | return; 56 | } 57 | state->pos = 0; 58 | state->anim = anim; 59 | state->last_time = millis(); 60 | 61 | led_anim_loop(state); 62 | } 63 | 64 | void led_anim_red(led_anim_t *anim) 65 | { 66 | led_anim_start(&led_anim_state_red, anim); 67 | } 68 | 69 | void led_anim_blue(led_anim_t *anim) 70 | { 71 | led_anim_start(&led_anim_state_blue, anim); 72 | } 73 | 74 | void led_setup() 75 | { 76 | pinMode(LED_STATUS, OUTPUT); 77 | pinMode(LED_ACTIVITY, OUTPUT); 78 | 79 | digitalWrite(LED_STATUS, LOW); 80 | digitalWrite(LED_ACTIVITY, LOW); 81 | } 82 | 83 | bool led_loop() 84 | { 85 | led_anim_loop(&led_anim_state_red); 86 | led_anim_loop(&led_anim_state_blue); 87 | 88 | return false; 89 | } 90 | 91 | -------------------------------------------------------------------------------- /WiFi.ino: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | const char *ssid = "x"; 5 | const char *password = "x"; 6 | bool connecting = false; 7 | bool wifi_enabled = false; 8 | 9 | 10 | void wifi_setup() 11 | { 12 | Serial.printf("[WiFi] Connecting...\n"); 13 | WiFi.begin(ssid, password); 14 | connecting = true; 15 | wifi_enabled = true; 16 | } 17 | 18 | bool wifi_loop(void) 19 | { 20 | int status = WiFi.status(); 21 | int curTime = millis(); 22 | static int nextTime = 0; 23 | static int stateCounter = 0; 24 | 25 | if(!wifi_enabled) 26 | { 27 | return false; 28 | } 29 | 30 | if(nextTime > curTime) 31 | { 32 | return false; 33 | } 34 | 35 | /* standard refresh time */ 36 | nextTime = curTime + 100; 37 | 38 | switch(status) 39 | { 40 | case WL_CONNECTED: 41 | if(connecting) 42 | { 43 | connecting = false; 44 | Serial.print("[WiFi] Connected, IP address: "); 45 | Serial.println(WiFi.localIP()); 46 | ota_setup(); 47 | } 48 | break; 49 | 50 | case WL_CONNECTION_LOST: 51 | Serial.printf("[WiFi] Connection lost\n"); 52 | connecting = false; 53 | WiFi.disconnect(); 54 | WiFi.mode(WIFI_OFF); 55 | nextTime = curTime + 500; 56 | break; 57 | 58 | case WL_CONNECT_FAILED: 59 | Serial.printf("[WiFi] Connection failed\n"); 60 | connecting = false; 61 | WiFi.disconnect(); 62 | WiFi.mode(WIFI_OFF); 63 | nextTime = curTime + 1000; 64 | break; 65 | 66 | case WL_NO_SSID_AVAIL: 67 | Serial.printf("[WiFi] No SSID\n"); 68 | connecting = false; 69 | WiFi.disconnect(); 70 | WiFi.mode(WIFI_OFF); 71 | nextTime = curTime + 2000; 72 | break; 73 | 74 | case WL_SCAN_COMPLETED: 75 | Serial.printf("[WiFi] Scan completed\n"); 76 | connecting = false; 77 | WiFi.disconnect(); 78 | WiFi.mode(WIFI_OFF); 79 | break; 80 | 81 | case WL_DISCONNECTED: 82 | if(!connecting) 83 | { 84 | Serial.printf("[WiFi] Disconnected\n"); 85 | connecting = false; 86 | WiFi.disconnect(); 87 | WiFi.mode(WIFI_OFF); 88 | break; 89 | } 90 | else 91 | { 92 | if(++stateCounter > 50) 93 | { 94 | Serial.printf("[WiFi] Timeout, aborting\n"); 95 | connecting = false; 96 | WiFi.disconnect(); 97 | WiFi.mode(WIFI_OFF); 98 | } 99 | } 100 | 101 | case WL_IDLE_STATUS: 102 | if(!connecting) 103 | { 104 | connecting = true; 105 | Serial.printf("[WiFi] Idle, connecting to %s\n", ssid); 106 | WiFi.mode(WIFI_STA); 107 | WiFi.begin(ssid, password); 108 | stateCounter = 0; 109 | break; 110 | } 111 | 112 | case WL_NO_SHIELD: 113 | if(!connecting) 114 | { 115 | connecting = true; 116 | Serial.printf("[WiFi] Disabled (%d), connecting to %s\n", status, ssid); 117 | WiFi.mode(WIFI_STA); 118 | WiFi.begin(ssid, password); 119 | stateCounter = 0; 120 | break; 121 | } 122 | } 123 | 124 | return false; 125 | } 126 | 127 | 128 | -------------------------------------------------------------------------------- /analogWrite.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * Licensed under the MIT license. 4 | * https://github.com/ERROPiX/ESP32_AnalogWrite 5 | */ 6 | 7 | #include "analogWrite.h" 8 | 9 | analog_write_channel_t _analog_write_channels[16] = { 10 | {-1, 5000, 13}, 11 | {-1, 5000, 13}, 12 | {-1, 5000, 13}, 13 | {-1, 5000, 13}, 14 | {-1, 5000, 13}, 15 | {-1, 5000, 13}, 16 | {-1, 5000, 13}, 17 | {-1, 5000, 13}, 18 | {-1, 5000, 13}, 19 | {-1, 5000, 13}, 20 | {-1, 5000, 13}, 21 | {-1, 5000, 13}, 22 | {-1, 5000, 13}, 23 | {-1, 5000, 13}, 24 | {-1, 5000, 13}, 25 | {-1, 5000, 13}}; 26 | 27 | int analogWriteChannel(uint8_t pin) 28 | { 29 | int channel = -1; 30 | 31 | // Check if pin already attached to a channel 32 | for (uint8_t i = 0; i < 16; i++) 33 | { 34 | if (_analog_write_channels[i].pin == pin) 35 | { 36 | channel = i; 37 | break; 38 | } 39 | } 40 | 41 | // If not, attach it to a free channel 42 | if (channel == -1) 43 | { 44 | for (uint8_t i = 0; i < 16; i++) 45 | { 46 | if (_analog_write_channels[i].pin == -1) 47 | { 48 | _analog_write_channels[i].pin = pin; 49 | channel = i; 50 | ledcSetup(channel, _analog_write_channels[i].frequency, _analog_write_channels[i].resolution); 51 | ledcAttachPin(pin, channel); 52 | break; 53 | } 54 | } 55 | } 56 | 57 | return channel; 58 | } 59 | 60 | void analogWriteFrequency(double frequency) 61 | { 62 | for (uint8_t i = 0; i < 16; i++) 63 | { 64 | _analog_write_channels[i].frequency = frequency; 65 | _analog_write_channels[i].pin = -1; 66 | } 67 | } 68 | 69 | void analogWriteFrequency(uint8_t pin, double frequency) 70 | { 71 | int channel = analogWriteChannel(pin); 72 | 73 | // Make sure the pin was attached to a channel, if not do nothing 74 | if (channel != -1 && channel < 16) 75 | { 76 | _analog_write_channels[channel].frequency = frequency; 77 | _analog_write_channels[channel].pin = -1; 78 | } 79 | } 80 | 81 | void analogWriteResolution(uint8_t resolution) 82 | { 83 | for (uint8_t i = 0; i < 16; i++) 84 | { 85 | _analog_write_channels[i].resolution = resolution; 86 | _analog_write_channels[i].pin = -1; 87 | } 88 | } 89 | 90 | void analogWriteResolution(uint8_t pin, uint8_t resolution) 91 | { 92 | int channel = analogWriteChannel(pin); 93 | 94 | // Make sure the pin was attached to a channel, if not do nothing 95 | if (channel != -1 && channel < 16) 96 | { 97 | _analog_write_channels[channel].resolution = resolution; 98 | _analog_write_channels[channel].pin = -1; 99 | } 100 | } 101 | 102 | void analogWrite(uint8_t pin, uint32_t value, uint32_t valueMax) 103 | { 104 | int channel = analogWriteChannel(pin); 105 | 106 | // Make sure the pin was attached to a channel, if not do nothing 107 | if (channel != -1 && channel < 16) 108 | { 109 | uint8_t resolution = _analog_write_channels[channel].resolution; 110 | uint32_t levels = pow(2, resolution); 111 | uint32_t duty = ((levels - 1) / valueMax) * min(value, valueMax); 112 | 113 | // write duty to LEDC 114 | ledcWrite(channel, duty); 115 | } 116 | } 117 | -------------------------------------------------------------------------------- /BLE.ino: -------------------------------------------------------------------------------- 1 | 2 | #include "LED.h" 3 | #include "Config.h" 4 | 5 | #define STEAM_CONTROLLER_BUTTON_A 0x800000 6 | #define STEAM_CONTROLLER_BUTTON_X 0x400000 7 | #define STEAM_CONTROLLER_BUTTON_B 0x200000 8 | #define STEAM_CONTROLLER_BUTTON_Y 0x100000 9 | #define STEAM_CONTROLLER_BUTTON_LEFT_UPPER_PADDLE 0x080000 10 | #define STEAM_CONTROLLER_BUTTON_RIGHT_UPPER_PADDLE 0x040000 11 | #define STEAM_CONTROLLER_BUTTON_LEFT_PADDLE 0x020000 12 | #define STEAM_CONTROLLER_BUTTON_RIGHT_PADDLE 0x010000 13 | #define STEAM_CONTROLLER_BUTTON_LEFT_INNER_PADDLE 0x008000 14 | #define STEAM_CONTROLLER_BUTTON_NAV_RIGHT 0x004000 15 | #define STEAM_CONTROLLER_BUTTON_STEAM 0x002000 16 | #define STEAM_CONTROLLER_BUTTON_NAV_LEFT 0x001000 17 | #define STEAM_CONTROLLER_BUTTON_JOYSTICK 0x000040 18 | #define STEAM_CONTROLLER_BUTTON_RIGHT_TOUCH 0x000010 19 | #define STEAM_CONTROLLER_BUTTON_LEFT_TOUCH 0x000008 20 | #define STEAM_CONTROLLER_BUTTON_RIGHT_PAD 0x000004 21 | #define STEAM_CONTROLLER_BUTTON_LEFT_PAD 0x000002 22 | #define STEAM_CONTROLLER_BUTTON_RIGHT_INNER_PADDLE 0x000001 23 | 24 | #define STEAM_CONTROLLER_FLAG_REPORT 0x0004 25 | #define STEAM_CONTROLLER_FLAG_BUTTONS 0x0010 26 | #define STEAM_CONTROLLER_FLAG_PADDLES 0x0020 27 | #define STEAM_CONTROLLER_FLAG_JOYSTICK 0x0080 28 | #define STEAM_CONTROLLER_FLAG_LEFT_PAD 0x0100 29 | #define STEAM_CONTROLLER_FLAG_RIGHT_PAD 0x0200 30 | 31 | 32 | static BLEUUID hidUUID("00001812-0000-1000-8000-00805f9b34fb"); /* controller announces this service */ 33 | static BLEUUID serviceUUID("100F6C32-1735-4313-B402-38567131E5F3"); /* but instead use this one */ 34 | static BLEUUID inputUUID("100F6C33-1735-4313-B402-38567131E5F3"); /* and this characterstic within it to receive the reports */ 35 | static BLEUUID rprtUUID("100F6C34-1735-4313-B402-38567131E5F3"); /* plus this one to configure the controller for sending */ 36 | uint8_t startReportCommand[] = { 0xC0, 0x87, 0x03, 0x08, 0x07, 0x00 }; /* command sent to enable report sending. see: https://github.com/haxpor/sdl2-samples/blob/master/android-project/app/src/main/java/org/libsdl/app/HIDDeviceBLESteamController.java */ 37 | 38 | static boolean doConnect = false; 39 | static boolean ble_connected = false; 40 | static boolean ble_scanning = false; 41 | static BLERemoteCharacteristic* ble_remote_characteristic; 42 | static BLEAdvertisedDevice* ble_remote_device; 43 | static uint32_t ble_last_buttons = 0; 44 | 45 | static bool ble_led_on = false; 46 | static bool ble_led_bright = false; 47 | static bool btn_steam = false; 48 | 49 | template int sgn(T val) 50 | { 51 | return (T(0) < val) - (val < T(0)); 52 | } 53 | 54 | static void parsePacket(uint8_t* buf, size_t len) 55 | { 56 | int pos = 0; 57 | 58 | if(buf[pos] != 0xc0) 59 | { 60 | Serial.printf("[SC] unknown reply 0x%02X\n", buf[pos]); 61 | return; 62 | } 63 | pos++; 64 | 65 | if((buf[pos] & 0x0f) == 0x05) 66 | { 67 | Serial.print("[SC] Idle: "); 68 | for(int pos = 0; pos < len; pos++) 69 | { 70 | Serial.printf("0x%02X ", buf[pos]); 71 | } 72 | Serial.println(); 73 | } 74 | else if((buf[pos] & 0x0f) == STEAM_CONTROLLER_FLAG_REPORT) 75 | { 76 | uint16_t flags = ((buf[pos+1] << 8) | buf[pos]) & ~0x0f; 77 | 78 | pos += 2; 79 | 80 | if(flags & STEAM_CONTROLLER_FLAG_BUTTONS) 81 | { 82 | uint32_t buttons = (buf[pos+0] << 16) | (buf[pos+1] << 8) | buf[pos+2]; 83 | 84 | #if 0 85 | Serial.printf("[SC] Buttons: 0x%08X ", buttons); 86 | if(buttons & STEAM_CONTROLLER_BUTTON_A) 87 | { 88 | Serial.printf("A, "); 89 | } 90 | if(buttons & STEAM_CONTROLLER_BUTTON_X) 91 | { 92 | Serial.printf("X, "); 93 | } 94 | if(buttons & STEAM_CONTROLLER_BUTTON_B) 95 | { 96 | Serial.printf("B, "); 97 | } 98 | if(buttons & STEAM_CONTROLLER_BUTTON_Y) 99 | { 100 | Serial.printf("Y, "); 101 | } 102 | if(buttons & STEAM_CONTROLLER_BUTTON_LEFT_PADDLE) 103 | { 104 | Serial.printf("left paddle, "); 105 | } 106 | if(buttons & STEAM_CONTROLLER_BUTTON_RIGHT_PADDLE) 107 | { 108 | Serial.printf("right paddle, "); 109 | } 110 | if(buttons & STEAM_CONTROLLER_BUTTON_LEFT_INNER_PADDLE) 111 | { 112 | Serial.printf("left inner paddle, "); 113 | } 114 | if(buttons & STEAM_CONTROLLER_BUTTON_RIGHT_INNER_PADDLE) 115 | { 116 | Serial.printf("right inner paddle, "); 117 | } 118 | if(buttons & STEAM_CONTROLLER_BUTTON_NAV_LEFT) 119 | { 120 | Serial.printf("arrow left, "); 121 | } 122 | if(buttons & STEAM_CONTROLLER_BUTTON_STEAM) 123 | { 124 | Serial.printf("Steam, "); 125 | } 126 | if(buttons & STEAM_CONTROLLER_BUTTON_NAV_RIGHT) 127 | { 128 | Serial.printf("arrow right, "); 129 | } 130 | if(buttons & STEAM_CONTROLLER_BUTTON_LEFT_PAD) 131 | { 132 | Serial.printf("left pad, "); 133 | } 134 | if(buttons & STEAM_CONTROLLER_BUTTON_RIGHT_PAD) 135 | { 136 | Serial.printf("right pad, "); 137 | } 138 | if(buttons & STEAM_CONTROLLER_BUTTON_RIGHT_TOUCH) 139 | { 140 | Serial.printf("right touch, "); 141 | } 142 | if(buttons & STEAM_CONTROLLER_BUTTON_LEFT_TOUCH) 143 | { 144 | Serial.printf("left touch, "); 145 | } 146 | if(buttons & STEAM_CONTROLLER_BUTTON_JOYSTICK) 147 | { 148 | Serial.printf("joy, "); 149 | } 150 | Serial.printf("\n"); 151 | #endif 152 | 153 | pos += 3; 154 | flags &= ~STEAM_CONTROLLER_FLAG_BUTTONS; 155 | 156 | if(!btn_steam && (buttons & STEAM_CONTROLLER_BUTTON_STEAM)) 157 | { 158 | lego_init(); 159 | btn_steam = true; 160 | } 161 | else if(btn_steam && !(buttons & STEAM_CONTROLLER_BUTTON_STEAM)) 162 | { 163 | btn_steam = false; 164 | } 165 | 166 | if(buttons & STEAM_CONTROLLER_BUTTON_A) 167 | { 168 | ble_led_bright = true; 169 | } 170 | else if(ble_last_buttons & STEAM_CONTROLLER_BUTTON_A) 171 | { 172 | ble_led_bright = false; 173 | } 174 | 175 | if(buttons & STEAM_CONTROLLER_BUTTON_B) 176 | { 177 | ble_led_on ^= true; 178 | } 179 | 180 | if(btn_steam) 181 | { 182 | if(buttons & STEAM_CONTROLLER_BUTTON_A) 183 | { 184 | current_config.invert[0] ^= true; 185 | printf("[CFG] Invert motor A: %s\n", current_config.invert[0] ? "yes" : "no"); 186 | config_modified = true; 187 | } 188 | 189 | if(buttons & STEAM_CONTROLLER_BUTTON_B) 190 | { 191 | current_config.invert[1] ^= true; 192 | printf("[CFG] Invert motor B: %s\n", current_config.invert[1] ? "yes" : "no"); 193 | config_modified = true; 194 | } 195 | 196 | if(buttons & STEAM_CONTROLLER_BUTTON_X) 197 | { 198 | current_config.swap ^= true; 199 | printf("[CFG] Swap motor A/B: %s\n", current_config.swap ? "yes" : "no"); 200 | config_modified = true; 201 | } 202 | 203 | if(buttons & STEAM_CONTROLLER_BUTTON_Y) 204 | { 205 | current_config.kinematics ^= true; 206 | printf("[CFG] Use tank kinematics: %s\n", current_config.kinematics ? "yes" : "no"); 207 | config_modified = true; 208 | } 209 | 210 | if(buttons & STEAM_CONTROLLER_BUTTON_NAV_RIGHT) 211 | { 212 | printf("[CFG] Enable WiFi\n"); 213 | wifi_setup(); 214 | } 215 | 216 | if(buttons & STEAM_CONTROLLER_BUTTON_NAV_LEFT) 217 | { 218 | printf("[CFG] Reboot\n"); 219 | ESP.restart(); 220 | } 221 | } 222 | 223 | ble_last_buttons = buttons; 224 | } 225 | 226 | if(flags & STEAM_CONTROLLER_FLAG_PADDLES) 227 | { 228 | uint8_t left = buf[pos]; 229 | uint8_t right = buf[pos+1]; 230 | #if 0 231 | printf("[SC] FrontPaddle: %d %d\n", left, right); 232 | #endif 233 | pos+=2; 234 | 235 | if(btn_steam) 236 | { 237 | printf("[CFG] set intensity: %d\n", right); 238 | current_config.light_intensity = right; 239 | } 240 | flags &= ~STEAM_CONTROLLER_FLAG_PADDLES; 241 | } 242 | 243 | if(flags & STEAM_CONTROLLER_FLAG_JOYSTICK) 244 | { 245 | int16_t joy_x = ((uint16_t)buf[pos+1] << 8 | buf[pos]); 246 | int16_t joy_y = ((uint16_t)buf[pos+3] << 8 | buf[pos+2]); 247 | 248 | float joy_x_f = (float)joy_x / 32760.0f; 249 | float joy_y_f = (float)joy_y / 32760.0f; 250 | 251 | if(current_config.kinematics) 252 | { 253 | kin_set(joy_x_f, joy_y_f); 254 | } 255 | else 256 | { 257 | lego_motor(0, joy_x_f); 258 | lego_motor(1, joy_y_f); 259 | } 260 | #if 0 261 | printf("[SC] Joystick: %d %d\n", joy_x, joy_y); 262 | #endif 263 | pos+=4; 264 | flags &= ~STEAM_CONTROLLER_FLAG_JOYSTICK; 265 | } 266 | 267 | if(flags & STEAM_CONTROLLER_FLAG_LEFT_PAD) 268 | { 269 | int16_t joy_x = (buf[pos+1] << 8 | buf[pos]); 270 | int16_t joy_y = (buf[pos+3] << 8 | buf[pos+2]); 271 | 272 | #if 0 273 | printf("[SC] LeftPad: %d %d\n", joy_x, joy_y); 274 | #endif 275 | pos+=4; 276 | flags &= ~STEAM_CONTROLLER_FLAG_LEFT_PAD; 277 | } 278 | 279 | if(flags & STEAM_CONTROLLER_FLAG_RIGHT_PAD) 280 | { 281 | int16_t joy_x = (buf[pos+1] << 8 | buf[pos]); 282 | int16_t joy_y = (buf[pos+3] << 8 | buf[pos+2]); 283 | 284 | #if 0 285 | printf("[SC] RightPad: %d %d\n", joy_x, joy_y); 286 | #endif 287 | pos+=4; 288 | flags &= ~STEAM_CONTROLLER_FLAG_RIGHT_PAD; 289 | } 290 | 291 | int ble_led_value = 0; 292 | if(ble_led_bright) 293 | { 294 | ble_led_value = 0xFF; 295 | } 296 | else if(ble_led_on) 297 | { 298 | ble_led_value = current_config.light_intensity; 299 | } 300 | 301 | lego_led(ble_led_value); 302 | 303 | if(flags) 304 | { 305 | Serial.print("[SC] still got flags: "); 306 | for(int pos = 0; pos < len; pos++) 307 | { 308 | Serial.printf("0x%02X ", buf[pos]); 309 | } 310 | Serial.println(); 311 | } 312 | } 313 | else 314 | { 315 | Serial.print("[SC] Unknown packet: "); 316 | for(int pos = 0; pos < len; pos++) 317 | { 318 | Serial.printf("0x%02X ", buf[pos]); 319 | } 320 | Serial.println(); 321 | } 322 | } 323 | 324 | static void report_cbr(BLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t* buf, size_t len, bool isNotify) 325 | { 326 | parsePacket(buf, len); 327 | } 328 | 329 | class MyClientCallback : public BLEClientCallbacks 330 | { 331 | void onConnect(BLEClient* pclient) 332 | { 333 | } 334 | 335 | void onDisconnect(BLEClient* pclient) 336 | { 337 | ble_connected = false; 338 | } 339 | }; 340 | 341 | bool connectToServer() 342 | { 343 | Serial.printf("[BLE] Connecting to %s\n", ble_remote_device->getAddress().toString().c_str()); 344 | 345 | BLEClient* pClient = BLEDevice::createClient(); 346 | pClient->setClientCallbacks(new MyClientCallback()); 347 | pClient->connect(ble_remote_device); 348 | 349 | #if 0 350 | Serial.println(" - Services: "); 351 | std::map *rmtServices = pClient->getServices(); 352 | 353 | for (std::map::iterator its = rmtServices->begin(); its != rmtServices->end(); ++its) 354 | { 355 | Serial.print(" "); 356 | Serial.print(its->first.c_str()); 357 | Serial.println(); 358 | 359 | Serial.println(" Characteristics: "); 360 | std::map* serviceCharacteristics = its->second->getCharacteristics(); 361 | 362 | if(!serviceCharacteristics) 363 | { 364 | Serial.println(" <> "); 365 | } 366 | else 367 | { 368 | for (std::map::iterator itc = serviceCharacteristics->begin(); itc != serviceCharacteristics->end(); ++itc) 369 | { 370 | Serial.print(" "); 371 | Serial.print(itc->first.c_str()); 372 | Serial.print(", "); 373 | Serial.print(itc->second->canNotify() ? "NOTIFY " : ""); 374 | Serial.print(itc->second->canRead() ? "READ " : ""); 375 | Serial.print(itc->second->canWrite() ? "WRITE " : ""); 376 | Serial.print(itc->second->canWriteNoResponse() ? "WRITE_NO_RESPONSE " : ""); 377 | Serial.println(); 378 | } 379 | } 380 | Serial.println(); 381 | } 382 | Serial.println(); 383 | 384 | #endif 385 | 386 | BLERemoteService* service = pClient->getService(serviceUUID); 387 | if (service == nullptr) 388 | { 389 | Serial.printf("[BLE] FAILED to find our service UUID: %s\n", serviceUUID.toString().c_str()); 390 | pClient->disconnect(); 391 | return false; 392 | } 393 | Serial.println("[BLE] service found"); 394 | 395 | ble_remote_characteristic = service->getCharacteristic(inputUUID); 396 | if (ble_remote_characteristic == nullptr) 397 | { 398 | Serial.printf("[BLE] FAILED to find our characteristic UUID: %s\n", inputUUID.toString().c_str()); 399 | pClient->disconnect(); 400 | return false; 401 | } 402 | Serial.println("[BLE] characteristic found"); 403 | 404 | 405 | if(ble_remote_characteristic->canNotify()) 406 | { 407 | ble_remote_characteristic->registerForNotify(report_cbr); 408 | Serial.println("[BLE] characteristic subscribed"); 409 | } 410 | 411 | BLERemoteCharacteristic *reportChar = service->getCharacteristic(rprtUUID); 412 | 413 | if(reportChar && reportChar->canWrite()) 414 | { 415 | reportChar->writeValue(startReportCommand, sizeof(startReportCommand)); 416 | } 417 | else 418 | { 419 | Serial.println("[BLE] FAILED to start reporting"); 420 | pClient->disconnect(); 421 | return false; 422 | } 423 | 424 | ble_connected = true; 425 | return true; 426 | } 427 | 428 | class BLENewDevice: public BLEAdvertisedDeviceCallbacks 429 | { 430 | void onResult(BLEAdvertisedDevice advertisedDevice) 431 | { 432 | bool hasServices = advertisedDevice.haveServiceUUID(); 433 | bool isHid = hasServices && advertisedDevice.isAdvertisingService(hidUUID); 434 | const char *devName = advertisedDevice.getName().c_str(); 435 | 436 | Serial.print("[BLE] Device found: "); 437 | Serial.print(devName); 438 | Serial.print(hasServices ? " " : " "); 439 | Serial.print(isHid ? " " : " "); 440 | Serial.println(); 441 | 442 | /* found a HID device which advertises as SteamController */ 443 | if (isHid && !strcmp(devName, "SteamController")) 444 | { 445 | Serial.printf("[BLE] connecting to '%s'\n", devName); 446 | BLEDevice::getScan()->stop(); 447 | ble_remote_device = new BLEAdvertisedDevice(advertisedDevice); 448 | ble_scanning = false; 449 | doConnect = true; 450 | } 451 | } 452 | }; 453 | 454 | void ble_scan_complete (BLEScanResults results) 455 | { 456 | Serial.println("[BLE] Scan finished"); 457 | ble_scanning = false; 458 | } 459 | 460 | void ble_setup() 461 | { 462 | Serial.println("[BLE] initializing"); 463 | BLEDevice::init(""); 464 | 465 | BLEScan *pBLEScan = BLEDevice::getScan(); 466 | pBLEScan->setAdvertisedDeviceCallbacks(new BLENewDevice()); 467 | Serial.println("[BLE] start intial scan"); 468 | pBLEScan->start(5, &ble_scan_complete, false); 469 | 470 | ble_scanning = true; 471 | } 472 | 473 | bool ble_loop() 474 | { 475 | bool busy = false; 476 | 477 | if(doConnect) 478 | { 479 | doConnect = false; 480 | 481 | if(connectToServer()) 482 | { 483 | Serial.println("[BLE] Connected"); 484 | led_anim_blue(led_anim_ble_connected); 485 | } 486 | else 487 | { 488 | Serial.println("[BLE] FAILED to connect"); 489 | led_anim_blue(led_anim_emergency); 490 | } 491 | } 492 | 493 | if(!ble_connected) 494 | { 495 | if(!ble_scanning) 496 | { 497 | Serial.println("[BLE] Restart scan"); 498 | BLEDevice::getScan()->start(5, &ble_scan_complete, false); 499 | ble_scanning = true; 500 | } 501 | led_anim_blue(led_anim_shortdoubleblinkloop); 502 | } 503 | 504 | return busy; 505 | } 506 | 507 | --------------------------------------------------------------------------------