the code didnt work so i created example sender and receiver code
This commit is contained in:
parent
dfbcc976e6
commit
7ae4d9ae5e
58
espnow example/espnow_receiver_code/espnow_receiver_code.ino
Normal file
58
espnow example/espnow_receiver_code/espnow_receiver_code.ino
Normal file
@ -0,0 +1,58 @@
|
|||||||
|
#include <esp_now.h>
|
||||||
|
#include <WiFi.h>
|
||||||
|
|
||||||
|
// Structure example to receive data
|
||||||
|
// Must match the sender structure
|
||||||
|
typedef struct struct_message {
|
||||||
|
int x;
|
||||||
|
int y;
|
||||||
|
bool b;
|
||||||
|
} struct_message;
|
||||||
|
|
||||||
|
// Create a struct_message called myData
|
||||||
|
struct_message myData;
|
||||||
|
|
||||||
|
// Callback function that will be executed when data is received
|
||||||
|
void OnDataRecv(const esp_now_recv_info_t *esp_now_info, const uint8_t *incomingData, int len) {
|
||||||
|
// Copy incoming data into the myData structure
|
||||||
|
memcpy(&myData, incomingData, sizeof(myData));
|
||||||
|
|
||||||
|
// Print the MAC address of the sender
|
||||||
|
// Serial.print("Received from MAC: ");
|
||||||
|
// for (int i = 0; i < 6; i++) {
|
||||||
|
// Serial.printf("%02X", esp_now_info->src_addr[i]);
|
||||||
|
// if (i < 5) Serial.print(":");
|
||||||
|
// }
|
||||||
|
// Serial.println();
|
||||||
|
|
||||||
|
// Print received data
|
||||||
|
// Serial.print("x: ");
|
||||||
|
Serial.print(myData.x);
|
||||||
|
Serial.print(" ");
|
||||||
|
Serial.print(myData.y);
|
||||||
|
Serial.print(" ");
|
||||||
|
Serial.println(myData.b);
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
// Initialize Serial Monitor
|
||||||
|
Serial.begin(115200);
|
||||||
|
|
||||||
|
// Set device as a Wi-Fi Station
|
||||||
|
WiFi.mode(WIFI_STA);
|
||||||
|
|
||||||
|
// Initialize ESP-NOW
|
||||||
|
if (esp_now_init() != ESP_OK) {
|
||||||
|
Serial.println("Error initializing ESP-NOW");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Register callback function to receive data
|
||||||
|
esp_now_register_recv_cb(OnDataRecv);
|
||||||
|
|
||||||
|
Serial.println("Receiver ready. Waiting for data...");
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
// Nothing to do here
|
||||||
|
}
|
82
espnow example/espnow_sender_code/espnow_sender_code.ino
Normal file
82
espnow example/espnow_sender_code/espnow_sender_code.ino
Normal file
@ -0,0 +1,82 @@
|
|||||||
|
#include <esp_now.h>
|
||||||
|
#include <WiFi.h>
|
||||||
|
|
||||||
|
// REPLACE WITH THE MAC Address of your receiver - 1C:69:20:E9:13:EC
|
||||||
|
uint8_t broadcastAddress[] = {0x1C, 0x69, 0x20, 0xE9, 0x13, 0xEC}; // Replace with actual receiver MAC address
|
||||||
|
|
||||||
|
// Define variables to be sent
|
||||||
|
typedef struct struct_message {
|
||||||
|
int x;
|
||||||
|
int y;
|
||||||
|
bool b;
|
||||||
|
} struct_message;
|
||||||
|
|
||||||
|
// Create a struct_message called myData
|
||||||
|
struct_message myData;
|
||||||
|
|
||||||
|
// Callback when data is sent
|
||||||
|
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
|
||||||
|
// Serial.print("Last Packet Send Status: ");
|
||||||
|
// Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
|
||||||
|
// Initialize Serial Monitor
|
||||||
|
Serial.begin(115200);
|
||||||
|
|
||||||
|
pinMode(34, INPUT_PULLUP);
|
||||||
|
|
||||||
|
// Set device as a Wi-Fi Station
|
||||||
|
WiFi.mode(WIFI_STA);
|
||||||
|
|
||||||
|
// Initialize ESP-NOW
|
||||||
|
if (esp_now_init() != ESP_OK) {
|
||||||
|
Serial.println("Error initializing ESP-NOW");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Register callback function to get the status of transmitted packets
|
||||||
|
esp_now_register_send_cb(OnDataSent);
|
||||||
|
|
||||||
|
// Register peer information (receiver)
|
||||||
|
esp_now_peer_info_t peerInfo;
|
||||||
|
|
||||||
|
memcpy(peerInfo.peer_addr, broadcastAddress, sizeof(broadcastAddress));
|
||||||
|
peerInfo.channel = 0;
|
||||||
|
peerInfo.encrypt = false;
|
||||||
|
|
||||||
|
// Add peer
|
||||||
|
if (esp_now_add_peer(&peerInfo) != ESP_OK) {
|
||||||
|
Serial.println("Failed to add peer");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
Serial.println("Sender ready. Sending data...");
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
|
||||||
|
// Set values to send (example: read analog pins and digital pin)
|
||||||
|
myData.x = map(analogRead(32), 0, 4095, -100, +100); // Map analog value from pin GPIO32
|
||||||
|
myData.y = map(analogRead(33), 0, 4095, -100, +100); // Map analog value from pin GPIO33
|
||||||
|
myData.b = !digitalRead(34); // Read digital value from pin GPIO34
|
||||||
|
|
||||||
|
// Send message via ESP-NOW
|
||||||
|
esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &myData, sizeof(myData));
|
||||||
|
|
||||||
|
if (result == ESP_OK) {
|
||||||
|
// Serial.print("Sent successfully -> ");
|
||||||
|
// Serial.print("x: ");
|
||||||
|
Serial.print(myData.x);
|
||||||
|
Serial.print(" ");
|
||||||
|
Serial.print(myData.y);
|
||||||
|
Serial.print(" ");
|
||||||
|
Serial.println(myData.b);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
Serial.println("Error sending the data");
|
||||||
|
}
|
||||||
|
|
||||||
|
delay(20); // Small delay between sends to avoid flooding the network
|
||||||
|
}
|
@ -1,194 +1,144 @@
|
|||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
#include "L298N_ESP32.h"
|
#include "espnow.h"
|
||||||
#include "Servo_ESP32.h"
|
#include "motor.h"
|
||||||
|
#include "servo.h"
|
||||||
|
#include "fuzzy.h"
|
||||||
|
|
||||||
// Motor 1 Pins
|
#define terminal Serial
|
||||||
#define MOTOR1_PIN1 27
|
|
||||||
#define MOTOR1_PIN2 26
|
|
||||||
#define MOTOR1_ENABLE 14
|
|
||||||
|
|
||||||
// Motor 2 Pins
|
void setup() {
|
||||||
#define MOTOR2_PIN1 25
|
|
||||||
#define MOTOR2_PIN2 33
|
|
||||||
#define MOTOR2_ENABLE 32
|
|
||||||
|
|
||||||
// Servo Pins
|
// Terminal
|
||||||
#define SERVO1_PIN 13
|
terminal.begin(115200);
|
||||||
#define SERVO2_PIN 12
|
|
||||||
|
|
||||||
// Motor Objects
|
terminal.println("Hello");
|
||||||
L298N_ESP32 motor1(MOTOR1_PIN1, MOTOR1_PIN2, MOTOR1_ENABLE, 0);
|
|
||||||
L298N_ESP32 motor2(MOTOR2_PIN1, MOTOR2_PIN2, MOTOR2_ENABLE, 1);
|
|
||||||
|
|
||||||
// Servo Objects
|
// ESPNOW Communications
|
||||||
Servo_ESP32 servo1(SERVO1_PIN, 2, 1000, 2000);
|
espnow_begin();
|
||||||
Servo_ESP32 servo2(SERVO2_PIN, 3, 1000, 2000);
|
|
||||||
|
|
||||||
// ESPNOW
|
// Initialize Motors
|
||||||
#include <esp_now.h>
|
motor1.begin();
|
||||||
#include <WiFi.h>
|
motor2.begin();
|
||||||
|
|
||||||
// Packet Data Structure
|
// Initialize Servos
|
||||||
struct PacketData {
|
servo1.begin();
|
||||||
uint8_t X; // Steering (left/right)
|
servo2.begin();
|
||||||
uint8_t Y; // Throttle (forward/backward)
|
|
||||||
bool B; // Button
|
|
||||||
};
|
|
||||||
|
|
||||||
// Data Received From Remote
|
// Print Terminal Message
|
||||||
PacketData data;
|
// terminal.println("Initialization complete.");
|
||||||
|
|
||||||
// Constants
|
|
||||||
const unsigned long MOTOR_ACTION_DELAY = 5000; // Delay between motor actions
|
|
||||||
const int SERVO1_START = 10;
|
|
||||||
const int SERVO1_END = 170;
|
|
||||||
const int SERVO2_START = 20;
|
|
||||||
const int SERVO2_END = 160;
|
|
||||||
|
|
||||||
enum MotorState {
|
|
||||||
STOPPED,
|
|
||||||
FORWARD_M1,
|
|
||||||
BACKWARD_M1,
|
|
||||||
FORWARD_M2,
|
|
||||||
BACKWARD_M2
|
|
||||||
};
|
|
||||||
|
|
||||||
MotorState currentMotorState = STOPPED;
|
|
||||||
unsigned long motorStateStartTime = 0;
|
|
||||||
|
|
||||||
void setup() {
|
|
||||||
|
|
||||||
Serial.begin(115200);
|
|
||||||
|
|
||||||
// Initialize Motors
|
|
||||||
motor1.begin();
|
|
||||||
motor2.begin();
|
|
||||||
|
|
||||||
// Initialize Servos
|
|
||||||
servo1.begin();
|
|
||||||
servo2.begin();
|
|
||||||
|
|
||||||
Serial.println("Initialization complete.");
|
|
||||||
|
|
||||||
// ESP-NOW setup
|
|
||||||
WiFi.mode(WIFI_STA);
|
|
||||||
if (esp_now_init() != ESP_OK) {
|
|
||||||
Serial.println("ESP-NOW init failed");
|
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// When Receive Data Execute OnDataRecv Function
|
void loop() {
|
||||||
esp_now_register_recv_cb(OnDataRecv);
|
|
||||||
|
|
||||||
}
|
terminal.print(data.X); // 0% to 100%
|
||||||
|
terminal.print(" ");
|
||||||
|
terminal.print(data.Y);
|
||||||
|
terminal.print(" ");
|
||||||
|
terminal.println(data.B);
|
||||||
|
|
||||||
void loop() {
|
delay(20);
|
||||||
|
|
||||||
// Non Blocking Motor control
|
// // Non Blocking Motor control
|
||||||
if(millis() - motorStateStartTime >= MOTOR_ACTION_DELAY) {
|
// if(millis() - motorStateStartTime >= MOTOR_ACTION_DELAY) {
|
||||||
|
|
||||||
motorStateStartTime = millis();
|
// motorStateStartTime = millis();
|
||||||
|
|
||||||
switch (currentMotorState) {
|
// switch(currentMotorState) {
|
||||||
|
|
||||||
case STOPPED:
|
// case STOPPED:
|
||||||
Serial.println("Moving motor 1 forward...");
|
// Serial.println("Moving motor 1 forward...");
|
||||||
motor1.forward();
|
// motor1.forward();
|
||||||
motor1.setPercentage(75); //PWM set percentage
|
// motor1.setPercentage(75); //PWM set percentage
|
||||||
motor2.stop();
|
// motor2.stop();
|
||||||
currentMotorState = FORWARD_M1;
|
// currentMotorState = FORWARD_M1;
|
||||||
break;
|
// break;
|
||||||
case FORWARD_M1:
|
// case FORWARD_M1:
|
||||||
Serial.println("Moving motor 1 backward...");
|
// Serial.println("Moving motor 1 backward...");
|
||||||
motor1.backward();
|
// motor1.backward();
|
||||||
motor1.setPercentage(50); //PWM set percentage
|
// motor1.setPercentage(50); //PWM set percentage
|
||||||
motor2.stop();
|
// motor2.stop();
|
||||||
currentMotorState = BACKWARD_M1;
|
// currentMotorState = BACKWARD_M1;
|
||||||
break;
|
// break;
|
||||||
case BACKWARD_M1:
|
// case BACKWARD_M1:
|
||||||
Serial.println("Moving motor 2 forward...");
|
// Serial.println("Moving motor 2 forward...");
|
||||||
motor1.stop();
|
// motor1.stop();
|
||||||
motor2.forward();
|
// motor2.forward();
|
||||||
motor2.setPercentage(25); //PWM set percentage
|
// motor2.setPercentage(25); //PWM set percentage
|
||||||
currentMotorState = FORWARD_M2;
|
// currentMotorState = FORWARD_M2;
|
||||||
break;
|
// break;
|
||||||
case FORWARD_M2:
|
// case FORWARD_M2:
|
||||||
Serial.println("Moving motor 2 backward...");
|
// Serial.println("Moving motor 2 backward...");
|
||||||
motor1.stop();
|
// motor1.stop();
|
||||||
motor2.backward();
|
// motor2.backward();
|
||||||
motor2.setPercentage(100); //PWM set percentage
|
// motor2.setPercentage(100); //PWM set percentage
|
||||||
currentMotorState = BACKWARD_M2;
|
// currentMotorState = BACKWARD_M2;
|
||||||
break;
|
// break;
|
||||||
case BACKWARD_M2:
|
// case BACKWARD_M2:
|
||||||
Serial.println("Stopping motors ...");
|
// Serial.println("Stopping motors ...");
|
||||||
motor1.stop();
|
// motor1.stop();
|
||||||
motor2.stop();
|
// motor2.stop();
|
||||||
currentMotorState = STOPPED;
|
// currentMotorState = STOPPED;
|
||||||
break;
|
// break;
|
||||||
|
|
||||||
}
|
// }
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // Non Blocking Servos
|
||||||
|
// static unsigned long servoTimer = 0;
|
||||||
|
// static int servo1Pos = SERVO1_START;
|
||||||
|
// static int servo2Pos = SERVO2_START;
|
||||||
|
// static bool servoForward = true;
|
||||||
|
// unsigned long currentTime = millis();
|
||||||
|
|
||||||
|
// if(currentTime - servoTimer >= 20) { // Adjust for speed
|
||||||
|
|
||||||
|
// servoTimer = currentTime;
|
||||||
|
|
||||||
|
// if (servoForward) {
|
||||||
|
// servo1Pos++;
|
||||||
|
// if (servo1Pos >= SERVO1_END) {
|
||||||
|
// servoForward = false;
|
||||||
|
// }
|
||||||
|
// } else {
|
||||||
|
// servo1Pos--;
|
||||||
|
// if (servo1Pos <= SERVO1_START) {
|
||||||
|
// servoForward = true;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// servo1.write(servo1Pos);
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
|
// // Non Blocking Servos
|
||||||
|
// static unsigned long servoTimer2 = 0;
|
||||||
|
// static int servo2Pos2 = SERVO2_START;
|
||||||
|
// static bool servoForward2 = true;
|
||||||
|
// unsigned long currentTime2 = millis();
|
||||||
|
|
||||||
|
// if(currentTime2 - servoTimer2 >= 20) { // Adjust for speed
|
||||||
|
|
||||||
|
// servoTimer2 = currentTime2;
|
||||||
|
|
||||||
|
// if (servoForward2) {
|
||||||
|
// servo2Pos2++;
|
||||||
|
// if (servo2Pos2 >= SERVO2_END) {
|
||||||
|
// servoForward2 = false;
|
||||||
|
// }
|
||||||
|
// } else {
|
||||||
|
// servo2Pos2--;
|
||||||
|
// if (servo2Pos2 <= SERVO2_START) {
|
||||||
|
// servoForward2 = true;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
// servo2.write(servo2Pos2);
|
||||||
|
|
||||||
|
// }
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//Non Blocking Servos
|
|
||||||
static unsigned long servoTimer = 0;
|
|
||||||
static int servo1Pos = SERVO1_START;
|
|
||||||
static int servo2Pos = SERVO2_START;
|
|
||||||
static bool servoForward = true;
|
|
||||||
unsigned long currentTime = millis();
|
|
||||||
|
|
||||||
if(currentTime - servoTimer >= 20) { // Adjust for speed
|
|
||||||
|
|
||||||
servoTimer = currentTime;
|
|
||||||
|
|
||||||
if (servoForward) {
|
|
||||||
servo1Pos++;
|
|
||||||
if (servo1Pos >= SERVO1_END) {
|
|
||||||
servoForward = false;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
servo1Pos--;
|
|
||||||
if (servo1Pos <= SERVO1_START) {
|
|
||||||
servoForward = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
servo1.write(servo1Pos);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
//Non Blocking Servos
|
|
||||||
static unsigned long servoTimer2 = 0;
|
|
||||||
static int servo2Pos2 = SERVO2_START;
|
|
||||||
static bool servoForward2 = true;
|
|
||||||
unsigned long currentTime2 = millis();
|
|
||||||
|
|
||||||
if(currentTime2 - servoTimer2 >= 20) { // Adjust for speed
|
|
||||||
|
|
||||||
servoTimer2 = currentTime2;
|
|
||||||
|
|
||||||
if (servoForward2) {
|
|
||||||
servo2Pos2++;
|
|
||||||
if (servo2Pos2 >= SERVO2_END) {
|
|
||||||
servoForward2 = false;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
servo2Pos2--;
|
|
||||||
if (servo2Pos2 <= SERVO2_START) {
|
|
||||||
servoForward2 = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
servo2.write(servo2Pos2);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// ESP-NOW callback
|
|
||||||
void OnDataRecv(const esp_now_recv_info *info, const uint8_t *data, int len) {
|
|
||||||
if (len == sizeof(PacketData)) {
|
|
||||||
memcpy(&data, data, sizeof(PacketData));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
44
espnow-car/espnow.h
Normal file
44
espnow-car/espnow.h
Normal file
@ -0,0 +1,44 @@
|
|||||||
|
|
||||||
|
// ESPNOW
|
||||||
|
#include <esp_now.h>
|
||||||
|
#include <WiFi.h>
|
||||||
|
|
||||||
|
// Packet Data Structure
|
||||||
|
struct PacketData {
|
||||||
|
uint8_t X; // Steering (left/right)
|
||||||
|
uint8_t Y; // Throttle (forward/backward)
|
||||||
|
bool B; // Button
|
||||||
|
};
|
||||||
|
|
||||||
|
// Data Received From Remote
|
||||||
|
PacketData data;
|
||||||
|
|
||||||
|
// ESP-NOW callback
|
||||||
|
void OnDataRecv(const esp_now_recv_info *info, const uint8_t *data, int len) {
|
||||||
|
if (len == sizeof(PacketData)) {
|
||||||
|
memcpy(&data, data, sizeof(PacketData));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Function To Start ESPNOW
|
||||||
|
void espnow_begin() {
|
||||||
|
|
||||||
|
// ESP-NOW setup
|
||||||
|
WiFi.mode(WIFI_STA);
|
||||||
|
if (esp_now_init() != ESP_OK) {
|
||||||
|
Serial.println("ESP-NOW init failed");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
delay(100);
|
||||||
|
|
||||||
|
// Print MAC Address
|
||||||
|
Serial.println(WiFi.macAddress());
|
||||||
|
|
||||||
|
delay(2000);
|
||||||
|
|
||||||
|
// When Receive Data Execute OnDataRecv Function
|
||||||
|
esp_now_register_recv_cb(OnDataRecv);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
65
espnow-car/fuzzy.h
Normal file
65
espnow-car/fuzzy.h
Normal file
@ -0,0 +1,65 @@
|
|||||||
|
#ifndef FUZZY_H
|
||||||
|
#define FUZZY_H
|
||||||
|
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
// Rear servo configuration
|
||||||
|
const int REAR_MIN = 52;
|
||||||
|
const int REAR_MAX = 140;
|
||||||
|
const int REAR_STRAIGHT = 95;
|
||||||
|
|
||||||
|
// Fuzzy logic functions
|
||||||
|
float triangularMF(float x, float a, float b, float c) {
|
||||||
|
return std::max(std::min((x - a) / (b - a), (c - x) / (c - b)), 0.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
float calculateCentroid(float* memberships, float* angles, int size) {
|
||||||
|
|
||||||
|
float sumMembership = 0;
|
||||||
|
float sumProduct = 0;
|
||||||
|
for (int i = 0; i < size; i++) {
|
||||||
|
sumMembership += memberships[i];
|
||||||
|
sumProduct += memberships[i] * angles[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
return sumProduct / sumMembership;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
float fuzzyRearSteering(float accelX, float accelY) {
|
||||||
|
|
||||||
|
// Define membership functions for inputs
|
||||||
|
float accelNeg = triangularMF(accelX, -1, -0.5, 0);
|
||||||
|
float accelZero = triangularMF(accelX, -0.5, 0, 0.5);
|
||||||
|
float accelPos = triangularMF(accelX, 0, 0.5, 1);
|
||||||
|
|
||||||
|
float rotSmall = triangularMF(accelY, -1, -0.5, 0);
|
||||||
|
float rotMedium = triangularMF(accelY, -0.5, 0, 0.5);
|
||||||
|
float rotLarge = triangularMF(accelY, 0, 0.5, 1);
|
||||||
|
|
||||||
|
// Define rule base
|
||||||
|
float ruleStrengths[9];
|
||||||
|
ruleStrengths[0] = std::min(accelNeg, rotSmall); // Right
|
||||||
|
ruleStrengths[1] = std::min(accelNeg, rotMedium); // Straight
|
||||||
|
ruleStrengths[2] = std::min(accelNeg, rotLarge); // Left
|
||||||
|
ruleStrengths[3] = std::min(accelZero, rotSmall); // Straight
|
||||||
|
ruleStrengths[4] = std::min(accelZero, rotMedium); // Straight
|
||||||
|
ruleStrengths[5] = std::min(accelZero, rotLarge); // Left
|
||||||
|
ruleStrengths[6] = std::min(accelPos, rotSmall); // Left
|
||||||
|
ruleStrengths[7] = std::min(accelPos, rotMedium); // Straight
|
||||||
|
ruleStrengths[8] = std::min(accelPos, rotLarge); // Right
|
||||||
|
|
||||||
|
// Define output angles (adjusted for rear servo range)
|
||||||
|
float outputAngles[9] = {
|
||||||
|
REAR_MAX, REAR_STRAIGHT, REAR_MIN, // Right, Straight, Left
|
||||||
|
REAR_STRAIGHT, REAR_STRAIGHT, REAR_MIN, // Straight, Straight, Left
|
||||||
|
REAR_MIN, REAR_STRAIGHT, REAR_MAX // Left, Straight, Right
|
||||||
|
};
|
||||||
|
|
||||||
|
// Defuzzification using centroid method
|
||||||
|
return calculateCentroid(ruleStrengths, outputAngles, 9);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
@ -1,3 +1,4 @@
|
|||||||
|
|
||||||
#ifndef L298N_ESP32_h
|
#ifndef L298N_ESP32_h
|
||||||
#define L298N_ESP32_h
|
#define L298N_ESP32_h
|
||||||
|
|
||||||
|
31
espnow-car/motor.h
Normal file
31
espnow-car/motor.h
Normal file
@ -0,0 +1,31 @@
|
|||||||
|
|
||||||
|
// Libraries
|
||||||
|
#include "l298n_esp32.h"
|
||||||
|
|
||||||
|
// Motor 1 Pins
|
||||||
|
#define MOTOR1_PIN1 27
|
||||||
|
#define MOTOR1_PIN2 26
|
||||||
|
#define MOTOR1_ENABLE 14
|
||||||
|
|
||||||
|
// Motor 2 Pins
|
||||||
|
#define MOTOR2_PIN1 25
|
||||||
|
#define MOTOR2_PIN2 33
|
||||||
|
#define MOTOR2_ENABLE 32
|
||||||
|
|
||||||
|
// Motor State
|
||||||
|
enum MotorState {
|
||||||
|
STOPPED,
|
||||||
|
FORWARD_M1,
|
||||||
|
BACKWARD_M1,
|
||||||
|
FORWARD_M2,
|
||||||
|
BACKWARD_M2
|
||||||
|
};
|
||||||
|
|
||||||
|
// Motor Variables
|
||||||
|
MotorState currentMotorState = STOPPED;
|
||||||
|
unsigned long motorStateStartTime = 0;
|
||||||
|
const unsigned long MOTOR_ACTION_DELAY = 5000; // Delay between motor actions
|
||||||
|
|
||||||
|
// Motor Objects
|
||||||
|
L298N_ESP32 motor1(MOTOR1_PIN1, MOTOR1_PIN2, MOTOR1_ENABLE, 0);
|
||||||
|
L298N_ESP32 motor2(MOTOR2_PIN1, MOTOR2_PIN2, MOTOR2_ENABLE, 1);
|
17
espnow-car/servo.h
Normal file
17
espnow-car/servo.h
Normal file
@ -0,0 +1,17 @@
|
|||||||
|
|
||||||
|
// Servo Library
|
||||||
|
#include "Servo_ESP32.h"
|
||||||
|
|
||||||
|
// Servo Pins
|
||||||
|
#define SERVO1_PIN 13
|
||||||
|
#define SERVO2_PIN 12
|
||||||
|
|
||||||
|
// Servo Objects
|
||||||
|
Servo_ESP32 servo1(SERVO1_PIN, 2, 1000, 2000);
|
||||||
|
Servo_ESP32 servo2(SERVO2_PIN, 3, 1000, 2000);
|
||||||
|
|
||||||
|
// Constants
|
||||||
|
const int SERVO1_START = 10;
|
||||||
|
const int SERVO1_END = 170;
|
||||||
|
const int SERVO2_START = 20;
|
||||||
|
const int SERVO2_END = 160;
|
@ -1,74 +1,78 @@
|
|||||||
#ifndef Servo_ESP32_h
|
|
||||||
#define Servo_ESP32_h
|
|
||||||
|
|
||||||
#include <Arduino.h>
|
#ifndef Servo_ESP32_h
|
||||||
|
#define Servo_ESP32_h
|
||||||
|
|
||||||
class Servo_ESP32 {
|
#include <Arduino.h>
|
||||||
private:
|
|
||||||
uint8_t servoPin;
|
|
||||||
uint8_t pwmChannel;
|
|
||||||
int minPulseWidth;
|
|
||||||
int maxPulseWidth;
|
|
||||||
int currentAngle;
|
|
||||||
const int freq = 50; // 50Hz for standard servos
|
|
||||||
const int resolution = 16; // 16-bit resolution for smoother control
|
|
||||||
bool _invert = false;
|
|
||||||
uint8_t _min_pos = 0;
|
|
||||||
uint8_t _max_pos = 180;
|
|
||||||
uint8_t _init_pos = 90;
|
|
||||||
uint8_t _prv_pos = 0;
|
|
||||||
|
|
||||||
bool verify(uint8_t input) {
|
class Servo_ESP32 {
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
uint8_t servoPin;
|
||||||
|
uint8_t pwmChannel;
|
||||||
|
int minPulseWidth;
|
||||||
|
int maxPulseWidth;
|
||||||
|
int currentAngle;
|
||||||
|
const int freq = 50; // 50Hz for standard servos
|
||||||
|
const int resolution = 16; // 16-bit resolution for smoother control
|
||||||
|
bool _invert = false;
|
||||||
|
uint8_t _min_pos = 0;
|
||||||
|
uint8_t _max_pos = 180;
|
||||||
|
uint8_t _init_pos = 90;
|
||||||
|
uint8_t _prv_pos = 0;
|
||||||
|
|
||||||
|
bool verify(uint8_t input) {
|
||||||
if (input != _prv_pos) {
|
if (input != _prv_pos) {
|
||||||
_prv_pos = input;
|
_prv_pos = input;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
Servo_ESP32() {}
|
|
||||||
Servo_ESP32(uint8_t pin, uint8_t channel, int minPulse = 500, int maxPulse = 2500) :
|
|
||||||
servoPin(pin), pwmChannel(channel), minPulseWidth(minPulse), maxPulseWidth(maxPulse), currentAngle(0) {}
|
|
||||||
|
|
||||||
void begin() {
|
Servo_ESP32() {}
|
||||||
|
|
||||||
|
Servo_ESP32(uint8_t pin, uint8_t channel, int minPulse = 500, int maxPulse = 2500) : servoPin(pin), pwmChannel(channel), minPulseWidth(minPulse), maxPulseWidth(maxPulse), currentAngle(0) {}
|
||||||
|
|
||||||
|
void begin() {
|
||||||
ledcAttachChannel(pwmChannel, freq, resolution, servoPin); // Correct order: channel, freq, resolution, pin
|
ledcAttachChannel(pwmChannel, freq, resolution, servoPin); // Correct order: channel, freq, resolution, pin
|
||||||
write(_init_pos);
|
write(_init_pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setPin(uint8_t pin) {
|
void setPin(uint8_t pin) {
|
||||||
servoPin = pin;
|
servoPin = pin;
|
||||||
}
|
}
|
||||||
|
|
||||||
void invert() {
|
void invert() {
|
||||||
_invert = !_invert;
|
_invert = !_invert;
|
||||||
}
|
}
|
||||||
|
|
||||||
void setMin(uint8_t start) {
|
void setMin(uint8_t start) {
|
||||||
_min_pos = start;
|
_min_pos = start;
|
||||||
}
|
}
|
||||||
|
|
||||||
void setMax(uint8_t stop) {
|
void setMax(uint8_t stop) {
|
||||||
_max_pos = stop;
|
_max_pos = stop;
|
||||||
}
|
}
|
||||||
|
|
||||||
void setInit(uint8_t init) {
|
void setInit(uint8_t init) {
|
||||||
_init_pos = init;
|
_init_pos = init;
|
||||||
}
|
}
|
||||||
|
|
||||||
void goMax() {
|
void goMax() {
|
||||||
write(_max_pos);
|
write(_max_pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
void goMin() {
|
void goMin() {
|
||||||
write(_min_pos);
|
write(_min_pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
void goInit() {
|
void goInit() {
|
||||||
write(_init_pos);
|
write(_init_pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
void write(int angle) {
|
void write(int angle) {
|
||||||
if (!verify(angle)) return;
|
if (!verify(angle)) return;
|
||||||
currentAngle = _invert ? 180 - angle : angle;
|
currentAngle = _invert ? 180 - angle : angle;
|
||||||
currentAngle = constrain(currentAngle, 0, 180);
|
currentAngle = constrain(currentAngle, 0, 180);
|
||||||
@ -76,29 +80,30 @@ public:
|
|||||||
minPulseWidth,
|
minPulseWidth,
|
||||||
maxPulseWidth);
|
maxPulseWidth);
|
||||||
ledcWrite(pwmChannel, dutyCycle);
|
ledcWrite(pwmChannel, dutyCycle);
|
||||||
}
|
}
|
||||||
|
|
||||||
int read() {
|
int read() {
|
||||||
return currentAngle;
|
return currentAngle;
|
||||||
}
|
}
|
||||||
|
|
||||||
void move(uint8_t pos) {
|
void move(uint8_t pos) {
|
||||||
pos = constrain(pos, _min_pos, _max_pos);
|
pos = constrain(pos, _min_pos, _max_pos);
|
||||||
write(pos);
|
write(pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
void percent(uint8_t pos) {
|
void percent(uint8_t pos) {
|
||||||
pos = map(pos, 0, 100, _min_pos, _max_pos);
|
pos = map(pos, 0, 100, _min_pos, _max_pos);
|
||||||
write(pos);
|
write(pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
void override(uint8_t pos) {
|
void override(uint8_t pos) {
|
||||||
write(pos);
|
write(pos);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Add these functions to access the private attributes
|
// Add these functions to access the private attributes
|
||||||
uint8_t getMin() const { return _min_pos; }
|
uint8_t getMin() const { return _min_pos; }
|
||||||
uint8_t getMax() const { return _max_pos; }
|
uint8_t getMax() const { return _max_pos; }
|
||||||
};
|
|
||||||
|
|
||||||
#endif
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
@ -5,6 +5,9 @@
|
|||||||
// HEADER FILE
|
// HEADER FILE
|
||||||
#include "header.h"
|
#include "header.h"
|
||||||
|
|
||||||
|
// REPLACE WITH YOUR RECEIVER MAC ADDRESS - 1C:69:20:E9:13:EC
|
||||||
|
// uint8_t receiverMacAddress[] = {0x1C, 0x69, 0x20, 0xE9, 0x13, 0xEC}; // 1C:69:20:E9:13:EC
|
||||||
|
|
||||||
//==========================================================
|
//==========================================================
|
||||||
// SETUP
|
// SETUP
|
||||||
//==========================================================
|
//==========================================================
|
||||||
@ -13,10 +16,41 @@
|
|||||||
void setup() {
|
void setup() {
|
||||||
|
|
||||||
Serial.begin(115200);
|
Serial.begin(115200);
|
||||||
|
|
||||||
|
Serial.println();
|
||||||
|
Serial.println(WiFi.macAddress());
|
||||||
|
WiFi.mode(WIFI_STA);
|
||||||
|
delay(5000);
|
||||||
|
|
||||||
JY.setPin(X_AXIS_PIN, Y_AXIS_PIN, SWITCH_PIN);
|
JY.setPin(X_AXIS_PIN, Y_AXIS_PIN, SWITCH_PIN);
|
||||||
JY.begin();
|
JY.begin();
|
||||||
|
|
||||||
// StartWireless(); // Initialize ESP-NOW
|
// StartWireless(); // Initialize ESP-NOW
|
||||||
|
|
||||||
|
// Init ESP-NOW
|
||||||
|
if(esp_now_init() != ESP_OK) {
|
||||||
|
Serial.println("Error initializing ESP-NOW");
|
||||||
|
return;
|
||||||
|
} else {
|
||||||
|
Serial.println("Succes: Initialized ESP-NOW");
|
||||||
|
}
|
||||||
|
|
||||||
|
esp_now_register_send_cb(OnDataSent);
|
||||||
|
|
||||||
|
// Register peer
|
||||||
|
esp_now_peer_info_t peerInfo;
|
||||||
|
memcpy(peerInfo.peer_addr, receiverMacAddress, 6);
|
||||||
|
peerInfo.channel = 0;
|
||||||
|
peerInfo.encrypt = false;
|
||||||
|
|
||||||
|
// Add peer
|
||||||
|
if(esp_now_add_peer(&peerInfo) != ESP_OK) {
|
||||||
|
Serial.println("Failed to add peer");
|
||||||
|
return;
|
||||||
|
} else {
|
||||||
|
Serial.println("Succes: Added peer");
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//==========================================================
|
//==========================================================
|
||||||
@ -26,15 +60,24 @@
|
|||||||
// LOOP
|
// LOOP
|
||||||
void loop() {
|
void loop() {
|
||||||
|
|
||||||
JD.X = map(average(JY.readX(), 10), 0, 4095, 0, 255); // Map to 0-255
|
JD.X = map(analogRead(32), 0, 4095, 0, 100); //map(average(JY.readX(), 10), 0, 4095, 0, 255); // Map to 0-255
|
||||||
JD.Y = map(average(JY.readY(), 10), 0, 4095, 0, 255); // Map to 0-255
|
JD.Y = map(analogRead(33), 0, 4095, 0, 100); //map(average(JY.readY(), 10), 0, 4095, 0, 255); // Map to 0-255
|
||||||
JD.B = JY.readB();
|
JD.B = digitalRead(34); //JY.readB();
|
||||||
|
|
||||||
Serial.print(JD.X); Serial.print(" ");
|
Serial.print(JD.X); Serial.print(" ");
|
||||||
Serial.print(JD.Y); Serial.print(" ");
|
Serial.print(JD.Y); Serial.print(" ");
|
||||||
Serial.println(!JD.B);
|
Serial.println(!JD.B);
|
||||||
|
|
||||||
// SendData(JD); // Uncomment to send data
|
// SendData(JD); // Uncomment to send data
|
||||||
|
|
||||||
|
esp_err_t result = esp_now_send(receiverMacAddress, (uint8_t *) &JD, sizeof(JD));
|
||||||
|
if (result == ESP_OK) {
|
||||||
|
// Serial.println("Sent with success");
|
||||||
|
} else {
|
||||||
|
// Serial.println("Error sending the data");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
delay(20);
|
delay(20);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -6,9 +6,9 @@
|
|||||||
#include "joystic.h"
|
#include "joystic.h"
|
||||||
|
|
||||||
// JOYSTIC PIN DEFINITION
|
// JOYSTIC PIN DEFINITION
|
||||||
#define X_AXIS_PIN 32 // ADC1_CH4 (Valid)
|
#define X_AXIS_PIN 32 // ADC1_CH4 (Valid)
|
||||||
#define Y_AXIS_PIN 33 // ADC1_CH5 (Valid)
|
#define Y_AXIS_PIN 33 // ADC1_CH5 (Valid)
|
||||||
#define SWITCH_PIN 34 // Changed from 25 (now ADC1_CH6)
|
#define SWITCH_PIN 34 // Changed from 25 (now ADC1_CH6)
|
||||||
|
|
||||||
// DATA STRUCTURE
|
// DATA STRUCTURE
|
||||||
struct JoysticData {
|
struct JoysticData {
|
||||||
@ -36,7 +36,7 @@
|
|||||||
// THE RECEIVER MAC ADDRESS
|
// THE RECEIVER MAC ADDRESS
|
||||||
// 1C:69:20:E9:13:EC
|
// 1C:69:20:E9:13:EC
|
||||||
|
|
||||||
// REPLACE WITH YOUR RECEIVER MAC ADDRESS
|
// REPLACE WITH YOUR RECEIVER MAC ADDRESS - 1C:69:20:E9:13:EC
|
||||||
uint8_t receiverMacAddress[] = {0x1C, 0x69, 0x20, 0xE9, 0x13, 0xEC}; // 1C:69:20:E9:13:EC
|
uint8_t receiverMacAddress[] = {0x1C, 0x69, 0x20, 0xE9, 0x13, 0xEC}; // 1C:69:20:E9:13:EC
|
||||||
|
|
||||||
// CALLBACK WHEN DATA IS SENT
|
// CALLBACK WHEN DATA IS SENT
|
||||||
|
Loading…
x
Reference in New Issue
Block a user