109 lines
2.8 KiB
C++
109 lines
2.8 KiB
C++
#include <WiFi.h>
|
|
#include <esp_now.h>
|
|
#include <esp_wifi.h>
|
|
#include <ESP32Servo.h>
|
|
|
|
//------------------------------------------------
|
|
// This is The Car Unit
|
|
//------------------------------------------------
|
|
// RC Car Remote MAC Address: 64:E8:33:B5:D8:40
|
|
// RC Car MAC Address: 50:78:7D:43:14:00
|
|
//------------------------------------------------
|
|
|
|
Servo leftServoA, leftServoB, rightServoA, rightServoB;
|
|
int leftPins[2] = {0, 1};
|
|
int rightPins[2] = {2, 3};
|
|
|
|
typedef struct {
|
|
int speed;
|
|
int turn;
|
|
} CommandPacket;
|
|
|
|
typedef struct {
|
|
int leftSpeed;
|
|
int rightSpeed;
|
|
int battery;
|
|
} TelemetryPacket;
|
|
|
|
CommandPacket command;
|
|
TelemetryPacket telemetry;
|
|
|
|
unsigned long lastTelemetrySent = 0;
|
|
|
|
// RC Car Remote MAC Address: 64:E8:33:B5:D8:40
|
|
uint8_t remoteAddr[] = {0x64, 0xE8, 0x33, 0xB5, 0xD8, 0x40}; // MAC of remote
|
|
|
|
void onCommandRecv(const esp_now_recv_info_t *recv_info, const uint8_t *incomingData, int len) {
|
|
memcpy(&command, incomingData, sizeof(command));
|
|
|
|
int left = constrain(command.speed + command.turn, -255, 255);
|
|
int right = constrain(command.speed - command.turn, -255, 255);
|
|
|
|
int pwmLeft = map(left, -255, 255, 0, 180);
|
|
int pwmRight = map(right, -255, 255, 0, 180);
|
|
|
|
leftServoA.write(pwmLeft);
|
|
leftServoB.write(pwmLeft);
|
|
rightServoA.write(pwmRight);
|
|
rightServoB.write(pwmRight);
|
|
|
|
telemetry.leftSpeed = left;
|
|
telemetry.rightSpeed = right;
|
|
}
|
|
|
|
// Function to ensure peer is always registered
|
|
void ensurePeerConnected() {
|
|
if (!esp_now_is_peer_exist(remoteAddr)) {
|
|
esp_now_peer_info_t peerInfo = {};
|
|
memcpy(peerInfo.peer_addr, remoteAddr, 6);
|
|
peerInfo.channel = 0;
|
|
peerInfo.encrypt = false;
|
|
esp_err_t res = esp_now_add_peer(&peerInfo);
|
|
if (res == ESP_OK) {
|
|
Serial.println("Re-added remote peer");
|
|
} else {
|
|
Serial.printf("Failed to add peer: %d\n", res);
|
|
}
|
|
}
|
|
}
|
|
|
|
void setup() {
|
|
|
|
Serial.begin(115200);
|
|
leftServoA.attach(leftPins[0]);
|
|
leftServoB.attach(leftPins[1]);
|
|
rightServoA.attach(rightPins[0]);
|
|
rightServoB.attach(rightPins[1]);
|
|
WiFi.mode(WIFI_STA);
|
|
PrintMacAddress();
|
|
|
|
esp_now_init();
|
|
esp_now_peer_info_t peerInfo = {};
|
|
memcpy(peerInfo.peer_addr, remoteAddr, 6);
|
|
peerInfo.channel = 0;
|
|
peerInfo.encrypt = false;
|
|
esp_now_add_peer(&peerInfo);
|
|
esp_now_register_recv_cb(onCommandRecv);
|
|
|
|
}
|
|
|
|
void loop() {
|
|
|
|
ensurePeerConnected();
|
|
|
|
if (millis() - lastTelemetrySent > 500) {
|
|
telemetry.battery = analogRead(36); // Adjust this as needed
|
|
esp_now_send(remoteAddr, (uint8_t*)&telemetry, sizeof(telemetry));
|
|
lastTelemetrySent = millis();
|
|
}
|
|
|
|
}
|
|
|
|
void PrintMacAddress() {
|
|
delay(100); // Let hardware settle
|
|
uint8_t mac[6];
|
|
esp_wifi_get_mac(WIFI_IF_STA, mac); // Get MAC address for STA
|
|
Serial.print("Car MAC Address: ");
|
|
Serial.printf("%02X:%02X:%02X:%02X:%02X:%02X\n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
|
|
}
|