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 =
# ServoESP32
olikraus/U8g2@^2.35.19
DallasTemperature
# olikraus/U8g2@^2.35.19
# DallasTemperature
Adafruit NeoPixel

View File

@ -31,6 +31,7 @@
#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 DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
#define wheelCircumference 52.00
#define HoverSerial Serial2
// GPIOS: 16:RXD 17:TXD
@ -42,11 +43,20 @@ byte *p; // Pointer declaration for the new received data
byte incomingByte;
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
{
uint16_t start;
int16_t steer;
int16_t speed;
int16_t ctrl_mode; // 1 vlt // 2 trq
uint16_t checksum;
} SerialCommand;
SerialCommand Command;
@ -83,7 +93,8 @@ void Send(int16_t uSteer, int16_t uSpeed)
Command.start = (uint16_t)START_FRAME;
Command.steer = (int16_t)uSteer;
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
HoverSerial.write((uint8_t *)&Command, sizeof(Command));
@ -148,6 +159,26 @@ void Receive()
Serial.print(Feedback.boardTemp);
Serial.print(" 7: ");
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
{

View File

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

View File

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

View File

@ -30,25 +30,23 @@ bool settingsChangeValueFlag = false;
const char *settingPages[SETTINGS_COUNT] = {
"Page Display",
"LimitMode",
"FanOveride",
"CTRL Mode", // OLD FAN OVERRIDE
"Wheel diameter",
"Light Mode",
"Throttle Min",
"Throttle Center",
"Throttle Max"
};
"Throttle Max"};
// Setting rules format: default, min, max.
int settingRules[SETTINGS_COUNT][3]{
{0, 0, 3}, // page: 0 cycle, 1 speed, 2 volt, 3 temp
{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
{0, 0, 3}, // LightMode
{HAL_MIN, 1000, 3000},
{HAL_CENTER, 1000, 3000},
{HAL_MAX, 1000, 3000}
};
{HAL_MAX, 1000, 3000}};
/*
const char *settingAliases[SETTINGS_COUNT] = {
"Off", "ON"
@ -60,24 +58,28 @@ int settingRules[SETTINGS_COUNT][3] {
};
*/
// 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));
}
void updateBoardOptions() {
void updateBoardOptions()
{
boardOptions = 0;
boardOptions |= (uint8_t)settings[fanOveride] << 2;
boardOptions |= boardOptions | (uint8_t)lightActive << 3;
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);
for (int i = 0; i < SETTINGS_COUNT; i++) {
for (int i = 0; i < SETTINGS_COUNT; i++)
{
preferences.putInt(settingPages[i], settings[i]);
}
@ -90,44 +92,55 @@ void updateSettings() {
updateBoardOptions();
}
void setDefaultSettings() {
for (int i = 0; i < SETTINGS_COUNT; i++) {
void setDefaultSettings()
{
for (int i = 0; i < SETTINGS_COUNT; i++)
{
settings[i] = settingRules[i][0];
}
updateSettings();
}
void loadSettings() {
void loadSettings()
{
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
}
preferences.end();
bool rewriteSettings = false;
// 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];
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.
rewriteSettings = true;
settings[i] = settingRules[i][0];
}
}
if (rewriteSettings == true) {
if (rewriteSettings == true)
{
updateSettings();
} else {
}
else
{
// Calculate constants
// calculateRatios();
updateBoardOptions();
}
}
void drawSettingsMenu() {
void drawSettingsMenu()
{
// Position on OLED
int x = 0; int y = 10;
int x = 0;
int y = 10;
// Draw setting title
displayString = settingPages[currentSetting];
@ -142,16 +155,21 @@ void drawSettingsMenu() {
displayString.toCharArray(displayBuffer, displayString.length() + 1);
u8g2.setFont(u8g2_font_10x20_tr);
if (changeSelectedSetting == true) {
if (changeSelectedSetting == true)
{
u8g2.drawStr(x + 10, y + 20, displayBuffer);
} else {
}
else
{
u8g2.drawStr(x, y + 20, displayBuffer);
}
}
void drawSettingNumber() {
void drawSettingNumber()
{
// Position on OLED
int x = 2; int y = 10;
int x = 2;
int y = 10;
// Draw current setting number box
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
}
*/
void controlSettingsMenu() {
if (triggerActive()) {
if (settingsChangeFlag == false) {
void controlSettingsMenu()
{
if (triggerActive())
{
if (settingsChangeFlag == false)
{
// Save settings
if (changeSelectedSetting == true) {
if (changeSelectedSetting == true)
{
updateSettings();
}
changeSelectedSetting = !changeSelectedSetting;
settingsChangeFlag = true;
}
} else {
}
else
{
settingsChangeFlag = false;
}
if (hallMeasurement >= (settings[maxHallValue] - 250) && settingsLoopFlag == false) {
if (hallMeasurement >= (settings[maxHallValue] - 250) && settingsLoopFlag == false)
{
// Up
if (changeSelectedSetting == true) {
if (changeSelectedSetting == true)
{
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;
settingsLoopFlag = true;
}
} else {
if (currentSetting != 0) {
}
else
{
if (currentSetting != 0)
{
currentSetting--;
settingsLoopFlag = true;
}
}
} else if (hallMeasurement <= (settings[minHallValue] + 250) && settingsLoopFlag == false) {
}
else if (hallMeasurement <= (settings[minHallValue] + 250) && settingsLoopFlag == false)
{
// Down
if (changeSelectedSetting == true) {
if (changeSelectedSetting == true)
{
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;
settingsLoopFlag = true;
}
} else {
if (currentSetting < (SETTINGS_COUNT - 1)) {
}
else
{
if (currentSetting < (SETTINGS_COUNT - 1))
{
currentSetting++;
settingsLoopFlag = true;
}
}
} else if (inRange(hallMeasurement, settings[centerHallValue] - 50, settings[centerHallValue] + 50)) {
}
else if (inRange(hallMeasurement, settings[centerHallValue] - 50, settings[centerHallValue] + 50))
{
settingsLoopFlag = false;
}
}