tmp

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

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
 
 }

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

src/bafang_DisplaySlave.cpp

@@ -0,0 +1,132 @@
+#include <Arduino.h>
+
+#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()
+{
+
+}
+
+}
+}

src/bafang_MotorMaster.cpp

@@ -0,0 +1,115 @@
+#include <Arduino.h>
+
+#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);
+}
+
+}
+}

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;
+    Main::g_Debug[Main::g_CurrentChannel] = strtoul(argv[1], NULL, 10);
-    if(strtoul(argv[1], NULL, 10))
-        debug = true;
-
-    Main::g_Debug[Main::g_CurrentChannel] = debug;
-
     return 0;
 }
 
 int CMD_wifi(int argc, char **argv)
 {
+    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 != 2) {
-{
+        printUsage();
-    if(argc != 3) {
-        Main::channel()->println("Usage: serial <0|1|2> <text>");
         return 1;
     }
 
-    int port = strtoul(argv[1], NULL, 10);
+    if(strtoul(argv[1], NULL, 10))
-
+        Main::toggleWiFi(1);
-    if(port == 0)
+    else
-        Serial.println(argv[2]);
+        Main::toggleWiFi(0);
-    else if(port == 1)
-        Serial1.println(argv[2]);
-    else if(port == 2)
-        Serial2.println(argv[2]);
 
     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>"},
+    {"debug",       CMD_debug,      " : debug [0-9999]"},
-    {"wifi",        CMD_wifi,       " : show wifi config"},
+    {"wifi",        CMD_wifi,       " : wifi [0-1]"},
-    {"serial",      CMD_serial,     " : write to serial <0|1|2> <text>"},
+    {"bluetooth",   CMD_bluetooth,  " : bluetooth [0-1]"},
+    {"proxy",       CMD_proxy,      " : proxy <0|1>"},
+    {"motor",       CMD_motor,      " : motor <hex> [waitMs]"},
+    {"profile",     CMD_profile,    " : profile <0-1>"},
     { 0, 0, 0 }
 };
 

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;
+int g_CurrentChannel = CHANNEL_HWSERIAL;
-bool g_Debug[NUM_CHANNELS];
+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);
+    pinMode(THROTTLE_GPIO, INPUT);
-    Serial2.begin(115200);
+    pinMode(BRAKE_GPIO, INPUT);
 
-    WiFi.setHostname("ESP32-BAFANG");
+    Serial1.begin(1200, SERIAL_8N1, DISPLAY_RX, DISPLAY_TX); // Display
-    if(!WiFi.begin(g_WIFI_SSID, g_WIFI_Passphrase))
+    Serial2.begin(1200, SERIAL_8N1, MOTOR_RX, MOTOR_TX); // Motor
-        Serial.println("WiFi config error!");
-    else {
-        WiFi.setAutoConnect(true);
-    }
 
-    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);
+        channel()->printf("Error[%u]: ", error);
-        if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed");
+        if (error == OTA_AUTH_ERROR) channel()->println("Auth Failed");
-        else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed");
+        else if (error == OTA_BEGIN_ERROR) channel()->println("Begin Failed");
-        else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed");
+        else if (error == OTA_CONNECT_ERROR) channel()->println("Connect Failed");
-        else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed");
+        else if (error == OTA_RECEIVE_ERROR) channel()->println("Receive Failed");
-        else if (error == OTA_END_ERROR) Serial.println("End 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,19 +93,10 @@ Stream* channel(int num)
 
 void loop()
 {
-    if(g_Debug[g_CurrentChannel])
+    Bafang::tick();
-    {
-        while(Serial1.available())
-        {
-                channel()->write(Serial1.read());
-        }
 
-        while(Serial2.available())
+    if(WiFi.isConnected())
     {
-            channel()->write(Serial2.read());
-        }
-    }
-
         ArduinoOTA.handle();
 
         if(server.hasClient())
@@ -119,6 +107,7 @@ void loop()
         }
         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;
     }
 }
 

src/main.h

@@ -3,25 +3,32 @@
 
 #include <Arduino.h>
 
-#define MAX_SRV_CLIENTS 4
-
 namespace Main {
 
 enum {
-    NOSERIAL = -1,
+    CHANNEL_NONE = -1,
-    HWSERIAL = 0,
+    CHANNEL_HWSERIAL = 0,
-    BTSERIAL = 1,
+    CHANNEL_BTSERIAL = 1,
-    TCPSERIAL = 2,
+    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];