esp32_code

#include <WiFi.h>
#include <WiFiUdp.h>

const char* ssid = "change me";
const char* password = "change me";

WiFiUDP udp;
unsigned int localUdpPort = 1234;  // Local port to listen for UDP messages

// Motor pins connected to L298N motor driver
const int motorPin1 = 12;  // Motor 1 input pin 1
const int motorPin2 = 13;  // Motor 1 input pin 2
const int motorPin3 = 26;  // Motor 2 input pin 1
const int motorPin4 = 27;  // Motor 2 input pin 2
const int enAPin = 14;     // ENA pin (controls speed of motor 1)
const int enBPin = 25;     // ENB pin (controls speed of motor 2)

char packetBuffer[256];  // Buffer to hold incoming UDP packets

void setup()
{
  Serial.begin(115200);

  // Connect to Wi-Fi
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED)
  {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }

  // Start UDP
  udp.begin(localUdpPort);
  Serial.println("UDP server started");

  // Initialize motor pins as output
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorPin3, OUTPUT);
  pinMode(motorPin4, OUTPUT);
  pinMode(enAPin, OUTPUT);
  pinMode(enBPin, OUTPUT);
}

void driveMotors(float linear, float angular)
{
  // Calculate left and right motor speeds based on linear and angular velocities
  float leftSpeed = linear + angular;
  float rightSpeed = linear - angular;

  // Map speed values from -100 to 100 to the range of 0 to 255
  int leftSpeedMapped = map(leftSpeed, -100, 100, -255, 255);
  int rightSpeedMapped = map(rightSpeed, -100, 100, -255, 255);

  // Set motor directions based on the calculated speeds
  digitalWrite(motorPin1, leftSpeed > 0 ? HIGH : LOW);
  digitalWrite(motorPin2, leftSpeed < 0 ? HIGH : LOW);
  digitalWrite(motorPin3, rightSpeed > 0 ? HIGH : LOW);
  digitalWrite(motorPin4, rightSpeed < 0 ? HIGH : LOW);

  // Set motor speeds using analogWrite
  analogWrite(enAPin, abs(leftSpeedMapped));
  analogWrite(enBPin, abs(rightSpeedMapped));
}

void loop()
{
  // Check if a packet has arrived
  int packetSize = udp.parsePacket();
  if (packetSize)
  {
    // Read the packet into the buffer
    int bytesRead = udp.read(packetBuffer, sizeof(packetBuffer) - 1);
    if (bytesRead > 0)
    {
      // Null-terminate the received message
      packetBuffer[bytesRead] = '\0';

      // Parse the message and extract linear and angular velocities
      float linear, angular;
      sscanf(packetBuffer, "linear.x: %f, angular.z: %f", &linear, &angular);

      // Drive the motors based on the received velocities
      driveMotors(linear, angular);

      // Print the received message and velocities
      Serial.print("Received message: ");
      Serial.println(packetBuffer);
      Serial.print("Linear velocity: ");
      Serial.println(linear);
      Serial.print("Angular velocity: ");
      Serial.println(angular);
    }
  }
}