diff --git a/platformio.ini b/platformio.ini index 15c3f08..39c90d0 100644 --- a/platformio.ini +++ b/platformio.ini @@ -13,3 +13,14 @@ platform = espressif32 board = esp32dev framework = arduino board_build.partitions = partitions.csv + +upload_speed = 921600 + +monitor_speed = 115200 +monitor_flags = --echo +monitor_filters = esp32_exception_decoder + +lib_deps = https://github.com/plerup/espsoftwareserial.git + +platform_packages = + framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32.git diff --git a/src/bafang.cpp b/src/bafang.cpp index 1028aa3..ae557d6 100644 --- a/src/bafang.cpp +++ b/src/bafang.cpp @@ -4,7 +4,275 @@ #include "main.h" #include "bafang.h" -namespace Huawei { +namespace Bafang { +HardwareSerial &DisplaySerial = Serial1; +HardwareSerial &MotorSerial = Serial2; + +bool g_Proxy = true; + +static unsigned long s_LastDisplayRx = 0; +static unsigned long s_LastMotorRx = 0; + +static bool s_BlockDisplay = false; +static bool s_BlockMotor = false; + +static uint8_t s_MotorBuffer[255]; +static uint8_t s_MotorBufferLen = 0; + +enum eJobState { + STATE_NEW = 0, + STATE_SEND = 1, + STATE_RECV = 2 +}; + +struct sJob { + uint8_t *pData; + uint8_t dataLen; + uint16_t waitMs; + FCallback pfnCallback; + void *pUser; +}; + +static const int s_QueueSize = 8; +static sJob s_aQueue[s_QueueSize]; +static int s_QueueRdIdx = 0; +static int s_QueueWrIdx = 0; +static int s_QueueLen = 0; + +static eJobState s_JobState; +static unsigned long s_JobFinishTime = 0; +static void s_JobDone(); + + +void OnDisplayRx(uint8_t c) +{ + s_LastDisplayRx = millis(); + + if(s_BlockDisplay) + return; + + MotorSerial.write(c); +} + +void OnMotorRx(uint8_t c) +{ + s_LastMotorRx = millis(); + + if(s_BlockMotor) + { + if(s_MotorBufferLen <= sizeof(s_MotorBuffer)) + s_MotorBuffer[s_MotorBufferLen++] = c; + return; + } + + DisplaySerial.write(c); +} + +void recv() +{ + static int s_lastSerial = -1; + + while(Bafang::DisplaySerial.available()) + { + uint8_t c = Bafang::DisplaySerial.read(); + + if(Main::g_Debug[Main::g_CurrentChannel] > 10) + { + if(s_lastSerial != 1) + Main::channel()->write("\nD: "); + char sBuffer[4] = "XX "; + bytes2hex((uint8_t *)&c, 1, sBuffer, sizeof(sBuffer)); + Main::channel()->write(sBuffer); + s_lastSerial = 1; + } + + Bafang::OnDisplayRx(c); + } + + while(Bafang::MotorSerial.available()) + { + uint8_t c = Bafang::MotorSerial.read(); + + if(Main::g_Debug[Main::g_CurrentChannel] > 10) + { + if(s_lastSerial != 2) + Main::channel()->write("\nM: "); + char sBuffer[4] = "XX "; + bytes2hex((uint8_t *)&c, 1, sBuffer, sizeof(sBuffer)); + Main::channel()->write(sBuffer); + s_lastSerial = 2; + } + + Bafang::OnMotorRx(c); + } +} + +void tick() +{ + recv(); + + bool bProxy = g_Proxy; + + sJob *pJob = NULL; + if(s_QueueLen) + pJob = &s_aQueue[s_QueueRdIdx]; + + if(pJob) + { + bProxy = false; + + if(s_BlockDisplay && s_BlockMotor) + { + switch(s_JobState) + { + case STATE_NEW: + { + s_MotorBufferLen = 0; + s_JobFinishTime = millis() + (pJob->dataLen + 2) * BYTE_TIME + pJob->waitMs; + s_JobState = STATE_SEND; + MotorSerial.write(pJob->pData, pJob->dataLen); + } break; + case STATE_SEND: + { + if(s_MotorBufferLen) + { + s_JobState = STATE_RECV; + break; + } + + if(millis() >= s_JobFinishTime) + s_JobDone(); + } break; + case STATE_RECV: + { + if(millis() - s_LastMotorRx > 2 * BYTE_TIME) + s_JobDone(); + } break; + } + } + } + else if(s_MotorBufferLen && millis() - s_LastMotorRx > 2 * BYTE_TIME) + { + PrintCallback(Main::channel(), s_MotorBuffer, s_MotorBufferLen); + s_MotorBufferLen = 0; + } + + if(!bProxy) + { + if(millis() - s_LastDisplayRx > 2 * BYTE_TIME) + s_BlockDisplay = true; + + if(s_BlockDisplay && millis() - s_LastMotorRx > 2 * BYTE_TIME) + s_BlockMotor = true; + } + else + { + if(s_BlockDisplay && millis() - s_LastDisplayRx > 2 * BYTE_TIME) + { + s_BlockDisplay = false; + s_BlockMotor = false; + } + } +} + +static void s_JobDone() +{ + if(!s_QueueLen) + return; + + sJob &Job = s_aQueue[s_QueueRdIdx]; + + if(Job.pfnCallback) + Job.pfnCallback(Job.pUser, s_MotorBuffer, s_MotorBufferLen); + + delete Job.pData; + Job.pData = NULL; + Job.dataLen = 0; + Job.waitMs = 0; + Job.pfnCallback = NULL; + Job.pUser = NULL; + + s_QueueLen--; + s_QueueRdIdx++; + if(s_QueueRdIdx >= s_QueueSize) + s_QueueRdIdx = 0; + + s_JobState = STATE_NEW; +} + +bool QueueMotor(const uint8_t *pData, uint8_t dataLen, FCallback pfnCallback, void *pUser, uint16_t waitMs) +{ + if(!dataLen) + return false; + + if(s_QueueLen >= s_QueueSize) + return false; + + sJob &Job = s_aQueue[s_QueueWrIdx]; + Job.pData = new uint8_t[dataLen]; + memcpy(Job.pData, pData, dataLen); + Job.dataLen = dataLen; + Job.waitMs = waitMs; + Job.pfnCallback = pfnCallback; + Job.pUser = pUser; + + s_QueueLen++; + s_QueueWrIdx++; + if(s_QueueWrIdx >= s_QueueSize) + s_QueueWrIdx = 0; + + s_JobState = STATE_NEW; + return true; +} + +void PrintCallback(void *pUser, const uint8_t *pData, uint8_t dataLen) +{ + int channel = (int)pUser; + if(channel == Main::CHANNEL_NONE) + return; + + for(int i = 0; i < dataLen; i++) + Main::channel(channel)->printf("%02X", pData[i]); + Main::channel(channel)->write('\n'); +} + +void OnDisplayRequest(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen) +{ + char aBuf[512]; + bytes2hex(data, dataLen, aBuf, sizeof(aBuf)); + Serial.printf("Display: %s\n", aBuf); +} + +void OnMotorResponse(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen) +{ + char aBuf[512]; + bytes2hex(data, dataLen, aBuf, sizeof(aBuf)); + Serial.printf("Motor: %s\n", aBuf); +} + +int TransformPASCode(uint8_t PasCode) +{ + static const uint8_t PasLevelArray[] = { + PAS_LEVEL_0, PAS_LEVEL_1, PAS_LEVEL_2, PAS_LEVEL_3, + PAS_LEVEL_4, PAS_LEVEL_5, PAS_LEVEL_6, PAS_LEVEL_7, + PAS_LEVEL_8, PAS_LEVEL_9, PAS_LEVEL_PUSH + }; + + for(int i = 0; i < sizeof(PasLevelArray)/sizeof(*PasLevelArray); i++) + { + if(PasCode == PasLevelArray[i]) + return i; + } + + return -1; +} + +// rpm = 60 * v / (2 * pi * r) +// Example: 29inch tire, 30km/h +// v: 30 km/h / 3.6 = 8.33 m/s +// r: d = 29in * 0.7366m = -> r = 0.7366m / 2 = 0.3683m +// rps = 8.33 / (2 * pi * 0.3683) = 3.60 +// rpm = 60 * 3.60 = 216 } diff --git a/src/bafang.h b/src/bafang.h index 59060a7..e8c1140 100644 --- a/src/bafang.h +++ b/src/bafang.h @@ -4,8 +4,114 @@ namespace Bafang { +#define BAUD_RATE 1200 +#define BYTE_TIME (1000/((BAUD_RATE)/(1+8+1)) + 1) // Start + 8 Data + Stop +extern HardwareSerial &DisplaySerial; +extern HardwareSerial &MotorSerial; +extern bool g_Proxy; +void OnDisplayRx(uint8_t c); +void OnMotorRx(uint8_t c); + +void tick(); + +typedef void (*FCallback)(void *pUser, const uint8_t *pData, uint8_t dataLen); +bool QueueMotor(const uint8_t *pData, uint8_t dataLen, FCallback pfnCallback, void *pUser, uint16_t waitMs=0); +void PrintCallback(void *pUser, const uint8_t *pData, uint8_t dataLen); + +void OnDisplayRequest(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen); +void OnMotorResponse(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen); + +int TransformPASCode(uint8_t PasCode); + +namespace DisplaySlave { + +void OnData(uint8_t c); +void tick(); + +bool ReplyRawPacket(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen); + +} + +namespace MotorMaster { + +void OnData(uint8_t c); +void tick(); + +void SendRawPacket(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen); + +} + +enum eMode { + MODE_READ = 0x11, + MODE_WRITE = 0x16, +}; + +enum eCommand { + REQ_VOLTAGE = 0x01, + REQ_ERROR = 0x08, + REQ_CURRENT = 0x0A, + REQ_BAT = 0x11, + REQ_RPM = 0x20, + REQ_MOVING = 0x31, + REQ_UNK01 = 0x90, + REQ_UNK02 = 0x21, + REQ_UNK03 = 0x22, + REQ_UNK04 = 0x24, + REQ_UNK05 = 0x25, + REQ_UNKMENU = 0x60, + REQ_INFO = 0x51, + REQ_BASIC = 0x52, + REQ_PEDAL = 0x53, + REQ_THROTTLE = 0x54, + + CMD_LIGHT = 0x1A, + CMD_PAS_LEVEL = 0x0B, + CMD_RPMLIMIT = 0x1F +}; + +struct sPacketDesc { + eCommand cmd; + struct sInfo { + int len; + bool crc; + }; + sInfo req; + sInfo res; +}; + +const sPacketDesc PacketDesc[] = { + {REQ_VOLTAGE, {0, false}, {2, false}}, + {REQ_ERROR, {0, false}, {1, false}}, + {REQ_CURRENT, {0, false}, {1, true}}, + {REQ_BAT, {0, false}, {1, true}}, + {REQ_RPM, {0, false}, {3, false}}, + {REQ_MOVING, {0, false}, {1, true}}, + {REQ_UNK01, {0, false}, {2, true}}, + {REQ_UNK02, {0, true}, {1, false}}, + {REQ_UNK03, {0, true}, {1, false}}, + {REQ_UNK04, {0, true}, {1, false}}, + {REQ_UNK05, {0, true}, {1, false}}, + {REQ_UNKMENU, {0, true}, {0, false}}, + {CMD_LIGHT, {1, false}, {1, false}}, + {CMD_PAS_LEVEL, {1, true}, {0, false}}, + {CMD_RPMLIMIT, {2, true}, {0, false}} +}; + +enum { + PAS_LEVEL_0 = 0, + PAS_LEVEL_1 = 1, + PAS_LEVEL_2 = 12, + PAS_LEVEL_3 = 13, + PAS_LEVEL_4 = 14, + PAS_LEVEL_5 = 2, + PAS_LEVEL_6 = 21, + PAS_LEVEL_7 = 22, + PAS_LEVEL_8 = 23, + PAS_LEVEL_9 = 3, + PAS_LEVEL_PUSH = 6 +}; } #endif diff --git a/src/bafang_DisplaySlave.cpp b/src/bafang_DisplaySlave.cpp new file mode 100644 index 0000000..8a34db3 --- /dev/null +++ b/src/bafang_DisplaySlave.cpp @@ -0,0 +1,132 @@ +#include + +#include "utils.h" +#include "main.h" +#include "bafang.h" + +namespace Bafang { +namespace DisplaySlave { + +enum eDisplayState { + STATE_IDLE = 0, + STATE_REQUEST = 1, + STATE_RESPONSE = 2 +}; + +eDisplayState g_State; +uint8_t g_RxBuffer[255]; +uint8_t g_RxBufferLen = 0; +unsigned long g_LastRx = 0; + +void OnData(uint8_t c) +{ + static const sPacketDesc *packet = NULL; + + if(g_State == STATE_REQUEST && millis() - g_LastRx > 2 * BYTE_TIME) + g_State = STATE_IDLE; + + if(g_State == STATE_RESPONSE) + g_State = STATE_IDLE; + + if(g_State == STATE_IDLE) + { + packet = NULL; + g_RxBufferLen = 0; + } + + if(g_RxBufferLen >= sizeof(g_RxBuffer)) + return; + + g_RxBuffer[g_RxBufferLen++] = c; + + switch(g_State) + { + case STATE_IDLE: + { + if(c == MODE_READ || c == MODE_WRITE) + g_State = STATE_REQUEST; + } break; + + case STATE_REQUEST: + { + if(g_RxBufferLen == 2) + { + packet = NULL; + for(int i = 0; i < sizeof(PacketDesc)/sizeof(*PacketDesc); i++) + { + if(PacketDesc[i].cmd == c) + { + packet = &PacketDesc[i]; + break; + } + } + } + + if(!packet) + { + Serial.printf("Display: !!! UNKNOWN CMD %X\n", c); + g_State = STATE_IDLE; + break; + } + const sPacketDesc::sInfo &info = packet->req; + + if(info.len < 0) + { + g_State = STATE_IDLE; + break; + } + + int readLen = 2 + info.len; + if(info.crc) + readLen++; + + if(g_RxBufferLen < readLen) + break; + + if(info.crc) + { + uint8_t crc = 0; + for(int i = 0; i < g_RxBufferLen - 1; i++) + crc += g_RxBuffer[i]; + + if(c != crc) + { + char bla[16]; + bytes2hex(g_RxBuffer, g_RxBufferLen, bla, sizeof(bla)); + Serial.printf("!!! CRC ERROR %X != %X -> %s\n", c, crc, bla); + g_State = STATE_IDLE; + break; + } + } + + if(packet->res.len > 0) + g_State = STATE_RESPONSE; + else + g_State = STATE_IDLE; + + OnDisplayRequest(packet, g_RxBuffer, g_RxBufferLen); + } break; + + default: break; + } + + g_LastRx = millis(); +} + +bool ReplyRawPacket(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen) +{ + if(g_State != STATE_RESPONSE) + return false; + + DisplaySerial.write(data, dataLen); + g_State = STATE_IDLE; + return true; +} + +void tick() +{ + +} + +} +} \ No newline at end of file diff --git a/src/bafang_MotorMaster.cpp b/src/bafang_MotorMaster.cpp new file mode 100644 index 0000000..f3379ea --- /dev/null +++ b/src/bafang_MotorMaster.cpp @@ -0,0 +1,115 @@ +#include + +#include "utils.h" +#include "main.h" +#include "bafang.h" + +namespace Bafang { +namespace MotorMaster { + +typedef void (*CommandCallback)(void *pUser, uint8_t *pData, int dataLen, bool crcError); + +struct MotorCommand { + uint8_t *pSend; + uint8_t len; + bool crc; + CommandCallback pfnCallback; + void *pUser; +}; + +enum eMotorState { + STATE_IDLE = 0, + STATE_AWAITING = 1, + STATE_SUPERFLUOUS = 2 +}; + +eMotorState g_State; +uint8_t g_RxBuffer[255]; +uint8_t g_RxBufferLen = 0; +unsigned long g_LastRx = 0; + +const sPacketDesc *g_pPacketDesc = NULL; + +void OnData(uint8_t c) +{ + if(!g_RxBufferLen) + g_LastRx = millis(); + + if(millis() - g_LastRx > 2 * BYTE_TIME) + { + g_State = STATE_IDLE; + g_RxBufferLen = 0; + g_pPacketDesc = NULL; + } + + if(g_State == STATE_IDLE) + g_State = STATE_SUPERFLUOUS; + + if(g_RxBufferLen >= sizeof(g_RxBuffer)) + return; + + g_RxBuffer[g_RxBufferLen++] = c; + + if(g_pPacketDesc) + { + const sPacketDesc::sInfo &info = g_pPacketDesc->res; + + if(info.len < 0) + { + g_State = STATE_IDLE; + return; + } + + int readLen = info.len; + if(info.crc) + readLen++; + + if(g_RxBufferLen < readLen) + return; + + if(info.crc) + { + uint8_t crc = 0; + for(int i = 0; i < g_RxBufferLen - 1; i++) + crc += g_RxBuffer[i]; + + if(c != crc) + { + char bla[16]; + bytes2hex(g_RxBuffer, g_RxBufferLen, bla, sizeof(bla)); + Serial.printf("!!! CRC ERROR %X != %X -> %s\n", c, crc, bla); + g_State = STATE_IDLE; + return; + } + } + + OnMotorResponse(g_pPacketDesc, g_RxBuffer, g_RxBufferLen); + g_State = STATE_IDLE; + g_RxBufferLen = 0; + g_pPacketDesc = NULL; + } + + g_LastRx = millis(); +} + +void tick() +{ + if((g_State == STATE_SUPERFLUOUS || g_State == STATE_AWAITING) && g_RxBufferLen && millis() - g_LastRx > 2 * BYTE_TIME) + { + OnMotorResponse(g_pPacketDesc, g_RxBuffer, g_RxBufferLen); + g_State = STATE_IDLE; + g_RxBufferLen = 0; + g_pPacketDesc = NULL; + } +} + +void SendRawPacket(const sPacketDesc *packet, uint8_t *data, uint8_t dataLen) +{ + g_State = STATE_AWAITING; + g_RxBufferLen = 0; + g_pPacketDesc = packet; + MotorSerial.write(data, dataLen); +} + +} +} diff --git a/src/commands.cpp b/src/commands.cpp index 3196790..d08bfcc 100644 --- a/src/commands.cpp +++ b/src/commands.cpp @@ -8,6 +8,8 @@ namespace Commands { +static CommandEntry *s_pCurrentCommand = NULL; + int parseLine(char *line) { const int MAX_ARGV = 16; @@ -61,7 +63,9 @@ int parseLine(char *line) { if(strcmp(g_Commands[i].cmd, argv[0]) == 0) { + s_pCurrentCommand = &g_Commands[i]; int ret = g_Commands[i].fun(argc, argv); + s_pCurrentCommand = NULL; return ret; } } @@ -69,6 +73,14 @@ int parseLine(char *line) return -1; } +void printUsage() +{ + if(!s_pCurrentCommand) + return; + Main::channel()->print("Usage: "); + Main::channel()->println(s_pCurrentCommand->help); +} + int CMD_help(int argc, char **argv) { Main::channel()->println("Available Commands\n------------------"); @@ -86,54 +98,123 @@ int CMD_help(int argc, char **argv) int CMD_debug(int argc, char **argv) { + if(argc == 1) { + Serial.printf("Channel %d -> Debug: %d\n", Main::g_CurrentChannel, + Main::g_Debug[Main::g_CurrentChannel]); + return 0; + } + if(argc != 2) { - Main::channel()->println("Usage: debug <0|1>"); + printUsage(); return 1; } - bool debug = false; - if(strtoul(argv[1], NULL, 10)) - debug = true; - - Main::g_Debug[Main::g_CurrentChannel] = debug; - + Main::g_Debug[Main::g_CurrentChannel] = strtoul(argv[1], NULL, 10); return 0; } int CMD_wifi(int argc, char **argv) { - Main::channel()->printf("connected: %d\n", WiFi.status()); - Main::channel()->println(WiFi.localIP()); - WiFi.printDiag(*Main::channel()); - return 0; -} + if(argc == 1) + { + Main::channel()->printf("connected: %d\n", WiFi.status()); + Main::channel()->println(WiFi.localIP()); + if(WiFi.status() == WL_CONNECTED) + WiFi.printDiag(*Main::channel()); + return 0; + } -int CMD_serial(int argc, char **argv) -{ - if(argc != 3) { - Main::channel()->println("Usage: serial <0|1|2> "); + if(argc != 2) { + printUsage(); return 1; } - int port = strtoul(argv[1], NULL, 10); - - if(port == 0) - Serial.println(argv[2]); - else if(port == 1) - Serial1.println(argv[2]); - else if(port == 2) - Serial2.println(argv[2]); + if(strtoul(argv[1], NULL, 10)) + Main::toggleWiFi(1); + else + Main::toggleWiFi(0); return 0; } +int CMD_bluetooth(int argc, char **argv) +{ + if(argc == 1) + { + Main::channel()->printf("enabled: %d\n", btStarted()); + return 0; + } + + if(argc != 2) { + printUsage(); + return 1; + } + + if(strtoul(argv[1], NULL, 10)) + Main::toggleBluetooth(1); + else + Main::toggleBluetooth(0); + + return 0; +} + +int CMD_proxy(int argc, char **argv) +{ + if(argc != 2) { + printUsage(); + return 1; + } + + bool proxy = false; + if(strtoul(argv[1], NULL, 10)) + proxy = true; + + Bafang::g_Proxy = proxy; + + return 0; +} + +int CMD_motor(int argc, char **argv) +{ + if(argc < 2 || argc > 3) { + printUsage(); + return 1; + } + + uint8_t bytes[255]; + int len = hex2bytes(argv[1], bytes, sizeof(bytes)); + + uint16_t waitMs = 500; + if(argc >= 3) + waitMs = strtoul(argv[2], NULL, 10); + + Bafang::QueueMotor(bytes, len, Bafang::PrintCallback, (void *)Main::g_CurrentChannel, waitMs); + + return 0; +} + +int CMD_profile(int argc, char **argv) +{ + if(argc != 2) { + printUsage(); + return 1; + } + + int profile = strtoul(argv[1], NULL, 10); + Main::motorProfile(profile); + + return 0; +} CommandEntry g_Commands[] = { {"help", CMD_help, " : Display list of commands"}, - {"debug", CMD_debug, " : debug <0|1>"}, - {"wifi", CMD_wifi, " : show wifi config"}, - {"serial", CMD_serial, " : write to serial <0|1|2> "}, + {"debug", CMD_debug, " : debug [0-9999]"}, + {"wifi", CMD_wifi, " : wifi [0-1]"}, + {"bluetooth", CMD_bluetooth, " : bluetooth [0-1]"}, + {"proxy", CMD_proxy, " : proxy <0|1>"}, + {"motor", CMD_motor, " : motor [waitMs]"}, + {"profile", CMD_profile, " : profile <0-1>"}, { 0, 0, 0 } }; diff --git a/src/main.cpp b/src/main.cpp index eae9121..152493b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,8 +6,17 @@ #include "bafang.h" #include "commands.h" +#include "utils.h" #include "main.h" +#define DISPLAY_RX 18 +#define DISPLAY_TX 19 +#define MOTOR_RX 16 +#define MOTOR_TX 17 + +#define THROTTLE_GPIO 32 +#define BRAKE_GPIO 25 + WiFiServer server(23); WiFiClient serverClient; @@ -15,36 +24,31 @@ BluetoothSerial SerialBT; DummyStream SerialDummy; const char g_WIFI_SSID[] = "BotoX"; -const char g_WIFI_Passphrase[] = "D4701B981E5F34EF087DE8DA25F19B47F48E0FEA972FD10E3E6CFEE29C431A1F"; +const char g_WIFI_Passphrase[] = ""; namespace Main { -int g_CurrentChannel = HWSERIAL; -bool g_Debug[NUM_CHANNELS]; +int g_CurrentChannel = CHANNEL_HWSERIAL; +int g_Debug[NUM_CHANNELS]; char g_SerialBuffer[NUM_CHANNELS][255]; int g_SerialBufferPos[NUM_CHANNELS]; -unsigned long g_Time1000; - - void init() { + setCpuFrequencyMhz(80); + Serial.begin(115200); - while(!Serial); Serial.println("BOOTED!"); - Serial1.begin(115200); - Serial2.begin(115200); + pinMode(THROTTLE_GPIO, INPUT); + pinMode(BRAKE_GPIO, INPUT); - WiFi.setHostname("ESP32-BAFANG"); - if(!WiFi.begin(g_WIFI_SSID, g_WIFI_Passphrase)) - Serial.println("WiFi config error!"); - else { - WiFi.setAutoConnect(true); - } + Serial1.begin(1200, SERIAL_8N1, DISPLAY_RX, DISPLAY_TX); // Display + Serial2.begin(1200, SERIAL_8N1, MOTOR_RX, MOTOR_TX); // Motor - SerialBT.begin("ESP32-BAFANG"); + Bafang::DisplaySerial = Serial1; + Bafang::MotorSerial = Serial2; ArduinoOTA.onStart([]() { String type; @@ -54,29 +58,22 @@ void init() type = "filesystem"; // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end() - Serial.println("Start updating " + type); + channel()->println("Start updating " + type); }) .onEnd([]() { - Serial.println("\nEnd"); + channel()->println("\nEnd"); }) .onProgress([](unsigned int progress, unsigned int total) { - Serial.printf("Progress: %u%%\r", (progress / (total / 100))); + channel()->printf("Progress: %u%%\r", (progress / (total / 100))); }) .onError([](ota_error_t error) { - Serial.printf("Error[%u]: ", error); - if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed"); - else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed"); - else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed"); - else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed"); - else if (error == OTA_END_ERROR) Serial.println("End Failed"); + channel()->printf("Error[%u]: ", error); + if (error == OTA_AUTH_ERROR) channel()->println("Auth Failed"); + else if (error == OTA_BEGIN_ERROR) channel()->println("Begin Failed"); + else if (error == OTA_CONNECT_ERROR) channel()->println("Connect Failed"); + else if (error == OTA_RECEIVE_ERROR) channel()->println("Receive Failed"); + else if (error == OTA_END_ERROR) channel()->println("End Failed"); }); - - ArduinoOTA.begin(); - - server.begin(); - server.setNoDelay(true); - - } Stream* channel(int num) @@ -84,11 +81,11 @@ Stream* channel(int num) if(num == -1) num = g_CurrentChannel; - if(num == BTSERIAL && SerialBT.hasClient()) + if(num == CHANNEL_BTSERIAL && SerialBT.hasClient()) return &SerialBT; - else if(num == TCPSERIAL && serverClient) + else if(num == CHANNEL_TCPSERIAL && serverClient) return &serverClient; - else if(num == HWSERIAL) + else if(num == CHANNEL_HWSERIAL) return &Serial; return &SerialDummy; @@ -96,29 +93,21 @@ Stream* channel(int num) void loop() { - if(g_Debug[g_CurrentChannel]) + Bafang::tick(); + + if(WiFi.isConnected()) { - while(Serial1.available()) + ArduinoOTA.handle(); + + if(server.hasClient()) { - channel()->write(Serial1.read()); + if(serverClient) // disconnect current client if any + serverClient.stop(); + serverClient = server.available(); } - - while(Serial2.available()) - { - channel()->write(Serial2.read()); - } - } - - ArduinoOTA.handle(); - - if(server.hasClient()) - { - if(serverClient) // disconnect current client if any + if(!serverClient) serverClient.stop(); - serverClient = server.available(); } - if(!serverClient) - serverClient.stop(); for(int i = 0; i < NUM_CHANNELS; i++) { @@ -141,9 +130,155 @@ void loop() } } - if((millis() - g_Time1000) > 1000) + SecretFunction(); +} + +void SecretFunction() +{ + static bool s_bThrottle = false; + static unsigned long s_firstThrottleTime = 0; + static int s_throttleCount = 0; + static bool s_bTriggered = false; + + bool bBrake = !digitalRead(BRAKE_GPIO); + uint16_t throttleVal = analogRead(THROTTLE_GPIO); + + if(bBrake) { - g_Time1000 = millis(); + if(throttleVal > 1100 && !s_bThrottle) + { + s_bThrottle = true; + if(!s_throttleCount) + s_firstThrottleTime = millis(); + s_throttleCount++; + } + else if(throttleVal < 1000 && s_bThrottle) + { + s_bThrottle = false; + } + + if(!s_bTriggered && s_throttleCount && millis() - s_firstThrottleTime > 3000) + { + s_bTriggered = true; + SecretAction(s_throttleCount); + } + } + else + { + if(!s_bTriggered && s_throttleCount) + SecretAction(s_throttleCount); + s_bTriggered = false; + s_firstThrottleTime = 0; + s_throttleCount = 0; + } +} + +void SecretAction(int code) +{ + if(g_Debug[g_CurrentChannel]) + channel()->printf("SecretAction: %d\n", code); + + switch(code) + { + case 3: + case 4: { + motorProfile(1); + } break; + case 5: { + toggleBluetooth(); + } break; + case 6: { + toggleWiFi(); + } break; + } +} + +void toggleBluetooth(int mode) +{ + if(mode == -1) + mode = !btStarted(); + + if(mode == 1) + { + if(btStarted()) + { + channel()->println("Bluetooth already enabled."); + return; + } + + channel()->println("Enabling bluetooth..."); + SerialBT.begin("ESP32-BAFANG"); + } + else if(mode == 0) + { + if(!btStarted()) + { + channel()->println("Bluetooth already disabled."); + return; + } + + channel()->println("Disabling bluetooth..."); + SerialBT.end(); + } +} + +void toggleWiFi(int mode) +{ + if(mode == -1) + mode = !(WiFi.getMode() == WIFI_MODE_NULL); + + if(mode == 1) + { + if(WiFi.getMode() != WIFI_MODE_NULL) + { + channel()->println("WiFi already enabled."); + return; + } + + channel()->println("Enabling WiFi..."); + WiFi.begin(g_WIFI_SSID, g_WIFI_Passphrase); + WiFi.setHostname("ESP32-BAFANG"); + ArduinoOTA.begin(); + server.begin(); + server.setNoDelay(true); + } + else if(mode == 0) + { + if(WiFi.getMode() == WIFI_MODE_NULL) + { + channel()->println("WiFi already disabled."); + return; + } + + channel()->println("Disabling WiFi..."); + ArduinoOTA.end(); + server.end(); + WiFi.disconnect(true, true); + } +} + +void motorProfile(int profile) +{ + switch(profile) + { + case 0: { // Unlimited + const uint8_t basic[] = {0x16,0x52,0x18,0x26,0x1E,0x01,0x14,0x19,0x1E,0x23,0x28,0x2D,0x32,0x37,0x64,0x01,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x3A,0x01,0xFF}; + const uint8_t pedal[] = {0x16,0x53,0x0B,0x03,0xFF,0x28,0x0A,0x04,0x04,0xFF,0x0A,0x08,0x00,0x50,0xFB}; + const uint8_t throttle[] = {0x16,0x54,0x06,0x0B,0x2A,0x01,0x09,0x28,0x0F,0xD0}; + + Bafang::QueueMotor(basic, sizeof(basic), Bafang::PrintCallback, (void *)g_CurrentChannel, 500); + Bafang::QueueMotor(pedal, sizeof(pedal), Bafang::PrintCallback, (void *)g_CurrentChannel, 500); + Bafang::QueueMotor(throttle, sizeof(throttle), Bafang::PrintCallback, (void *)g_CurrentChannel, 500); + } break; + case 1: { // Limited 10A 26km/h + const uint8_t basic[] = {0x16,0x52,0x18,0x26,0x0A,0x01,0x14,0x19,0x1E,0x23,0x28,0x2D,0x32,0x37,0x64,0x01,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x3A,0x01,0xEB}; + const uint8_t pedal[] = {0x16,0x53,0x0B,0x03,0xFF,0x1A,0x0A,0x04,0x04,0xFF,0x0A,0x08,0x00,0x50,0xED}; + const uint8_t throttle[] = {0x16,0x54,0x06,0x0B,0x2A,0x01,0x09,0x1A,0x0F,0xC2}; + + Bafang::QueueMotor(basic, sizeof(basic), Bafang::PrintCallback, (void *)g_CurrentChannel, 500); + Bafang::QueueMotor(pedal, sizeof(pedal), Bafang::PrintCallback, (void *)g_CurrentChannel, 500); + Bafang::QueueMotor(throttle, sizeof(throttle), Bafang::PrintCallback, (void *)g_CurrentChannel, 500); + } break; } } diff --git a/src/main.h b/src/main.h index a700d98..fd83129 100644 --- a/src/main.h +++ b/src/main.h @@ -3,25 +3,32 @@ #include -#define MAX_SRV_CLIENTS 4 - namespace Main { enum { - NOSERIAL = -1, - HWSERIAL = 0, - BTSERIAL = 1, - TCPSERIAL = 2, + CHANNEL_NONE = -1, + CHANNEL_HWSERIAL = 0, + CHANNEL_BTSERIAL = 1, + CHANNEL_TCPSERIAL = 2, NUM_CHANNELS }; void init(); void loop(); -Stream* channel(int num = -1); +Stream *channel(int num = -1); + +void SecretFunction(); +void SecretAction(int code); + +void toggleBluetooth(int mode = -1); +void toggleWiFi(int mode = -1); + +void motorProfile(int profile); + extern int g_CurrentChannel; -extern bool g_Debug[NUM_CHANNELS]; +extern int g_Debug[NUM_CHANNELS]; extern char g_SerialBuffer[NUM_CHANNELS][255]; extern int g_SerialBufferPos[NUM_CHANNELS];