Settings working, adding steeringmode, limitmode working

This commit is contained in:
Lukas Bachschwell 2018-04-29 14:05:06 +02:00
parent 9a0a163f56
commit 1c948c7070
3 changed files with 371 additions and 201 deletions

View File

@ -12,6 +12,7 @@
platform = espressif32 platform = espressif32
board = esp32doit-devkit-v1 board = esp32doit-devkit-v1
framework = arduino framework = arduino
build_flags = -DLOG_LOCAL_LEVEL=ESP_LOG_DEBUG
src_filter = +<remote.cpp> src_filter = +<remote.cpp>

View File

@ -4,14 +4,14 @@
#include <esp_now.h> #include <esp_now.h>
#include <WiFi.h> #include <WiFi.h>
#include <U8g2lib.h> #include <U8g2lib.h>
#include <EEPROM.h> // ESP32 ? #include <Preferences.h>
#include "mac_config.h" #include "mac_config.h"
#include "graphics.h" #include "graphics.h"
#include "accel.h" #include "accel.h"
TaskHandle_t clickTaskHandle; TaskHandle_t clickTaskHandle;
Preferences preferences;
#define B_VOLT 0 #define B_VOLT 0
#define B_VOLT_D 1 #define B_VOLT_D 1
@ -22,6 +22,16 @@ TaskHandle_t clickTaskHandle;
uint8_t boardData[6] = {0, 0, 0, 0, 0, 0}; uint8_t boardData[6] = {0, 0, 0, 0, 0, 0};
//#define DEBUG
#ifdef DEBUG
#define DEBUG_PRINTLN(x) Serial.println(x)
#define DEBUG_PRINT(x) Serial.print(x)
#else
#define DEBUG_PRINTLN(x)
#define DEBUG_PRINT(x)
#endif
bool connected = false; bool connected = false;
uint8_t clickCounter = 0; uint8_t clickCounter = 0;
@ -38,25 +48,17 @@ unsigned long lastSignalBlink;
bool signalBlink = false; bool signalBlink = false;
unsigned long lastDataRotation; unsigned long lastDataRotation;
// Defining variables for Settings menu bool beginnerMode = false; // TODO: moved to setting
bool changeSettings = false;
bool changeSelectedSetting = false;
bool beginnerMode = false;
bool steeringMode = false; bool steeringMode = false;
bool settingsLoopFlag = false;
bool settingsChangeFlag = false;
bool settingsChangeValueFlag = false;
// Defining variables for Hall Effect throttle. // Defining variables for Hall Effect throttle.
short hallMeasurement; short hallMeasurement;
int throttle = 127; int throttle = 127;
int sendThrottle = 127; uint8_t esc1 = 127;
byte hallCenterMargin = 5; uint8_t esc2 = 127;
byte currentSetting = 0; byte hallCenterMargin = 5;
const byte numOfSettings = 11;
const float minVoltage = 3.4; const float minVoltage = 3.4;
const float maxVoltage = 4.1; const float maxVoltage = 4.1;
@ -65,7 +67,6 @@ const float refVoltage = 3.3;
const float deviderR1 = 1500; const float deviderR1 = 1500;
const float deviderR2 = 22000; const float deviderR2 = 22000;
// Global copy of board // Global copy of board
esp_now_peer_info_t board; esp_now_peer_info_t board;
#define CHANNEL 1 #define CHANNEL 1
@ -82,6 +83,10 @@ esp_now_peer_info_t board;
#define triggerPin 17 #define triggerPin 17
#define batteryMeasurePin 38 #define batteryMeasurePin 38
bool triggerActive();
#include "settings.h"
// ESPNOW functions ############################## // ESPNOW functions ##############################
// Scan for boards in AP mode // Scan for boards in AP mode
@ -93,11 +98,11 @@ void ScanForBoard() {
bool boardFound = 0; bool boardFound = 0;
memset(&board, 0, sizeof(board)); memset(&board, 0, sizeof(board));
Serial.println(""); DEBUG_PRINTLN("");
if (scanResults == 0) { if (scanResults == 0) {
Serial.println("No WiFi devices in AP Mode found"); DEBUG_PRINTLN("No WiFi devices in AP Mode found");
} else { } else {
Serial.print("Found "); Serial.print(scanResults); Serial.println(" devices "); DEBUG_PRINT("Found "); DEBUG_PRINT(scanResults); DEBUG_PRINTLN(" devices ");
for (int i = 0; i < scanResults; ++i) { for (int i = 0; i < scanResults; ++i) {
// Print SSID and RSSI for each device found // Print SSID and RSSI for each device found
String SSID = WiFi.SSID(i); String SSID = WiFi.SSID(i);
@ -105,21 +110,21 @@ void ScanForBoard() {
String BSSIDstr = WiFi.BSSIDstr(i); String BSSIDstr = WiFi.BSSIDstr(i);
if (PRINTSCANRESULTS) { if (PRINTSCANRESULTS) {
Serial.print(i + 1); DEBUG_PRINT(i + 1);
Serial.print(": "); DEBUG_PRINT(": ");
Serial.print(SSID); DEBUG_PRINT(SSID);
Serial.print(" ("); DEBUG_PRINT(" (");
Serial.print(RSSI); DEBUG_PRINT(RSSI);
Serial.print(")"); DEBUG_PRINT(")");
Serial.println(""); DEBUG_PRINTLN("");
} }
delay(10); delay(10);
// Check if the current device starts with `board` // Check if the current device starts with `board`
if (SSID.indexOf("ESK8") == 0) { if (SSID.indexOf("ESK8") == 0) {
// SSID of interest // SSID of interest
Serial.println("Found a board."); DEBUG_PRINTLN("Found a board.");
Serial.print(i + 1); Serial.print(": "); Serial.print(SSID); Serial.print(" ["); Serial.print(BSSIDstr); Serial.print("]"); Serial.print(" ("); Serial.print(RSSI); Serial.print(")"); Serial.println(""); DEBUG_PRINT(i + 1); DEBUG_PRINT(": "); DEBUG_PRINT(SSID); DEBUG_PRINT(" ["); DEBUG_PRINT(BSSIDstr); DEBUG_PRINT("]"); DEBUG_PRINT(" ("); DEBUG_PRINT(RSSI); DEBUG_PRINT(")"); DEBUG_PRINTLN("");
// Get BSSID => Mac Address of the board // Get BSSID => Mac Address of the board
int mac[6]; int mac[6];
if ( 6 == sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x%c", &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5] ) ) { if ( 6 == sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x%c", &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5] ) ) {
@ -140,9 +145,9 @@ void ScanForBoard() {
} }
if (boardFound) { if (boardFound) {
Serial.println("board Found, processing.."); DEBUG_PRINTLN("board Found, processing..");
} else { } else {
Serial.println("board Not Found, trying again."); DEBUG_PRINTLN("board Not Found, trying again.");
} }
// clean up ram // clean up ram
@ -154,19 +159,19 @@ void deletePeer() {
const esp_now_peer_info_t *peer = &board; const esp_now_peer_info_t *peer = &board;
const uint8_t *peer_addr = board.peer_addr; const uint8_t *peer_addr = board.peer_addr;
esp_err_t delStatus = esp_now_del_peer(peer_addr); esp_err_t delStatus = esp_now_del_peer(peer_addr);
Serial.print("board Delete Status: "); DEBUG_PRINT("board Delete Status: ");
if (delStatus == ESP_OK) { if (delStatus == ESP_OK) {
// Delete success // Delete success
Serial.println("Success"); DEBUG_PRINTLN("Success");
} else if (delStatus == ESP_ERR_ESPNOW_NOT_INIT) { } else if (delStatus == ESP_ERR_ESPNOW_NOT_INIT) {
// How did we get so far!! // How did we get so far!!
Serial.println("ESPNOW Not Init"); DEBUG_PRINTLN("ESPNOW Not Init");
} else if (delStatus == ESP_ERR_ESPNOW_ARG) { } else if (delStatus == ESP_ERR_ESPNOW_ARG) {
Serial.println("Invalid Argument"); DEBUG_PRINTLN("Invalid Argument");
} else if (delStatus == ESP_ERR_ESPNOW_NOT_FOUND) { } else if (delStatus == ESP_ERR_ESPNOW_NOT_FOUND) {
Serial.println("Peer not found."); DEBUG_PRINTLN("Peer not found.");
} else { } else {
Serial.println("Not sure what happened"); DEBUG_PRINTLN("Not sure what happened");
} }
} }
@ -178,76 +183,75 @@ bool manageBoard() {
deletePeer(); deletePeer();
} }
Serial.print("board Status: "); DEBUG_PRINT("board Status: ");
const esp_now_peer_info_t *peer = &board; const esp_now_peer_info_t *peer = &board;
const uint8_t *peer_addr = board.peer_addr; const uint8_t *peer_addr = board.peer_addr;
// check if the peer exists // check if the peer exists
bool exists = esp_now_is_peer_exist(peer_addr); bool exists = esp_now_is_peer_exist(peer_addr);
if ( exists) { if ( exists) {
// board already paired. // board already paired.
Serial.println("Already Paired"); DEBUG_PRINTLN("Already Paired");
return true; return true;
} else { } else {
// board not paired, attempt pair // board not paired, attempt pair
esp_err_t addStatus = esp_now_add_peer(peer); esp_err_t addStatus = esp_now_add_peer(peer);
if (addStatus == ESP_OK) { if (addStatus == ESP_OK) {
// Pair success // Pair success
Serial.println("Pair success"); DEBUG_PRINTLN("Pair success");
return true; return true;
} else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) { } else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) {
// How did we get so far!! // How did we get so far!!
Serial.println("ESPNOW Not Init"); DEBUG_PRINTLN("ESPNOW Not Init");
return false; return false;
} else if (addStatus == ESP_ERR_ESPNOW_ARG) { } else if (addStatus == ESP_ERR_ESPNOW_ARG) {
Serial.println("Invalid Argument"); DEBUG_PRINTLN("Invalid Argument");
return false; return false;
} else if (addStatus == ESP_ERR_ESPNOW_FULL) { } else if (addStatus == ESP_ERR_ESPNOW_FULL) {
Serial.println("Peer list full"); DEBUG_PRINTLN("Peer list full");
return false; return false;
} else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) { } else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) {
Serial.println("Out of memory"); DEBUG_PRINTLN("Out of memory");
return false; return false;
} else if (addStatus == ESP_ERR_ESPNOW_EXIST) { } else if (addStatus == ESP_ERR_ESPNOW_EXIST) {
Serial.println("Peer Exists"); DEBUG_PRINTLN("Peer Exists");
return true; return true;
} else { } else {
Serial.println("Not sure what happened"); DEBUG_PRINTLN("Not sure what happened");
return false; return false;
} }
} }
} else { } else {
// No board found to process // No board found to process
Serial.println("No board found to process"); DEBUG_PRINTLN("No board found to process");
return false; return false;
} }
} }
// send data // send data
void sendData() { void sendData() {
uint8_t esc1 = sendThrottle; const uint8_t data[] = { esc1, esc2 };
uint8_t esc2 = esc1; //const uint8_t data[] = { esc1, esc2, options};
const uint8_t data[] = { esc1, esc2 }; // no mixture for the normal mode
const uint8_t *peer_addr = board.peer_addr; const uint8_t *peer_addr = board.peer_addr;
Serial.print("Sending: "); Serial.println(esc1); DEBUG_PRINT("Sending: "); DEBUG_PRINTLN(esc1);
esp_err_t result = esp_now_send(peer_addr, data, sizeof(data)); esp_err_t result = esp_now_send(peer_addr, data, sizeof(data));
Serial.print("Send Status: "); DEBUG_PRINT("Send Status: ");
if (result == ESP_OK) { if (result == ESP_OK) {
Serial.println("Success"); DEBUG_PRINTLN("Success");
} else if (result == ESP_ERR_ESPNOW_NOT_INIT) { } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
// How did we get so far!! // How did we get so far!!
Serial.println("ESPNOW not Init."); DEBUG_PRINTLN("ESPNOW not Init.");
} else if (result == ESP_ERR_ESPNOW_ARG) { } else if (result == ESP_ERR_ESPNOW_ARG) {
Serial.println("Invalid Argument"); DEBUG_PRINTLN("Invalid Argument");
} else if (result == ESP_ERR_ESPNOW_INTERNAL) { } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
Serial.println("Internal Error"); DEBUG_PRINTLN("Internal Error");
} else if (result == ESP_ERR_ESPNOW_NO_MEM) { } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
Serial.println("ESP_ERR_ESPNOW_NO_MEM"); DEBUG_PRINTLN("ESP_ERR_ESPNOW_NO_MEM");
} else if (result == ESP_ERR_ESPNOW_NOT_FOUND) { } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
Serial.println("Peer not found."); DEBUG_PRINTLN("Peer not found.");
} else { } else {
Serial.println("Not sure what happened"); DEBUG_PRINTLN("Not sure what happened");
} }
} }
@ -256,14 +260,14 @@ void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
char macStr[18]; char macStr[18];
snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x", snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]); mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
Serial.print("Last Packet Sent to: "); Serial.println(macStr); DEBUG_PRINT("Last Packet Sent to: "); DEBUG_PRINTLN(macStr);
Serial.print("Last Packet Send Status: "); DEBUG_PRINT("Last Packet Send Status: ");
if(status == ESP_NOW_SEND_SUCCESS) { if(status == ESP_NOW_SEND_SUCCESS) {
connected = true; connected = true;
Serial.println("Delivery Success"); DEBUG_PRINTLN("Delivery Success");
} else { } else {
connected = false; connected = false;
Serial.println("Delivery Fail"); DEBUG_PRINTLN("Delivery Fail");
} }
} }
@ -272,18 +276,18 @@ void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
char macStr[18]; char macStr[18];
snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x", snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]); mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
Serial.print("Last Response Recv from: "); Serial.println(macStr); DEBUG_PRINT("Last Response Recv from: "); DEBUG_PRINTLN(macStr);
memcpy(boardData, data, data_len); memcpy(boardData, data, data_len);
Serial.print("Recieved data! len: "); DEBUG_PRINT("Recieved data! len: ");
Serial.println(data_len); DEBUG_PRINTLN(data_len);
} }
//############ End ESP Now //############ End ESP Now
//############ Hardware Helpers //############ Hardware Helpers
// Check if an integer is within a min and max value
bool inRange(int val, int minimum, int maximum) { int c_map(int value, int inMin, int inMax, int outMin, int outMax) {
return ((minimum <= val) && (val <= maximum)); return constrain(map(value, inMin, inMax, outMin, outMax), outMin, outMax);
} }
// Return true if trigger is activated, false otherwice // Return true if trigger is activated, false otherwice
@ -305,15 +309,18 @@ void calculateThrottlePosition() {
for (int i = 0; i < 10; i++) { for (int i = 0; i < 10; i++) {
total += analogRead(leverPin); total += analogRead(leverPin);
} }
hallMeasurement = total / 10; hallMeasurement = total / 10;
Serial.print("HAL: "); DEBUG_PRINT("HAL: ");
Serial.println(hallMeasurement); DEBUG_PRINTLN(hallMeasurement);
//DEBUG_PRINT( (String)hallMeasurement );
int maxSpeed = 255;
if(settings[limitMode] == 1) maxSpeed = 190;
if (hallMeasurement >= HAL_CENTER) { if (hallMeasurement >= HAL_CENTER) {
throttle = constrain(map(hallMeasurement, HAL_CENTER, HAL_MAX, 127, 255), 127, 255); throttle = c_map(hallMeasurement, HAL_CENTER, HAL_MAX, 127, maxSpeed);
} else { } else {
throttle = constrain(map(hallMeasurement, HAL_MIN, HAL_CENTER, 0, 127), 0, 127); throttle = c_map(hallMeasurement, HAL_MIN, HAL_CENTER, 0, 127);
} }
// removeing center noise // removeing center noise
if (abs(throttle - 127) < hallCenterMargin) { if (abs(throttle - 127) < hallCenterMargin) {
@ -332,8 +339,8 @@ float batteryVoltage() {
batteryVoltage = (refVoltage / 4095.0) * ((float)total / 10.0); batteryVoltage = (refVoltage / 4095.0) * ((float)total / 10.0);
// Now we have the actual Voltage, lets calculate the value befor the devider // Now we have the actual Voltage, lets calculate the value befor the devider
batteryVoltage = batteryVoltage / ( deviderR1 / (deviderR1 + deviderR2)); batteryVoltage = batteryVoltage / ( deviderR1 / (deviderR1 + deviderR2));
Serial.print("Batt: "); DEBUG_PRINT("Batt: ");
Serial.println(batteryVoltage); DEBUG_PRINTLN(batteryVoltage);
return batteryVoltage; return batteryVoltage;
} }
@ -362,29 +369,30 @@ int getStrength(int points){
} }
averageRSSI=rssi/points; averageRSSI=rssi/points;
Serial.print("RSSI: "); DEBUG_PRINT("RSSI: ");
Serial.println(averageRSSI); DEBUG_PRINTLN(averageRSSI);
return averageRSSI; return averageRSSI;
} }
void checkClicks(void * parameter) { void checkClicks(void * parameter) {
for (;;) { for (;;) {
//Serial.print("Trig: "); Serial.print(triggerActive()); Serial.print(" LAST: "); Serial.println(lastTriggerState); //DEBUG_PRINT("Trig: "); DEBUG_PRINT(triggerActive()); DEBUG_PRINT(" LAST: "); DEBUG_PRINTLN(lastTriggerState);
if(millis()-lastClick > clickDiff && clickCounter!=0) { if(millis()-lastClick > clickDiff && clickCounter!=0) {
Serial.println("reset"); DEBUG_PRINTLN("reset");
clickCounter = 0; clickCounter = 0;
} }
if(!triggerActive() && lastTriggerState) { if(!triggerActive() && lastTriggerState) {
Serial.println("CLICK##################### "); DEBUG_PRINTLN("CLICK##################### ");
int timeSinceLastClick = millis()-lastClick; int timeSinceLastClick = millis()-lastClick;
lastClick = millis(); lastClick = millis();
if(timeSinceLastClick < clickDiff) clickCounter++; if(timeSinceLastClick < clickDiff) clickCounter++;
else clickCounter = 1; else clickCounter = 1;
if(clickCounter == 3) { if(clickCounter == 3) {
Serial.println("YEAH TRIPPLE"); DEBUG_PRINTLN("YEAH TRIPPLE");
steeringMode = !steeringMode; if(changeSettings) changeSettings = false;
else steeringMode = !steeringMode;
vTaskDelay(pdMS_TO_TICKS(2000)); vTaskDelay(pdMS_TO_TICKS(2000));
} }
@ -569,99 +577,19 @@ void drawPage() {
u8g2.drawStr(x + 86 + 2, y + 13, displayBuffer); u8g2.drawStr(x + 86 + 2, y + 13, displayBuffer);
} }
void drawMode() {
void drawSettingsMenu() { if(steeringMode) {
/* displayString = (String)esc1 + " |S| " + (String)esc2;
// Position on OLED displayString.toCharArray(displayBuffer, 12);
int x = 0; int y = 10;
// Draw setting title
displayString = settingPages[currentSetting][0];
displayString.toCharArray(displayBuffer, displayString.length() + 1);
u8g2.setFont(u8g2_font_profont12_tr); u8g2.setFont(u8g2_font_profont12_tr);
u8g2.drawStr(x, y, displayBuffer); u8g2.drawStr(25, 50, displayBuffer);
int val = getSettingValue(currentSetting);
displayString = (String)val + "" + settingPages[currentSetting][1];
displayString.toCharArray(displayBuffer, displayString.length() + 1);
u8g2.setFont(u8g2_font_10x20_tr );
if (changeSelectedSetting == true) {
u8g2.drawStr(x + 10, y + 20, displayBuffer);
} else {
u8g2.drawStr(x, y + 20, displayBuffer);
} }
*/ if(settings[limitMode] == 1) {
displayString = "LIM";
displayString.toCharArray(displayBuffer, 12);
u8g2.setFont(u8g2_font_profont12_tr);
u8g2.drawStr(105, 50, displayBuffer);
} }
void controlSettingsMenu() {
/*
if (triggerActive()) {
if (settingsChangeFlag == false) {
// Save settings to EEPROM
if (changeSelectedSetting == true) {
updateEEPROMSettings();
}
changeSelectedSetting = !changeSelectedSetting;
settingsChangeFlag = true;
}
} else {
settingsChangeFlag = false;
}
if (hallMeasurement >= (remoteSettings.maxHallValue - 150) && settingsLoopFlag == false) {
// Up
if (changeSelectedSetting == true) {
int val = getSettingValue(currentSetting) + 1;
if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2])) {
setSettingValue(currentSetting, val);
settingsLoopFlag = true;
}
} else {
if (currentSetting != 0) {
currentSetting--;
settingsLoopFlag = true;
}
}
}
else if (hallMeasurement <= (remoteSettings.minHallValue + 150) && settingsLoopFlag == false) {
// Down
if (changeSelectedSetting == true) {
int val = getSettingValue(currentSetting) - 1;
if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2])) {
setSettingValue(currentSetting, val);
settingsLoopFlag = true;
}
} else {
if (currentSetting < (numOfSettings - 1)) {
currentSetting++;
settingsLoopFlag = true;
}
}
} else if (inRange(hallMeasurement, remoteSettings.centerHallValue - 50, remoteSettings.centerHallValue + 50)) {
settingsLoopFlag = false;
}*/
}
void drawSettingNumber() {
// Position on OLED
int x = 2; int y = 10;
// Draw current setting number box
u8g2.drawRFrame(x + 102, y - 10, 22, 32, 4);
// Draw current setting number
displayString = (String)(currentSetting + 1);
displayString.toCharArray(displayBuffer, displayString.length() + 1);
u8g2.setFont(u8g2_font_profont22_tn);
u8g2.drawStr(x + 108, 22, displayBuffer);
} }
void updateMainDisplay() { void updateMainDisplay() {
@ -677,6 +605,7 @@ void updateMainDisplay() {
drawPage(); drawPage();
drawBatteryLevel(); drawBatteryLevel();
drawSignal(); drawSignal();
drawMode();
} }
} while ( u8g2.nextPage() ); } while ( u8g2.nextPage() );
@ -701,7 +630,9 @@ void setup() {
Serial.begin(115200); Serial.begin(115200);
//Set device in STA mode to begin with //Set device in STA mode to begin with
WiFi.mode(WIFI_STA); WiFi.mode(WIFI_STA);
Serial.println("ESPNowSkate"); DEBUG_PRINTLN("ESPNowSkate");
loadSettings();
// reset the screen // reset the screen
pinMode(16, OUTPUT); pinMode(16, OUTPUT);
@ -725,7 +656,7 @@ void setup() {
initAccel(); initAccel();
Serial.println("ESPNowSkate Sender"); DEBUG_PRINTLN("ESPNowSkate Sender");
u8g2.begin(); u8g2.begin();
drawStartScreen(); drawStartScreen();
@ -735,13 +666,13 @@ void setup() {
} }
// This is the mac address of the Master in Station Mode // This is the mac address of the Master in Station Mode
Serial.print("STA MAC: "); Serial.println(WiFi.macAddress()); DEBUG_PRINT("STA MAC: "); DEBUG_PRINTLN(WiFi.macAddress());
if (esp_now_init() == ESP_OK) { if (esp_now_init() == ESP_OK) {
Serial.println("ESPNow Init Success"); DEBUG_PRINTLN("ESPNow Init Success");
} }
else { else {
Serial.println("ESPNow Init Failed"); DEBUG_PRINTLN("ESPNow Init Failed");
ESP.restart(); ESP.restart();
} }
@ -767,22 +698,49 @@ void loop() {
updateMainDisplay(); updateMainDisplay();
readAccel(); readAccel();
Serial.print("Accel Values: ");
Serial.print(AcX); Serial.print("Accel Value Left Right( Y): ");
Serial.print(" "); Serial.print(map(AcY, -15000, 15000, -100, 100));
Serial.println(GyX); Serial.print("Help(Y): ");
Serial.println(map(GyY, -15000, 15000, -100, 100));
calculateThrottlePosition(); calculateThrottlePosition();
if (changeSettings == true) { if (changeSettings == true) {
// Use throttle and trigger to change settings // Use throttle and trigger to change settings
//controlSettingsMenu(); controlSettingsMenu();
} else { } else {
// Use throttle and trigger to drive motors // Use throttle and trigger to drive motors
if (triggerActive()) { if (triggerActive()) {
sendThrottle = throttle; if(!steeringMode) {
esc1 = (uint8_t) throttle;
esc2 = (uint8_t) throttle;
} else {
DEBUG_PRINT("Value: ");
int map = c_map(AcX, 15000, -15000, -50, 50);
DEBUG_PRINTLN(map);
esc1 = (uint8_t) constrain(throttle + map, 0, 200);
esc2 = (uint8_t) constrain(throttle - map, 0, 200);
if(throttle == 127) {
esc1 = 127;
esc2 = 127;
}
DEBUG_PRINT("SteeringMode: ESC1: "); DEBUG_PRINT(esc1); DEBUG_PRINT(" ESC2: "); DEBUG_PRINTLN(esc2);
}
} else { } else {
// 127 is the middle position - no throttle and no brake/reverse // 127 is the middle position - no throttle and no brake/reverse
sendThrottle = 127; esc1 = 127;
esc2 = 127;
if(throttle < 30) { // enter settings mode
changeSettings = true;
}
// could use the other side for cruise control...
if(throttle > 230) {
}
}
} }
// If board is found, it would be populate in `board` variable // If board is found, it would be populate in `board` variable
@ -795,11 +753,10 @@ void loop() {
sendData(); sendData();
} else { } else {
// board pair failed // board pair failed
Serial.println("board not found / paired!"); DEBUG_PRINTLN("board not found / paired!");
} }
} }
else { else {
// No board found to process // No board found to process
} }
} }
}

212
src/settings.h Normal file
View File

@ -0,0 +1,212 @@
uint8_t currentSetting = 0;
#define SETTINGS_COUNT 7
#define limitMode 0
#define fanOveride 1
#define wheelDiameter 2 // Speed factor
#define minHallValue 3
#define centerHallValue 4
#define maxHallValue 5
int settings[SETTINGS_COUNT];
float gearRatio;
float ratioRpmSpeed;
float ratioPulseDistance;
// Defining variables for Settings menu
bool changeSettings = false;
bool changeSelectedSetting = false;
bool settingsLoopFlag = false;
bool settingsChangeFlag = false;
bool settingsChangeValueFlag = false;
const char *settingPages[SETTINGS_COUNT] = {
"LimitMode",
"FanOveride",
"TriggerMode",
"Wheel diameter",
"Throttle Min",
"Throttle Center",
"Throttle Max"
};
// Setting rules format: default, min, max.
int settingRules[SETTINGS_COUNT][3] {
{0, 0, 1}, // default is no limit
{0, 0, 2}, // 0 no, 1 on, 2 off
{0, 0, 3}, // 0 Killswitch, 1 cruise & 2 data toggle
{83, 0, 250}, // wheel mm
{HAL_MIN, 1000, 2000},
{HAL_CENTER, 1000, 2000},
{HAL_MAX, 1000, 2000}
};
/*
const char *settingAliases[SETTINGS_COUNT] = {
"Off", "ON"
"Auto", "Off", "On",
//"Wheel diameter",
//"Throttle Min",
//"Throttle Center",
//"Throttle Max"
};
*/
// Check if an integer is within a min and max value
bool inRange(int val, int minimum, int maximum) {
return ((minimum <= val) && (val <= maximum));
}
void updateSettings() {
preferences.begin("remote-set", false);
for (int i = 0; i < SETTINGS_COUNT; i++) {
preferences.putInt(settingPages[i], settings[i]);
}
preferences.end();
DEBUG_PRINTLN("UPDATED");
preferences.begin("remote-set", false);
DEBUG_PRINTLN(preferences.getInt(settingPages[0], 0));
preferences.end();
//calculateRatios();
}
void setDefaultSettings() {
for (int i = 0; i < SETTINGS_COUNT; i++) {
settings[i] = settingRules[i][0];
}
updateSettings();
}
void loadSettings() {
preferences.begin("remote-set", false);
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++) {
int val = settings[i];
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) {
updateSettings();
} else {
// Calculate constants
//calculateRatios();
}
}
void drawSettingsMenu() {
// Position on OLED
int x = 0; int y = 10;
// Draw setting title
displayString = settingPages[currentSetting];
displayString.toCharArray(displayBuffer, displayString.length() + 1);
u8g2.setFont(u8g2_font_profont12_tr);
u8g2.drawStr(x, y, displayBuffer);
int val = settings[currentSetting];
displayString = (String)val + ""; //+ settingPages[currentSetting][1];
displayString.toCharArray(displayBuffer, displayString.length() + 1);
u8g2.setFont(u8g2_font_10x20_tr );
if (changeSelectedSetting == true) {
u8g2.drawStr(x + 10, y + 20, displayBuffer);
} else {
u8g2.drawStr(x, y + 20, displayBuffer);
}
}
void drawSettingNumber() {
// Position on OLED
int x = 2; int y = 10;
// Draw current setting number box
u8g2.drawRFrame(x + 102, y - 10, 22, 32, 4);
// Draw current setting number
displayString = (String)(currentSetting + 1);
displayString.toCharArray(displayBuffer, displayString.length() + 1);
u8g2.setFont(u8g2_font_profont22_tn);
u8g2.drawStr(x + 108, 22, displayBuffer);
}
/*
// Update values used to calculate speed and distance travelled.
void calculateRatios() {
// Defining variables for speed and distance calculation
gearRatio = (float)remoteSettings.motorPulley / (float)remoteSettings.wheelPulley;
ratioRpmSpeed = (gearRatio * 60 * (float)remoteSettings.wheelDiameter * 3.14156) / (((float)remoteSettings.motorPoles / 2) * 1000000); // ERPM to Km/h
ratioPulseDistance = (gearRatio * (float)remoteSettings.wheelDiameter * 3.14156) / (((float)remoteSettings.motorPoles * 3) * 1000000); // Pulses to km travelled
}
*/
void controlSettingsMenu() {
if (triggerActive()) {
if (settingsChangeFlag == false) {
// Save settings
if (changeSelectedSetting == true) {
updateSettings();
}
changeSelectedSetting = !changeSelectedSetting;
settingsChangeFlag = true;
}
} else {
settingsChangeFlag = false;
}
if (hallMeasurement >= (HAL_MAX - 250) && settingsLoopFlag == false) { //settings[maxHallValue]
// Up
if (changeSelectedSetting == true) {
int val = settings[currentSetting] - 1;
if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2])) {
settings[currentSetting] = val;
settingsLoopFlag = true;
}
} else {
if (currentSetting != 0) {
currentSetting--;
settingsLoopFlag = true;
}
}
} else if (hallMeasurement <= (HAL_MIN + 250) && settingsLoopFlag == false) { //settings[minHallValue]
// Down
if (changeSelectedSetting == true) {
int val = settings[currentSetting] + 1;
if (inRange(val, settingRules[currentSetting][1], settingRules[currentSetting][2])) {
settings[currentSetting] = val;
settingsLoopFlag = true;
}
} else {
if (currentSetting < (SETTINGS_COUNT - 1)) {
currentSetting++;
settingsLoopFlag = true;
}
}
} else if (inRange(hallMeasurement, HAL_CENTER - 50, HAL_CENTER + 50)) { // settings[centerHallValue]
settingsLoopFlag = false;
}
}