68 lines
1.9 KiB
C++
68 lines
1.9 KiB
C++
#include <WiFi.h>
|
|
#include <esp_now.h>
|
|
#include <ESP32Servo.h>
|
|
|
|
Servo leftServo1, leftServo2, rightServo1, rightServo2;
|
|
int leftServoPin1 = 12, leftServoPin2 = 13, rightServoPin1 = 14, rightServoPin2 = 15;
|
|
|
|
typedef struct {
|
|
int speed;
|
|
int turn;
|
|
} CommandPacket;
|
|
|
|
typedef struct {
|
|
int leftServoSpeed;
|
|
int rightServoSpeed;
|
|
int batteryLevel;
|
|
} TelemetryPacket;
|
|
|
|
CommandPacket command;
|
|
uint8_t controllerAddr[] = {0x24, 0x6F, 0x28, 0x33, 0xDE, 0xB4}; // Remote MAC
|
|
|
|
void onCommandRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
|
|
memcpy(&command, incomingData, sizeof(command));
|
|
int leftSpeed = constrain(command.speed + command.turn, -255, 255);
|
|
int rightSpeed = constrain(command.speed - command.turn, -255, 255);
|
|
|
|
// Map speed value to servo position (0-180 for continuous rotation speed control)
|
|
int leftPWM = map(leftSpeed, -255, 255, 0, 180);
|
|
int rightPWM = map(rightSpeed, -255, 255, 0, 180);
|
|
|
|
leftServo1.write(leftPWM);
|
|
leftServo2.write(leftPWM);
|
|
rightServo1.write(rightPWM);
|
|
rightServo2.write(rightPWM);
|
|
|
|
// Store for telemetry
|
|
telemetry.leftServoSpeed = leftSpeed;
|
|
telemetry.rightServoSpeed = rightSpeed;
|
|
}
|
|
|
|
TelemetryPacket telemetry;
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
leftServo1.attach(leftServoPin1);
|
|
leftServo2.attach(leftServoPin2);
|
|
rightServo1.attach(rightServoPin1);
|
|
rightServo2.attach(rightServoPin2);
|
|
WiFi.mode(WIFI_STA);
|
|
esp_now_init();
|
|
esp_now_peer_info_t peerInfo;
|
|
memcpy(peerInfo.peer_addr, controllerAddr, 6);
|
|
peerInfo.channel = 0;
|
|
peerInfo.encrypt = false;
|
|
esp_now_add_peer(&peerInfo);
|
|
esp_now_register_recv_cb(onCommandRecv);
|
|
}
|
|
|
|
unsigned long lastTelemetrySent = 0;
|
|
void loop() {
|
|
// Telemetry every 500 ms
|
|
if (millis() - lastTelemetrySent > 500) {
|
|
telemetry.batteryLevel = analogRead(36); // Replace with battery calculation if needed
|
|
esp_now_send(controllerAddr, (uint8_t*)&telemetry, sizeof(telemetry));
|
|
lastTelemetrySent = millis();
|
|
}
|
|
}
|