Add ControlMode For Hover
Signed-off-by: Lukas Bachschwell <lukas@lbsfilm.at>
This commit is contained in:
parent
f061d98098
commit
b1050c300c
GPG Key ID:
CCC6AA87CC8DF425
@ -35,6 +35,6 @@ monitor_speed = 115200
|
||||
|
||||
lib_deps =
|
||||
# ServoESP32
|
||||
olikraus/U8g2@^2.35.19
|
||||
DallasTemperature
|
||||
# olikraus/U8g2@^2.35.19
|
||||
# DallasTemperature
|
||||
Adafruit NeoPixel
|
||||
|
||||
@ -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
|
||||
{
|
||||
|
||||
@ -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()
|
||||
{
|
||||
|
||||
67
src/rpm.h
67
src/rpm.h
@ -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;
|
||||
}
|
||||
|
||||
|
||||
124
src/settings.h
124
src/settings.h
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user