tmp
This commit is contained in:
parent
42e7c98d46
commit
e497346b77
@ -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
|
||||
|
||||
270
src/bafang.cpp
270
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
|
||||
|
||||
}
|
||||
|
||||
106
src/bafang.h
106
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
|
||||
|
||||
132
src/bafang_DisplaySlave.cpp
Normal file
132
src/bafang_DisplaySlave.cpp
Normal file
@ -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()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
115
src/bafang_MotorMaster.cpp
Normal file
115
src/bafang_MotorMaster.cpp
Normal file
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
127
src/commands.cpp
127
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)
|
||||
{
|
||||
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> <text>");
|
||||
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> <text>"},
|
||||
{"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 <hex> [waitMs]"},
|
||||
{"profile", CMD_profile, " : profile <0-1>"},
|
||||
{ 0, 0, 0 }
|
||||
};
|
||||
|
||||
|
||||
231
src/main.cpp
231
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,19 +93,10 @@ Stream* channel(int num)
|
||||
|
||||
void loop()
|
||||
{
|
||||
if(g_Debug[g_CurrentChannel])
|
||||
{
|
||||
while(Serial1.available())
|
||||
{
|
||||
channel()->write(Serial1.read());
|
||||
}
|
||||
Bafang::tick();
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
23
src/main.h
23
src/main.h
@ -3,25 +3,32 @@
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#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];
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user