611 lines
16 KiB
C++
611 lines
16 KiB
C++
// ESPNOWSkate by Lukas Bachschwell this device MASTER =D
|
|
|
|
#include <Arduino.h>
|
|
#include <esp_now.h>
|
|
#include <WiFi.h>
|
|
#include <U8g2lib.h>
|
|
#include <EEPROM.h> // ESP32 ?
|
|
|
|
#include "mac_config.h"
|
|
#include "graphics.h"
|
|
|
|
// Defining variables for OLED display
|
|
U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R2, /* clock=*/ 15, /* data=*/ 4, /* reset=*/ 16);
|
|
char displayBuffer[20];
|
|
String displayString;
|
|
short displayData = 0;
|
|
unsigned long lastSignalBlink;
|
|
unsigned long lastDataRotation;
|
|
|
|
// Defining variables for Settings menu
|
|
bool changeSettings = false;
|
|
bool changeSelectedSetting = false;
|
|
|
|
bool settingsLoopFlag = false;
|
|
bool settingsChangeFlag = false;
|
|
bool settingsChangeValueFlag = false;
|
|
|
|
// Defining variables for Hall Effect throttle.
|
|
short hallMeasurement;
|
|
int throttle = 127;
|
|
int sendThrottle = 127;
|
|
byte hallCenterMargin = 4;
|
|
|
|
byte currentSetting = 0;
|
|
const byte numOfSettings = 11;
|
|
|
|
const float minVoltage = 3.2;
|
|
const float maxVoltage = 4.1;
|
|
const float refVoltage = 3.3;
|
|
|
|
|
|
// Global copy of slave
|
|
esp_now_peer_info_t slave;
|
|
#define CHANNEL 3
|
|
#define PRINTSCANRESULTS 0
|
|
#define DELETEBEFOREPAIR 0
|
|
#define HAL_MIN 1390
|
|
#define HAL_MAX 2260
|
|
#define HAL_CENTER 1890
|
|
#define TRIM_LOW 180
|
|
#define TRIM_HIGH 0
|
|
|
|
//#define pairingMode
|
|
#define leverPin 36
|
|
#define triggerPin 17
|
|
#define batteryMeasurePin 38
|
|
|
|
|
|
// ESPNOW functions ##############################
|
|
// Scan for slaves in AP mode
|
|
#ifdef pairingMode
|
|
void ScanForSlave() {
|
|
int8_t scanResults = WiFi.scanNetworks();
|
|
// reset on each scan
|
|
bool slaveFound = 0;
|
|
memset(&slave, 0, sizeof(slave));
|
|
|
|
Serial.println("");
|
|
if (scanResults == 0) {
|
|
Serial.println("No WiFi devices in AP Mode found");
|
|
} else {
|
|
Serial.print("Found "); Serial.print(scanResults); Serial.println(" devices ");
|
|
for (int i = 0; i < scanResults; ++i) {
|
|
// Print SSID and RSSI for each device found
|
|
String SSID = WiFi.SSID(i);
|
|
int32_t RSSI = WiFi.RSSI(i);
|
|
String BSSIDstr = WiFi.BSSIDstr(i);
|
|
|
|
if (PRINTSCANRESULTS) {
|
|
Serial.print(i + 1);
|
|
Serial.print(": ");
|
|
Serial.print(SSID);
|
|
Serial.print(" (");
|
|
Serial.print(RSSI);
|
|
Serial.print(")");
|
|
Serial.println("");
|
|
}
|
|
delay(10);
|
|
// Check if the current device starts with `Slave`
|
|
if (SSID.indexOf("ESK8") == 0) {
|
|
// SSID of interest
|
|
Serial.println("Found a Slave.");
|
|
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("");
|
|
// Get BSSID => Mac Address of the Slave
|
|
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] ) ) {
|
|
for (int ii = 0; ii < 6; ++ii ) {
|
|
slave.peer_addr[ii] = (uint8_t) mac[ii];
|
|
}
|
|
}
|
|
|
|
slave.channel = CHANNEL; // pick a channel
|
|
slave.encrypt = 0; // no encryption
|
|
|
|
slaveFound = 1;
|
|
// we are planning to have only one slave in this example;
|
|
// Hence, break after we find one, to be a bit efficient
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (slaveFound) {
|
|
Serial.println("Slave Found, processing..");
|
|
} else {
|
|
Serial.println("Slave Not Found, trying again.");
|
|
}
|
|
|
|
// clean up ram
|
|
WiFi.scanDelete();
|
|
}
|
|
#endif
|
|
|
|
void deletePeer() {
|
|
const esp_now_peer_info_t *peer = &slave;
|
|
const uint8_t *peer_addr = slave.peer_addr;
|
|
esp_err_t delStatus = esp_now_del_peer(peer_addr);
|
|
Serial.print("Slave Delete Status: ");
|
|
if (delStatus == ESP_OK) {
|
|
// Delete success
|
|
Serial.println("Success");
|
|
} else if (delStatus == ESP_ERR_ESPNOW_NOT_INIT) {
|
|
// How did we get so far!!
|
|
Serial.println("ESPNOW Not Init");
|
|
} else if (delStatus == ESP_ERR_ESPNOW_ARG) {
|
|
Serial.println("Invalid Argument");
|
|
} else if (delStatus == ESP_ERR_ESPNOW_NOT_FOUND) {
|
|
Serial.println("Peer not found.");
|
|
} else {
|
|
Serial.println("Not sure what happened");
|
|
}
|
|
}
|
|
|
|
// Check if the slave is already paired with the master.
|
|
// If not, pair the slave with master
|
|
bool manageSlave() {
|
|
if (slave.channel == CHANNEL) {
|
|
if (DELETEBEFOREPAIR) {
|
|
deletePeer();
|
|
}
|
|
|
|
Serial.print("Slave Status: ");
|
|
const esp_now_peer_info_t *peer = &slave;
|
|
const uint8_t *peer_addr = slave.peer_addr;
|
|
// check if the peer exists
|
|
bool exists = esp_now_is_peer_exist(peer_addr);
|
|
if ( exists) {
|
|
// Slave already paired.
|
|
Serial.println("Already Paired");
|
|
return true;
|
|
} else {
|
|
// Slave not paired, attempt pair
|
|
esp_err_t addStatus = esp_now_add_peer(peer);
|
|
if (addStatus == ESP_OK) {
|
|
// Pair success
|
|
Serial.println("Pair success");
|
|
return true;
|
|
} else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) {
|
|
// How did we get so far!!
|
|
Serial.println("ESPNOW Not Init");
|
|
return false;
|
|
} else if (addStatus == ESP_ERR_ESPNOW_ARG) {
|
|
Serial.println("Invalid Argument");
|
|
return false;
|
|
} else if (addStatus == ESP_ERR_ESPNOW_FULL) {
|
|
Serial.println("Peer list full");
|
|
return false;
|
|
} else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) {
|
|
Serial.println("Out of memory");
|
|
return false;
|
|
} else if (addStatus == ESP_ERR_ESPNOW_EXIST) {
|
|
Serial.println("Peer Exists");
|
|
return true;
|
|
} else {
|
|
Serial.println("Not sure what happened");
|
|
return false;
|
|
}
|
|
}
|
|
} else {
|
|
// No slave found to process
|
|
Serial.println("No Slave found to process");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// send data
|
|
void sendData() {
|
|
uint8_t esc1 = sendThrottle;
|
|
uint8_t esc2 = esc1;
|
|
|
|
const uint8_t data[] = { esc1, esc2 }; // no mixture for the normal mode
|
|
|
|
const uint8_t *peer_addr = slave.peer_addr;
|
|
Serial.print("Sending: "); Serial.println(esc1);
|
|
esp_err_t result = esp_now_send(peer_addr, data, sizeof(data));
|
|
Serial.print("Send Status: ");
|
|
if (result == ESP_OK) {
|
|
Serial.println("Success");
|
|
} else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
|
|
// How did we get so far!!
|
|
Serial.println("ESPNOW not Init.");
|
|
} else if (result == ESP_ERR_ESPNOW_ARG) {
|
|
Serial.println("Invalid Argument");
|
|
} else if (result == ESP_ERR_ESPNOW_INTERNAL) {
|
|
Serial.println("Internal Error");
|
|
} else if (result == ESP_ERR_ESPNOW_NO_MEM) {
|
|
Serial.println("ESP_ERR_ESPNOW_NO_MEM");
|
|
} else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
|
|
Serial.println("Peer not found.");
|
|
} else {
|
|
Serial.println("Not sure what happened");
|
|
}
|
|
}
|
|
|
|
// callback when data is sent from Master to Slave
|
|
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
|
|
char macStr[18];
|
|
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]);
|
|
Serial.print("Last Packet Sent to: "); Serial.println(macStr);
|
|
Serial.print("Last Packet Send Status: "); Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
|
|
}
|
|
//############ End ESP Now
|
|
|
|
//############ Hardware Helpers
|
|
//############
|
|
// Check if an integer is within a min and max value
|
|
bool inRange(int val, int minimum, int maximum) {
|
|
return ((minimum <= val) && (val <= maximum));
|
|
}
|
|
|
|
// Return true if trigger is activated, false otherwice
|
|
bool triggerActive() {
|
|
if (digitalRead(triggerPin) == LOW)
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
void calculateThrottlePosition() {
|
|
// Hall sensor reading can be noisy, lets make an average reading.
|
|
int total = 0;
|
|
for (int i = 0; i < 10; i++) {
|
|
total += analogRead(leverPin);
|
|
}
|
|
hallMeasurement = total / 10;
|
|
Serial.println(hallMeasurement);
|
|
//DEBUG_PRINT( (String)hallMeasurement );
|
|
|
|
if (hallMeasurement >= HAL_CENTER) {
|
|
throttle = constrain(map(hallMeasurement, HAL_CENTER, HAL_MAX, 127, 255), 127, 255);
|
|
} else {
|
|
throttle = constrain(map(hallMeasurement, HAL_MIN, HAL_CENTER, 0, 127), 0, 127);
|
|
}
|
|
// removeing center noise
|
|
if (abs(throttle - 127) < hallCenterMargin) {
|
|
throttle = 127;
|
|
}
|
|
}
|
|
|
|
// Function to calculate and return the remotes battery voltage.
|
|
float batteryVoltage() {
|
|
float batteryVoltage = 0.0;
|
|
int total = 0;
|
|
|
|
for (int i = 0; i < 10; i++) {
|
|
total += analogRead(batteryMeasurePin);
|
|
}
|
|
|
|
batteryVoltage = (refVoltage / 4096.0) * ((float)total / 10.0);
|
|
Serial.println(batteryVoltage);
|
|
return batteryVoltage;
|
|
}
|
|
|
|
// Function used to indicate the remotes battery level.
|
|
int batteryLevel() {
|
|
float voltage = batteryVoltage();
|
|
|
|
if (voltage <= minVoltage) {
|
|
return 0;
|
|
} else if (voltage >= maxVoltage) {
|
|
return 100;
|
|
} else {
|
|
return (voltage - minVoltage) * 100 / (maxVoltage - minVoltage);
|
|
}
|
|
}
|
|
|
|
// Take a number of measurements of the WiFi strength and return the average result.
|
|
int getStrength(int points){
|
|
long rssi = 0;
|
|
long averageRSSI=0;
|
|
|
|
for (int i=0; i < points; i++) {
|
|
rssi += WiFi.RSSI();
|
|
delay(20);
|
|
}
|
|
|
|
averageRSSI=rssi/points;
|
|
return averageRSSI;
|
|
}
|
|
|
|
//############ End Hardware Helpers
|
|
|
|
//############ Drawing Functions
|
|
|
|
void drawBatteryLevel() {
|
|
int level = batteryLevel();
|
|
|
|
// Position on OLED
|
|
int x = 108; int y = 4;
|
|
|
|
u8g2.drawFrame(x + 2, y, 18, 9);
|
|
u8g2.drawBox(x, y + 2, 2, 5);
|
|
|
|
for (int i = 0; i < 5; i++) {
|
|
int p = round((100 / 5) * i);
|
|
if (p <= level)
|
|
{
|
|
u8g2.drawBox(x + 4 + (3 * i), y + 2, 2, 5);
|
|
}
|
|
}
|
|
}
|
|
|
|
void drawThrottle() {
|
|
int x = 0;
|
|
int y = 18;
|
|
|
|
// Draw throttle
|
|
u8g2.drawHLine(x, y, 52);
|
|
u8g2.drawVLine(x, y, 10);
|
|
u8g2.drawVLine(x + 52, y, 10);
|
|
u8g2.drawHLine(x, y + 10, 5);
|
|
u8g2.drawHLine(x + 52 - 4, y + 10, 5);
|
|
|
|
if (sendThrottle >= 127) {
|
|
int width = map(sendThrottle, 127, 255, 0, 49);
|
|
|
|
for (int i = 0; i < width; i++) {
|
|
u8g2.drawVLine(x + i + 2, y + 2, 7);
|
|
}
|
|
} else {
|
|
int width = map(sendThrottle, 0, 126, 49, 0);
|
|
for (int i = 0; i < width; i++) {
|
|
u8g2.drawVLine(x + 50 - i, y + 2, 7);
|
|
}
|
|
}
|
|
}
|
|
|
|
void drawSignal() {
|
|
// Position on OLED
|
|
int x = 114; int y = 17;
|
|
/*
|
|
if (connected == true) {
|
|
if (triggerActive()) {
|
|
u8g2.drawXBM(x, y, 12, 12, signal_transmitting_bits);
|
|
} else {
|
|
u8g2.drawXBM(x, y, 12, 12, signal_connected_bits);
|
|
}
|
|
} else {
|
|
if (millis() - lastSignalBlink > 500) {
|
|
signalBlink = !signalBlink;
|
|
lastSignalBlink = millis();
|
|
}
|
|
|
|
if (signalBlink == true) {
|
|
u8g2.drawXBM(x, y, 12, 12, signal_connected_bits);
|
|
} else {
|
|
u8g2.drawXBM(x, y, 12, 12, signal_noconnection_bits);
|
|
}
|
|
}
|
|
*/
|
|
}
|
|
|
|
void drawTitleScreen(String title) {
|
|
u8g2.firstPage();
|
|
do {
|
|
title.toCharArray(displayBuffer, 20);
|
|
u8g2.setFont(u8g2_font_helvR10_tr );
|
|
u8g2.drawStr(12, 20, displayBuffer);
|
|
} while ( u8g2.nextPage() );
|
|
delay(1500);
|
|
}
|
|
|
|
void drawBoardVoltage() {
|
|
|
|
int x = 0;
|
|
int y = 16;
|
|
String suffix = "V";
|
|
String prefix = "BATTERY";
|
|
float value = 0.0; // TODO: No info this yet, measure in the board
|
|
|
|
// Display prefix (title)
|
|
displayString = prefix;
|
|
displayString.toCharArray(displayBuffer, 10);
|
|
u8g2.setFont(u8g2_font_profont12_tr);
|
|
u8g2.drawStr(x, y - 1, displayBuffer);
|
|
|
|
// Split up the float value: a number, b decimals.
|
|
int first = abs(floor(value));
|
|
int last = value * pow(10, 3) - first * pow(10, 3);
|
|
|
|
// Add leading zero
|
|
if (first <= 9) {
|
|
displayString = "0" + (String)first;
|
|
} else {
|
|
displayString = (String)first;
|
|
}
|
|
|
|
// Display numbers
|
|
displayString.toCharArray(displayBuffer, 10);
|
|
u8g2.setFont(u8g2_font_logisoso22_tn );
|
|
u8g2.drawStr(x + 55, y + 13, displayBuffer);
|
|
|
|
// Display decimals
|
|
displayString = "." + (String)last;
|
|
displayString.toCharArray(displayBuffer, 3);
|
|
u8g2.setFont(u8g2_font_profont12_tr);
|
|
u8g2.drawStr(x + 86, y - 1, displayBuffer);
|
|
|
|
// Display suffix
|
|
displayString = suffix;
|
|
displayString.toCharArray(displayBuffer, 10);
|
|
u8g2.setFont(u8g2_font_profont12_tr);
|
|
u8g2.drawStr(x + 86 + 2, y + 13, displayBuffer);
|
|
}
|
|
|
|
void drawSettingsMenu() {
|
|
/*
|
|
// Position on OLED
|
|
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.drawStr(x, y, 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);
|
|
}
|
|
*/
|
|
}
|
|
|
|
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() {
|
|
|
|
u8g2.firstPage();
|
|
do {
|
|
|
|
if (changeSettings == true) {
|
|
drawSettingsMenu();
|
|
drawSettingNumber();
|
|
} else {
|
|
drawThrottle();
|
|
drawBoardVoltage();
|
|
drawBatteryLevel();
|
|
drawSignal();
|
|
}
|
|
|
|
} while ( u8g2.nextPage() );
|
|
}
|
|
|
|
void drawStartScreen() {
|
|
u8g2.firstPage();
|
|
do {
|
|
u8g2.drawXBM( 4, 4, 24, 24, logo_bits);
|
|
|
|
displayString = "Esk8 remote";
|
|
displayString.toCharArray(displayBuffer, 12);
|
|
u8g2.setFont(u8g2_font_helvR10_tr );
|
|
u8g2.drawStr(34, 22, displayBuffer);
|
|
} while ( u8g2.nextPage() );
|
|
delay(1500);
|
|
}
|
|
|
|
//############ End Drawing Functions
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
//Set device in STA mode to begin with
|
|
WiFi.mode(WIFI_STA);
|
|
Serial.println("ESPNowSkate");
|
|
|
|
// reset the screen
|
|
pinMode(16, OUTPUT);
|
|
digitalWrite(16, LOW); // set GPIO16 low to reset OLED
|
|
delay(50);
|
|
digitalWrite(16, HIGH);
|
|
|
|
// setup other pins
|
|
pinMode(triggerPin, INPUT_PULLUP);
|
|
|
|
Serial.println("ESPNowSkate Sender");
|
|
u8g2.begin();
|
|
drawStartScreen();
|
|
|
|
if (triggerActive()) {
|
|
changeSettings = true;
|
|
drawTitleScreen("Remote Settings");
|
|
}
|
|
|
|
// This is the mac address of the Master in Station Mode
|
|
Serial.print("STA MAC: "); Serial.println(WiFi.macAddress());
|
|
|
|
if (esp_now_init() == ESP_OK) {
|
|
Serial.println("ESPNow Init Success");
|
|
}
|
|
else {
|
|
Serial.println("ESPNow Init Failed");
|
|
ESP.restart();
|
|
}
|
|
|
|
// Once ESPNow is successfully Init, we will register for Send CB to
|
|
// get the status of Trasnmitted packet
|
|
esp_now_register_send_cb(OnDataSent);
|
|
//ScanForSlave();
|
|
|
|
|
|
// Retrieve Slave from config:
|
|
for (int i = 0; i < 6; ++i ) {
|
|
slave.peer_addr[i] = (uint8_t) mac_receiver[i];
|
|
}
|
|
slave.channel = CHANNEL; // pick a channel
|
|
slave.encrypt = 0; // no encryption
|
|
}
|
|
|
|
void loop() {
|
|
// Call function to update display and LED
|
|
updateMainDisplay();
|
|
|
|
calculateThrottlePosition();
|
|
|
|
if (changeSettings == true) {
|
|
// Use throttle and trigger to change settings
|
|
//controlSettingsMenu();
|
|
} else {
|
|
// Use throttle and trigger to drive motors
|
|
if (triggerActive()) {
|
|
sendThrottle = throttle;
|
|
} else {
|
|
// 127 is the middle position - no throttle and no brake/reverse
|
|
sendThrottle = 127;
|
|
}
|
|
|
|
// If Slave is found, it would be populate in `slave` variable
|
|
// We will check if `slave` is defined and then we proceed further
|
|
if (slave.channel == CHANNEL) { // check if slave channel is defined
|
|
// `slave` is defined
|
|
// Add slave as peer if it has not been added already
|
|
bool isPaired = manageSlave();
|
|
if (isPaired) {
|
|
// pair success or already paired
|
|
// Send data to device
|
|
|
|
sendData();
|
|
/* update Value
|
|
char buf[10];
|
|
sprintf(buf, "%i", map(analogRead(leverPin), HAL_MIN, HAL_MAX, TRIM_LOW, TRIM_HIGH));
|
|
u8g2.firstPage();
|
|
do {
|
|
u8g2.setFont(u8g2_font_10x20_tr );
|
|
u8g2.drawStr(0, 20, buf);
|
|
} while ( u8g2.nextPage() );
|
|
*/
|
|
} else {
|
|
// slave pair failed
|
|
|
|
Serial.println("Slave not found / paired!");
|
|
}
|
|
}
|
|
else {
|
|
// No slave found to process
|
|
}
|
|
|
|
delay(20);
|
|
}
|
|
}
|