#include <esp_now.h>
#include <WiFi.h>
// Receiver MAC Address
uint8_t broadcastAddress[] = {0x30, 0xAE, 0xA4, 0x96, 0x5D, 0xEC};
// Tunable
int numReadings = 5; // Number of readings to average
int minSteering = 460; // What the pots/analog inputs are actually giving out as max and min
int maxSteering = 3200;
int minThrottle = 300;
int maxThrottle = 3200;
int throttleDeadband = 120; // Set the deadband range for the throttle here
int steeringDeadband = 50; // Set the deadband range for the steering here
int midPointRawSteering = 1950; // Set your desired midpoint for Steering here
int midPointRawThrottle = 1200;
int midPointMappedThrottle = 1500;
int SteeringTrimInfluence_min = -200;
int SteeringTrimInfluence_max = 200;
int steeringTrimDeadband = 20; // Set your desired deadband for changes in steeringTrim
unsigned long debounceDelay = 50; // Adjust the debounce delay as needed
// Endpoint adjustments // Flip values to reverse channel
int HighGearValue = 2000;
int LowGearValue = 1000;
int LockServo1_min = 1000;
int LockServo1_max = 2000;
int LockServo2_min = 2000;
int LockServo2_max = 1000;
int ThrottleEPA_max = 2000;
int ThrottleEPA_min = 1000;
int SteeringEPA_max = 2000;
int SteeringEPA_min = 1000;
// Non-tunable
int lastSteeringTrim = 0;
int steeringReadings[5] = {0, 0, 0, 0, 0};
int throttleReadings[5] = {0, 0, 0, 0, 0};
int filteredSteering = 0;
int filteredThrottle = 0;
int filteredSteeringTrim = 0;
bool firstSuccessfulPackage = false;
// Debounce variables
unsigned long lastHighGearTime = 0;
bool highGearState = HIGH;
unsigned long lastLowGearTime = 0;
bool lowGearState = HIGH;
unsigned long lastLockFTime = 0;
bool lockFState = HIGH;
unsigned long lastLockBTime = 0;
bool lockBState = HIGH;
// Pin assignments
const int analogInputPinSteering = 32;
const int analogInputPinThrottle = 33;
const int analogInputPinSteeringTrim = 35;
const int analogInputPinThrottleTrim = 34;
const int digitalInputPinHighGear = 18;
const int digitalInputPinLowGear = 19;
const int digitalInputPinLockF = 16;
const int digitalInputPinLockB = 17;
const int beeper = 27;
unsigned long lastSerialPrintTime = 0;
unsigned long serialPrintInterval = 500; // Update interval for Serial print in milliseconds
int totalPackagesSent = 0;
int successfulPackages = 0;
// Structure to send data
typedef struct struct_message {
int steering;
int throttle;
int gear;
int lockServo1;
int lockServo2;
} struct_message;
struct_message myData;
esp_now_peer_info_t peerInfo;
// Callback when data is sent
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
totalPackagesSent++; // Update how many packages were sent
if (status == ESP_NOW_SEND_SUCCESS) {
successfulPackages++; // Update how many were successful
}
}
void setup() {
Serial.begin(115200);
// Set up pin modes
pinMode(digitalInputPinHighGear, INPUT_PULLUP);
pinMode(digitalInputPinLowGear, INPUT_PULLUP);
pinMode(digitalInputPinLockF, INPUT_PULLUP);
pinMode(digitalInputPinLockB, INPUT_PULLUP);
pinMode(beeper, OUTPUT);
// Set up WiFi and ESPNow
WiFi.mode(WIFI_STA);
if (esp_now_init() != ESP_OK) {
Serial.println("Error initializing ESP-NOW");
return;
}
esp_now_register_send_cb(OnDataSent);
// Set up peer information
memcpy(peerInfo.peer_addr, broadcastAddress, 6);
peerInfo.channel = 0;
peerInfo.encrypt = false;
if (esp_now_add_peer(&peerInfo) != ESP_OK) {
Serial.println("Failed to add peer");
return;
}
// Play startup sounds on the beeper
for (int i = 0; i < 10; i++) {
digitalWrite(beeper, HIGH);
delay(10);
digitalWrite(beeper, LOW);
delay(10);
}
delay(200);
for (int i = 0; i < 10; i++) {
digitalWrite(beeper, HIGH);
delay(10);
digitalWrite(beeper, LOW);
delay(10);
}
}
void loop() {
// Read raw values from analog input pins
int rawSteering = analogRead(analogInputPinSteering);
int rawThrottle = analogRead(analogInputPinThrottle);
int rawSteeringTrim = analogRead(analogInputPinSteeringTrim);
int rawThrottleTrim = analogRead(analogInputPinThrottleTrim);
// Apply averaging filter to raw Steering
for (int i = 0; i < numReadings - 1; ++i) {
steeringReadings[i] = steeringReadings[i + 1];
}
steeringReadings[numReadings - 1] = rawSteering;
filteredSteering = 0;
for (int i = 0; i < numReadings; ++i) {
filteredSteering += steeringReadings[i];
}
filteredSteering /= numReadings;
// Apply averaging filter to raw Throttle
for (int i = 0; i < numReadings - 1; ++i) {
throttleReadings[i] = throttleReadings[i + 1];
}
throttleReadings[numReadings - 1] = rawThrottle;
filteredThrottle = 0;
for (int i = 0; i < numReadings; ++i) {
filteredThrottle += throttleReadings[i];
}
filteredThrottle /= numReadings;
// Apply averaging filter to raw SteeringTrim
filteredSteeringTrim = rawSteeringTrim;
// Apply deadband for Steering
if (filteredSteering >= midPointRawSteering - steeringDeadband && filteredSteering <= midPointRawSteering + steeringDeadband) {
filteredSteering = midPointRawSteering;
}
// Apply deadband for SteeringTrim
if (abs(filteredSteeringTrim - lastSteeringTrim) < steeringTrimDeadband) {
filteredSteeringTrim = lastSteeringTrim;
}
lastSteeringTrim = filteredSteeringTrim;
// Map the SteeringTrim to an influence range
int mappedSteeringTrim = map(filteredSteeringTrim, 0, 4095, SteeringTrimInfluence_min, SteeringTrimInfluence_max);
// Constrain values within defined limits
filteredSteering = constrain(filteredSteering, minSteering, maxSteering);
filteredThrottle = constrain(filteredThrottle, minThrottle, maxThrottle);
// Map the Steering and Throttle values to their defined ranges
int mappedSteering = map(filteredSteering, minSteering, maxSteering, SteeringEPA_max, SteeringEPA_min);
mappedSteering += mappedSteeringTrim;
mappedSteering = constrain(mappedSteering, SteeringEPA_min, SteeringEPA_max);
// Apply deadband for Throttle
if (filteredThrottle >= midPointRawThrottle - throttleDeadband && filteredThrottle <= midPointRawThrottle + throttleDeadband) {
filteredThrottle = midPointRawThrottle;
}
// Map the Throttle value to its defined range
int mappedThrottle = map(filteredThrottle, minThrottle, maxThrottle, ThrottleEPA_max, ThrottleEPA_min);
if (filteredThrottle > midPointRawThrottle) {
mappedThrottle = map(filteredThrottle, midPointRawThrottle, maxThrottle, midPointMappedThrottle, ThrottleEPA_min);
} else {
mappedThrottle = map(filteredThrottle, minThrottle, midPointRawThrottle, ThrottleEPA_max, midPointMappedThrottle);
}
// Read digital input values for gear shifting and servo locking
int digitalValueHighGear = digitalRead(digitalInputPinHighGear);
if (digitalValueHighGear != highGearState && millis() - lastHighGearTime > debounceDelay) {
lastHighGearTime = millis();
highGearState = digitalValueHighGear;
}
int digitalValueLowGear = digitalRead(digitalInputPinLowGear);
if (digitalValueLowGear != lowGearState && millis() - lastLowGearTime > debounceDelay) {
lastLowGearTime = millis();
lowGearState = digitalValueLowGear;
}
int digitalValueLockF = digitalRead(digitalInputPinLockF);
if (digitalValueLockF != lockFState && millis() - lastLockFTime > debounceDelay) {
lastLockFTime = millis();
lockFState = digitalValueLockF;
}
int digitalValueLockB = digitalRead(digitalInputPinLockB);
if (digitalValueLockB != lockBState && millis() - lastLockBTime > debounceDelay) {
lastLockBTime = millis();
lockBState = digitalValueLockB;
}
// Determine the gear based on high/low gear states
int Gear = (highGearState == HIGH && lowGearState == LOW) ? HighGearValue : LowGearValue;
// Determine servo positions based on locking states
int LockServo1 = 0;
int LockServo2 = 0;
if (lockFState == LOW && lockBState == HIGH) {
LockServo1 = LockServo1_min;
LockServo2 = LockServo2_min;
} else if (lockFState == HIGH && lockBState == HIGH) {
LockServo1 = LockServo1_max;
LockServo2 = LockServo2_min;
} else if (lockFState == HIGH && lockBState == LOW) {
LockServo1 = LockServo1_max;
LockServo2 = LockServo2_max;
}
// Populate the data structure
myData.steering = static_cast<int>(mappedSteering);
myData.throttle = static_cast<int>(mappedThrottle);
myData.gear = Gear;
myData.lockServo1 = LockServo1;
myData.lockServo2 = LockServo2;
// Send data using ESP-NOW
esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&myData, sizeof(myData));
// Print information to Serial Monitor at specified intervals
if (millis() - lastSerialPrintTime >= serialPrintInterval) {
lastSerialPrintTime = millis();
// Print successful data packages
Serial.print("Successful data packages: ");
Serial.print(successfulPackages);
Serial.print("/");
Serial.println(totalPackagesSent);
// Beeper warning code
if (successfulPackages > 0) {
firstSuccessfulPackage = true; // Set the flag after the first successful package
}
// Check if more than half of packages are lost after the first successful package
if (firstSuccessfulPackage == true) {
if (successfulPackages < totalPackagesSent / 2) {
digitalWrite(beeper, !digitalRead(beeper)); // Toggle the output state
} else {
digitalWrite(beeper, LOW);
}
}
// Reset counts after printing
totalPackagesSent = 0;
successfulPackages = 0;
/*// Print various sensor and control values
Serial.print("Steering (Raw): ");
Serial.print(filteredSteering);
Serial.print(" | Throttle (Raw): ");
Serial.print(filteredThrottle);
Serial.print(" | Steering: ");
Serial.print(mappedSteering);
Serial.print(" | Throttle: ");
Serial.print(mappedThrottle);
Serial.print(" | SteeringTrim (Raw): ");
Serial.print(filteredSteeringTrim);
Serial.print(" | MappedSteeringTrim: ");
Serial.print(mappedSteeringTrim);
Serial.print(" | HighGear: ");
Serial.print(highGearState);
Serial.print(" | LowGear: ");
Serial.print(lowGearState);
Serial.print(" | LockF: ");
Serial.print(lockFState);
Serial.print(" | LockB: ");
Serial.print(lockBState);
Serial.print(" | Gear: ");
Serial.print(Gear);
Serial.print(" | LockServo1: ");
Serial.print(LockServo1);
Serial.print(" | LockServo2: ");
Serial.println(LockServo2);*/
}
}
