Drone-FabAcademy-2025/src/controller/drone controller/src/main.cpp

#include <Arduino.h>
#include <WiFi.h>
#include <esp_now.h>

// declarations
int normalizePot(int pin, int minValue);
int mapPot(int normalizedValue);
int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin);
void espNow();
void readAllMultiPlexer();
void espNowLoop();

const int MAXPWMVALUE = 1000;
const int MINPWMVALUE = 2000;
const uint8_t broadcastAddress[] = {0x8C, 0xBF, 0xEA, 0xCC, 0x8E, 0x5C};

// Define the struct that will be sent
typedef struct struct_message
{
  int PWMCH1;
  int PWMCH2;
  int PWMCH3;
  int PWMCH4;
} struct_message;
struct_message JoystickData; // declare the struct as JoystickData

esp_now_peer_info_t peerInfo; // create a class object of the ESPNow class

void setup()
{
  espNow();
  Serial.begin(9600);

  pinMode(D3, OUTPUT);
  pinMode(D6, OUTPUT);
  pinMode(D7, OUTPUT);
  pinMode(D8, OUTPUT);
  pinMode(D9, OUTPUT);
  pinMode(D0, INPUT);
}

void loop()
{
  // Set values to send
  JoystickData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0)); //Right joystick Y
  JoystickData.PWMCH2 = mapPot(analogReadMultiPlexer(1, 0, 0, 0, A0)); // Right joystick X
  JoystickData.PWMCH3 = mapPot(analogReadMultiPlexer(0, 0, 0, 1, A0)); // left joystick Y
  JoystickData.PWMCH4 = mapPot(analogReadMultiPlexer(1, 0, 0, 1, A0)); // left joystick X
  Serial.print("PWMCH1: ");
  Serial.println(JoystickData.PWMCH1);
  Serial.print(" PWMCH2: ");
  Serial.println(JoystickData.PWMCH2);
  Serial.print(" PWMCH3: ");
  Serial.println(JoystickData.PWMCH3);  
  Serial.print(" PWMCH4: ");
  Serial.println(JoystickData.PWMCH4);

  espNowLoop();
  delay(10); // delay to avoid hammering the radio and to save power/heat
}


int mapPot(int normalizedValue)
{
  return map(normalizedValue, 400, 2500, MINPWMVALUE, MAXPWMVALUE); // map the normalized value to the PWM range
}

//legacy stuff for original potsliders
int normalizePot(int pin, int minValue) // normalize the pot value to a range of 80 to 4095 instead of 0 to 4095 because the potmeter is at lower values not accurate
{
  int pot = analogRead(pin);

  if (pot <= minValue)
  {
    return 80;
  }
  else
  {
    return pot;
  }
}

int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin)
{
  digitalWrite(D3, LOW);
  digitalWrite(D6, addressA);
  digitalWrite(D7, addressB);
  digitalWrite(D9, addressC);
  digitalWrite(D8, addressD);
  return analogRead(pin);
}

void espNow()
{ // Set device as a Wi-Fi Station
  WiFi.mode(WIFI_STA);
  // Init ESP-NOW
  if (esp_now_init() != ESP_OK)
  {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  // Register peer
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  // Add peer
  if (esp_now_add_peer(&peerInfo) != ESP_OK)
  {
    Serial.println("Failed to add peer");
    return;
  }
}

void readAllMultiPlexer()
{
  // we counting in binary
  Serial.print("0,0,0,0: ");
  Serial.println(analogReadMultiPlexer(0, 0, 0, 0, A0));
  Serial.print("1,0,0,0: ");
  Serial.println(analogReadMultiPlexer(1, 0, 0, 0, A0));
  Serial.print("0,1,0,0: ");
  Serial.println(analogReadMultiPlexer(0, 1, 0, 0, A0));
  Serial.print("1,1,0,0: ");
  Serial.println(analogReadMultiPlexer(1, 1, 0, 0, A0));
  Serial.print("0,0,1,0: ");
  Serial.println(analogReadMultiPlexer(0, 0, 1, 0, A0));
  Serial.print("1,0,1,0: ");
  Serial.println(analogReadMultiPlexer(1, 0, 1, 0, A0));
  Serial.print("0,1,1,0: ");
  Serial.println(analogReadMultiPlexer(0, 1, 1, 0, A0));
  Serial.print("1,1,1,0: ");
  Serial.println(analogReadMultiPlexer(1, 1, 1, 0, A0));
  Serial.print("0,0,0,1: ");
  Serial.println(analogReadMultiPlexer(0, 0, 0, 1, A0));
  Serial.print("1,0,0,1: ");
  Serial.println(analogReadMultiPlexer(1, 0, 0, 1, A0));
  Serial.print("0,1,0,1: ");
  Serial.println(analogReadMultiPlexer(0, 1, 0, 1, A0));
  Serial.print("1,1,0,1: ");
  Serial.println(analogReadMultiPlexer(1, 1, 0, 1, A0));
  Serial.print("0,0,1,1: ");
  Serial.println(analogReadMultiPlexer(0, 0, 1, 1, A0));
  Serial.print("1,0,1,1: ");
  Serial.println(analogReadMultiPlexer(1, 0, 1, 1, A0));
  Serial.print("0,1,1,1: ");
  Serial.println(analogReadMultiPlexer(0, 1, 1, 1, A0));
  Serial.print("1,1,1,1: ");
  Serial.println(analogReadMultiPlexer(1, 1, 1, 1, A0));
}

void espNowLoop(){
  // Send message via ESP-NOW
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&JoystickData, sizeof(JoystickData));
  if (result == ESP_OK)
  {
    // Serial.println("Sent with success");
  }
  else
  {
    // Serial.println("Error sending the data");
  }
}