Add ControlMode For Hover

Signed-off-by: Lukas Bachschwell <lukas@lbsfilm.at>
This commit is contained in:
Lukas Bachschwell 2024-05-18 15:35:43 +02:00
parent f061d98098
commit b1050c300c
Signed by: lbsadmin
GPG Key ID: CCC6AA87CC8DF425
5 changed files with 208 additions and 127 deletions

View File

@ -35,6 +35,6 @@ monitor_speed = 115200
lib_deps = lib_deps =
# ServoESP32 # ServoESP32
olikraus/U8g2@^2.35.19 # olikraus/U8g2@^2.35.19
DallasTemperature # DallasTemperature
Adafruit NeoPixel Adafruit NeoPixel

View File

@ -31,6 +31,7 @@
#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) #define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication #define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication
// #define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable) // #define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
#define wheelCircumference 52.00
#define HoverSerial Serial2 #define HoverSerial Serial2
// GPIOS: 16:RXD 17:TXD // GPIOS: 16:RXD 17:TXD
@ -42,11 +43,20 @@ byte *p; // Pointer declaration for the new received data
byte incomingByte; byte incomingByte;
byte incomingBytePrev; byte incomingBytePrev;
uint8_t sendTemperature = 0;
uint8_t sendTemperatureDecimals = 0;
uint8_t sendVoltage = 0;
uint8_t sendVoltageDecimals = 0;
uint8_t sendSpeed = 0;
uint8_t sendSpeedDecimals = 0;
int16_t ctrl_mode = 1;
typedef struct typedef struct
{ {
uint16_t start; uint16_t start;
int16_t steer; int16_t steer;
int16_t speed; int16_t speed;
int16_t ctrl_mode; // 1 vlt // 2 trq
uint16_t checksum; uint16_t checksum;
} SerialCommand; } SerialCommand;
SerialCommand Command; SerialCommand Command;
@ -83,7 +93,8 @@ void Send(int16_t uSteer, int16_t uSpeed)
Command.start = (uint16_t)START_FRAME; Command.start = (uint16_t)START_FRAME;
Command.steer = (int16_t)uSteer; Command.steer = (int16_t)uSteer;
Command.speed = (int16_t)uSpeed; Command.speed = (int16_t)uSpeed;
Command.checksum = (uint16_t)(Command.start ^ Command.steer ^ Command.speed); Command.ctrl_mode = (int16_t)ctrl_mode;
Command.checksum = (uint16_t)(Command.start ^ Command.steer ^ Command.speed ^ Command.ctrl_mode);
// Write to Serial // Write to Serial
HoverSerial.write((uint8_t *)&Command, sizeof(Command)); HoverSerial.write((uint8_t *)&Command, sizeof(Command));
@ -148,6 +159,26 @@ void Receive()
Serial.print(Feedback.boardTemp); Serial.print(Feedback.boardTemp);
Serial.print(" 7: "); Serial.print(" 7: ");
Serial.println(Feedback.cmdLed); Serial.println(Feedback.cmdLed);
sendVoltage = abs(floor(Feedback.batVoltage / 100));
sendVoltageDecimals = Feedback.batVoltage - sendVoltage * 100;
Serial.println();
Serial.print(" Volt: ");
Serial.print(sendVoltage);
Serial.print(" . ");
Serial.println(sendVoltageDecimals);
sendTemperature = abs(floor(Feedback.boardTemp / 10));
sendTemperatureDecimals = Feedback.boardTemp - sendTemperature * 10;
// average the two rpms and calculate the speed
// mm/rpm*3600
float kmh = wheelCircumference * (-Feedback.speedR_meas + Feedback.speedL_meas) / 2 * 60 / 100000;
// Serial.print(kmh); Serial.print(" "); Serial.println(direction[0]);
Serial.printf("KMH, %f0.2\n", kmh);
sendSpeed = abs(floor(kmh));
sendSpeedDecimals = (kmh - sendSpeed) * 100;
} }
else else
{ {

View File

@ -40,7 +40,7 @@ Servo esc2;
SSD1306 display(0x3c, 5, 4); SSD1306 display(0x3c, 5, 4);
#endif #endif
OneWire oneWire(ONE_WIRE_BUS); // OneWire oneWire(ONE_WIRE_BUS);
// DallasTemperature sensors(&oneWire); // one instance for all sensrs // DallasTemperature sensors(&oneWire); // one instance for all sensrs
esp_now_peer_info_t remote; esp_now_peer_info_t remote;
@ -49,12 +49,14 @@ float temperature = 0;
float voltage = 0; float voltage = 0;
float kmh = 0; float kmh = 0;
#ifndef VARIANT_HOVER
uint8_t sendTemperature = 0; uint8_t sendTemperature = 0;
uint8_t sendTemperatureDecimals = 0; uint8_t sendTemperatureDecimals = 0;
uint8_t sendVoltage = 0; uint8_t sendVoltage = 0;
uint8_t sendVoltageDecimals = 0; uint8_t sendVoltageDecimals = 0;
uint8_t sendSpeed = 0; uint8_t sendSpeed = 0;
uint8_t sendSpeedDecimals = 0; uint8_t sendSpeedDecimals = 0;
#endif
#include "rpm.h" #include "rpm.h"
@ -85,6 +87,9 @@ void setBoardOptions(uint8_t options)
lightMode = options & 3; lightMode = options & 3;
lightActive = (options >> 3) & 1; lightActive = (options >> 3) & 1;
fanMode = (options >> 2) & 3; fanMode = (options >> 2) & 3;
#ifdef VARIANT_HOVER
ctrl_mode = fanMode;
#endif
} }
// ESPNOW Functions ############################ // ESPNOW Functions ############################
@ -287,6 +292,7 @@ bool manageRemote()
// end ESPNOW functions // end ESPNOW functions
#ifndef VARIANT_HOVER
void checkTemperature() void checkTemperature()
{ {
// sensors.requestTemperatures(); // Send the command to get temperatures // sensors.requestTemperatures(); // Send the command to get temperatures
@ -338,6 +344,7 @@ void checkVoltage()
// Serial.println(batteryVoltage); // Serial.println(batteryVoltage);
digitalWrite(voltageEnable, LOW); // change to low digitalWrite(voltageEnable, LOW); // change to low
} }
#endif
void setup() void setup()
{ {

View File

@ -26,15 +26,10 @@ long rpmTimer = 0;
// Zero states are non existant // Zero states are non existant
int motorPosition[2][2][2]{ int motorPosition[2][2][2]{
{ {{0, 1},
{0, 1}, {3, 2}},
{3, 2} {{5, 6},
}, {4, 0}}};
{
{5, 6},
{4, 0}
}
};
// old state, new state // old state, new state
int relativeLookup[6][6]{ int relativeLookup[6][6]{
{0, 1, 2, 3, -2, -1}, // 1 {0, 1, 2, 3, -2, -1}, // 1
@ -48,16 +43,20 @@ int relativeLookup[6][6] {
#define PCNT_UNIT_LEFT PCNT_UNIT_0 #define PCNT_UNIT_LEFT PCNT_UNIT_0
#define PCNT_UNIT_RIGHT PCNT_UNIT_1 #define PCNT_UNIT_RIGHT PCNT_UNIT_1
int getState(bool isLeft)
int getState(bool isLeft) { {
if(isLeft) { if (isLeft)
{
return motorPosition[digitalRead(halBlueLeft)][digitalRead(halYellowLeft)][digitalRead(halGreenLeft)]; return motorPosition[digitalRead(halBlueLeft)][digitalRead(halYellowLeft)][digitalRead(halGreenLeft)];
} else { }
else
{
return motorPosition[digitalRead(halBlueRight)][digitalRead(halYellowRight)][digitalRead(halGreenRight)]; return motorPosition[digitalRead(halBlueRight)][digitalRead(halYellowRight)][digitalRead(halGreenRight)];
} }
} }
void initRPMPins() { void initRPMPins()
{
pinMode(halBlueLeft, INPUT_PULLUP); pinMode(halBlueLeft, INPUT_PULLUP);
pinMode(halYellowLeft, INPUT_PULLUP); pinMode(halYellowLeft, INPUT_PULLUP);
pinMode(halGreenLeft, INPUT_PULLUP); pinMode(halGreenLeft, INPUT_PULLUP);
@ -77,8 +76,7 @@ void initRPMPins() {
.counter_h_lim = 32767, .counter_h_lim = 32767,
.counter_l_lim = -1, .counter_l_lim = -1,
.unit = PCNT_UNIT_LEFT, .unit = PCNT_UNIT_LEFT,
.channel = PCNT_CHANNEL_0 .channel = PCNT_CHANNEL_0};
};
pcnt_config_t pcnt_config_right = { pcnt_config_t pcnt_config_right = {
.pulse_gpio_num = halBlueRight, .pulse_gpio_num = halBlueRight,
@ -90,13 +88,11 @@ void initRPMPins() {
.counter_h_lim = 32767, .counter_h_lim = 32767,
.counter_l_lim = -1, .counter_l_lim = -1,
.unit = PCNT_UNIT_RIGHT, .unit = PCNT_UNIT_RIGHT,
.channel = PCNT_CHANNEL_0 .channel = PCNT_CHANNEL_0};
};
pcnt_unit_config(&pcnt_config_left); pcnt_unit_config(&pcnt_config_left);
pcnt_unit_config(&pcnt_config_right); pcnt_unit_config(&pcnt_config_right);
pcnt_counter_pause(PCNT_UNIT_LEFT); pcnt_counter_pause(PCNT_UNIT_LEFT);
pcnt_counter_pause(PCNT_UNIT_RIGHT); pcnt_counter_pause(PCNT_UNIT_RIGHT);
@ -105,13 +101,15 @@ void initRPMPins() {
pcnt_counter_resume(PCNT_UNIT_LEFT); pcnt_counter_resume(PCNT_UNIT_LEFT);
pcnt_counter_resume(PCNT_UNIT_RIGHT); pcnt_counter_resume(PCNT_UNIT_RIGHT);
} }
void measureRpm(void * parameter) { void measureRpm(void *parameter)
{
while(true) { while (true)
if(millis()-rpmTimer>250) { {
if (millis() - rpmTimer > 250)
{
// read counters // read counters
pcnt_get_counter_value(PCNT_UNIT_LEFT, &counter[1]); pcnt_get_counter_value(PCNT_UNIT_LEFT, &counter[1]);
pcnt_get_counter_value(PCNT_UNIT_RIGHT, &counter[0]); pcnt_get_counter_value(PCNT_UNIT_RIGHT, &counter[0]);
@ -119,8 +117,7 @@ void measureRpm(void * parameter) {
pcnt_counter_clear(PCNT_UNIT_LEFT); pcnt_counter_clear(PCNT_UNIT_LEFT);
pcnt_counter_clear(PCNT_UNIT_RIGHT); pcnt_counter_clear(PCNT_UNIT_RIGHT);
rpmRight = (counter[0] / pulsesPerRotation) * 4; // LB: Actually only rps
rpmRight = (counter[0]/pulsesPerRotation)*4;
rpmLeft = (counter[1] / pulsesPerRotation) * 4; rpmLeft = (counter[1] / pulsesPerRotation) * 4;
kmh = (wheelCircumference / 100) * (rpmRight + rpmLeft) / 2 * 3600 / 1000; kmh = (wheelCircumference / 100) * (rpmRight + rpmLeft) / 2 * 3600 / 1000;
// Serial.print(kmh); Serial.print(" "); Serial.println(direction[0]); // Serial.print(kmh); Serial.print(" "); Serial.println(direction[0]);
@ -132,20 +129,28 @@ void measureRpm(void * parameter) {
} }
int state = getState(0); int state = getState(0);
if(lastState[0] != state) { if (lastState[0] != state)
if(relativeLookup[lastState[0]-1][state-1] > 0) { {
if (relativeLookup[lastState[0] - 1][state - 1] > 0)
{
direction[0] = FORWARD; direction[0] = FORWARD;
} else { }
else
{
direction[0] = BACKWARD; direction[0] = BACKWARD;
} }
lastState[0] = state; lastState[0] = state;
} }
state = getState(1); state = getState(1);
if(lastState[1] != state) { if (lastState[1] != state)
if(relativeLookup[lastState[1]-1][state-1] > 0) { {
if (relativeLookup[lastState[1] - 1][state - 1] > 0)
{
direction[1] = FORWARD; direction[1] = FORWARD;
} else { }
else
{
direction[1] = BACKWARD; direction[1] = BACKWARD;
} }

View File

@ -30,25 +30,23 @@ bool settingsChangeValueFlag = false;
const char *settingPages[SETTINGS_COUNT] = { const char *settingPages[SETTINGS_COUNT] = {
"Page Display", "Page Display",
"LimitMode", "LimitMode",
"FanOveride", "CTRL Mode", // OLD FAN OVERRIDE
"Wheel diameter", "Wheel diameter",
"Light Mode", "Light Mode",
"Throttle Min", "Throttle Min",
"Throttle Center", "Throttle Center",
"Throttle Max" "Throttle Max"};
};
// Setting rules format: default, min, max. // Setting rules format: default, min, max.
int settingRules[SETTINGS_COUNT][3]{ int settingRules[SETTINGS_COUNT][3]{
{0, 0, 3}, // page: 0 cycle, 1 speed, 2 volt, 3 temp {0, 0, 3}, // page: 0 cycle, 1 speed, 2 volt, 3 temp
{0, 0, 1}, // limit default is no limit {0, 0, 1}, // limit default is no limit
{0, 0, 2}, //fan 0 no, 1 on, 2 off {0, 0, 2}, // fan 0 no, 1 on, 2 off // CTRL Mode 0 and 1 vlt 2 is torq
{83, 0, 250}, // wheel mm {83, 0, 250}, // wheel mm
{0, 0, 3}, // LightMode {0, 0, 3}, // LightMode
{HAL_MIN, 1000, 3000}, {HAL_MIN, 1000, 3000},
{HAL_CENTER, 1000, 3000}, {HAL_CENTER, 1000, 3000},
{HAL_MAX, 1000, 3000} {HAL_MAX, 1000, 3000}};
};
/* /*
const char *settingAliases[SETTINGS_COUNT] = { const char *settingAliases[SETTINGS_COUNT] = {
"Off", "ON" "Off", "ON"
@ -60,24 +58,28 @@ int settingRules[SETTINGS_COUNT][3] {
}; };
*/ */
// Check if an integer is within a min and max value // Check if an integer is within a min and max value
bool inRange(int val, int minimum, int maximum) { bool inRange(int val, int minimum, int maximum)
{
return ((minimum <= val) && (val <= maximum)); return ((minimum <= val) && (val <= maximum));
} }
void updateBoardOptions() { void updateBoardOptions()
{
boardOptions = 0; boardOptions = 0;
boardOptions |= (uint8_t)settings[fanOveride] << 2; boardOptions |= (uint8_t)settings[fanOveride] << 2;
boardOptions |= boardOptions | (uint8_t)lightActive << 3; boardOptions |= boardOptions | (uint8_t)lightActive << 3;
boardOptions |= boardOptions | (uint8_t)settings[lightMode]; boardOptions |= boardOptions | (uint8_t)settings[lightMode];
DEBUG_PRINT("board: "); DEBUG_PRINTLN(boardOptions); DEBUG_PRINT("board: ");
DEBUG_PRINTLN(boardOptions);
} }
void updateSettings() { void updateSettings()
{
preferences.begin("remote-set", false); preferences.begin("remote-set", false);
for (int i = 0; i < SETTINGS_COUNT; i++) { for (int i = 0; i < SETTINGS_COUNT; i++)
{
preferences.putInt(settingPages[i], settings[i]); preferences.putInt(settingPages[i], settings[i]);
} }
@ -90,44 +92,55 @@ void updateSettings() {
updateBoardOptions(); updateBoardOptions();
} }
void setDefaultSettings() { void setDefaultSettings()
for (int i = 0; i < SETTINGS_COUNT; i++) { {
for (int i = 0; i < SETTINGS_COUNT; i++)
{
settings[i] = settingRules[i][0]; settings[i] = settingRules[i][0];
} }
updateSettings(); updateSettings();
} }
void loadSettings() { void loadSettings()
{
preferences.begin("remote-set", false); preferences.begin("remote-set", false);
for (int i = 0; i < SETTINGS_COUNT; i++) { for (int i = 0; i < SETTINGS_COUNT; i++)
{
settings[i] = preferences.getInt(settingPages[i], settingRules[i][0]); // retrieve default value settings[i] = preferences.getInt(settingPages[i], settingRules[i][0]); // retrieve default value
} }
preferences.end(); preferences.end();
bool rewriteSettings = false; bool rewriteSettings = false;
// Loop through all settings to check if everything is fine // Loop through all settings to check if everything is fine
for (int i = 0; i < SETTINGS_COUNT; i++) { for (int i = 0; i < SETTINGS_COUNT; i++)
{
int val = settings[i]; int val = settings[i];
if (!inRange(val, settingRules[i][1], settingRules[i][2])) { if (!inRange(val, settingRules[i][1], settingRules[i][2]))
{
// Setting is damaged or never written. Rewrite default. // Setting is damaged or never written. Rewrite default.
rewriteSettings = true; rewriteSettings = true;
settings[i] = settingRules[i][0]; settings[i] = settingRules[i][0];
} }
} }
if (rewriteSettings == true) { if (rewriteSettings == true)
{
updateSettings(); updateSettings();
} else { }
else
{
// Calculate constants // Calculate constants
// calculateRatios(); // calculateRatios();
updateBoardOptions(); updateBoardOptions();
} }
} }
void drawSettingsMenu() { void drawSettingsMenu()
{
// Position on OLED // Position on OLED
int x = 0; int y = 10; int x = 0;
int y = 10;
// Draw setting title // Draw setting title
displayString = settingPages[currentSetting]; displayString = settingPages[currentSetting];
@ -142,16 +155,21 @@ void drawSettingsMenu() {
displayString.toCharArray(displayBuffer, displayString.length() + 1); displayString.toCharArray(displayBuffer, displayString.length() + 1);
u8g2.setFont(u8g2_font_10x20_tr); u8g2.setFont(u8g2_font_10x20_tr);
if (changeSelectedSetting == true) { if (changeSelectedSetting == true)
{
u8g2.drawStr(x + 10, y + 20, displayBuffer); u8g2.drawStr(x + 10, y + 20, displayBuffer);
} else { }
else
{
u8g2.drawStr(x, y + 20, displayBuffer); u8g2.drawStr(x, y + 20, displayBuffer);
} }
} }
void drawSettingNumber() { void drawSettingNumber()
{
// Position on OLED // Position on OLED
int x = 2; int y = 10; int x = 2;
int y = 10;
// Draw current setting number box // Draw current setting number box
u8g2.drawRFrame(x + 102, y - 10, 22, 32, 4); u8g2.drawRFrame(x + 102, y - 10, 22, 32, 4);
@ -176,55 +194,75 @@ void drawSettingNumber() {
ratioPulseDistance = (gearRatio * (float)remoteSettings.wheelDiameter * 3.14156) / (((float)remoteSettings.motorPoles * 3) * 1000000); // Pulses to km travelled ratioPulseDistance = (gearRatio * (float)remoteSettings.wheelDiameter * 3.14156) / (((float)remoteSettings.motorPoles * 3) * 1000000); // Pulses to km travelled
} }
*/ */
void controlSettingsMenu() { void controlSettingsMenu()
if (triggerActive()) { {
if (settingsChangeFlag == false) { if (triggerActive())
{
if (settingsChangeFlag == false)
{
// Save settings // Save settings
if (changeSelectedSetting == true) { if (changeSelectedSetting == true)
{
updateSettings(); updateSettings();
} }
changeSelectedSetting = !changeSelectedSetting; changeSelectedSetting = !changeSelectedSetting;
settingsChangeFlag = true; settingsChangeFlag = true;
} }
}
} else { else
{
settingsChangeFlag = false; settingsChangeFlag = false;
} }
if (hallMeasurement >= (settings[maxHallValue] - 250) && settingsLoopFlag == false) { if (hallMeasurement >= (settings[maxHallValue] - 250) && settingsLoopFlag == false)
{
// Up // Up
if (changeSelectedSetting == true) { if (changeSelectedSetting == true)
{
int val = settings[currentSetting] - 1; int val = settings[currentSetting] - 1;
if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2])) { if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2]))
{
settings[currentSetting] = val; settings[currentSetting] = val;
settingsLoopFlag = true; settingsLoopFlag = true;
} }
} else { }
if (currentSetting != 0) { else
{
if (currentSetting != 0)
{
currentSetting--; currentSetting--;
settingsLoopFlag = true; settingsLoopFlag = true;
} }
} }
} else if (hallMeasurement <= (settings[minHallValue] + 250) && settingsLoopFlag == false) { }
else if (hallMeasurement <= (settings[minHallValue] + 250) && settingsLoopFlag == false)
{
// Down // Down
if (changeSelectedSetting == true) { if (changeSelectedSetting == true)
{
int val = settings[currentSetting] + 1; int val = settings[currentSetting] + 1;
if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2])) { if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2]))
{
settings[currentSetting] = val; settings[currentSetting] = val;
settingsLoopFlag = true; settingsLoopFlag = true;
} }
} else { }
if (currentSetting < (SETTINGS_COUNT - 1)) { else
{
if (currentSetting < (SETTINGS_COUNT - 1))
{
currentSetting++; currentSetting++;
settingsLoopFlag = true; settingsLoopFlag = true;
} }
} }
} else if (inRange(hallMeasurement, settings[centerHallValue] - 50, settings[centerHallValue] + 50)) { }
else if (inRange(hallMeasurement, settings[centerHallValue] - 50, settings[centerHallValue] + 50))
{
settingsLoopFlag = false; settingsLoopFlag = false;
} }
} }