Robotikiyat: Top Parking

#include "BluetoothSerial.h"
#include <ESP32Servo.h>

BluetoothSerial SerialBT;
Servo gripperServo;
Servo liftServo;

const int enA = 13;
const int in1 = 14;
const int in2 = 12;
const int enB = 25;
const int in3 = 27;
const int in4 = 26;

const int sensorPins[] = { 18, 19, 21, 22, 23 };
int s[5];

const int servoPin1 = 2;
const int servoPin2 = 4;

const int trigPin2 = 5;
const int echoPin2 = 17;
const int trigPin = 16;
const int echoPin = 35;

const int ledGreen = 32;
const int ledRed = 33;

enum RobotMode { MANUAL, AUTO };
RobotMode currentMode = MANUAL;

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

  pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  pinMode(ledGreen, OUTPUT);
  pinMode(ledRed, OUTPUT);

  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

  gripperServo.setPeriodHertz(50);
  gripperServo.attach(servoPin1, 500, 2400);
  gripperServo.write(180);

  liftServo.setPeriodHertz(50);
  liftServo.attach(servoPin2, 500, 2400);
  liftServo.write(0);

  SerialBT.begin("MRNC_Yahya_Teleguide");
  Serial.println("Teleguide Mode Active. Connect your phone via Bluetooth.");
}

int df;
int dl;

void loop() {
  if (SerialBT.available()) {
    char cmd = SerialBT.read();
    Serial.print("Command: ");
    Serial.println(cmd);

    if (cmd == 'w') {
      currentMode = MANUAL;
      move(0, 0);
      Serial.println("Mode: TELEGUIDE");
    } 
    else if (cmd == 'W') {
      currentMode = AUTO;
      move(0, 0);
      Serial.println("Mode: EVITEUR/SUIVEUR");
    } 
    else if (currentMode == MANUAL) {
      digitalWrite(ledGreen, HIGH);
      digitalWrite(ledRed, LOW);
      teleguide(cmd);
    }
  }

  if (currentMode == AUTO) {
    digitalWrite(ledGreen, LOW);
    digitalWrite(ledRed, HIGH);
    autoo();
  }
}

void autoo(){
  readSensors(); 
  dl = gdl(); 
  df = gdf();

  if(dl < 20 && s[0] == 1 && s[1] == 1 && s[2] == 1 && s[3] == 1 && s[4] == 1){
    eviteur();
  }else{
    suiveur();
  }
}

void move(int L, int R) {
  digitalWrite(in1, R >= 0 ? HIGH : LOW);
  digitalWrite(in2, R >= 0 ? LOW : HIGH);
  analogWrite(enA, abs(R));

  digitalWrite(in3, L >= 0 ? HIGH : LOW);
  digitalWrite(in4, L >= 0 ? LOW : HIGH);
  analogWrite(enB, abs(L));
}

long gdf() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 20000);
  return (duration == 0) ? 999 : (duration * 0.034 / 2);
}

long gdl() {
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  long duration = pulseIn(echoPin2, HIGH, 20000);
  return (duration == 0) ? 999 : (duration * 0.034 / 2);
}

void readSensors() {
  for (int i = 0; i < 5; i++) s[i] = digitalRead(sensorPins[i]);
}

void suiveur() {
  readSensors();

  if (s[2] == 0 && s[1] == 1 && s[3] == 1) {
    move(220, 220);
  } else if (s[1] == 0) {
    move(150, 220);
  } else if (s[0] == 0 && s[1] == 0) {
    move(90, 220);
  } else if (s[0] == 0 && s[1] == 0 && s[2] == 0) {
    move(0, 220);
  } else if (s[0] == 0) {
    move(-120, 220);
  } else if (s[3] == 0) {
    move(220, 150);
  } else if (s[3] == 0 && s[4] == 0) {
    move(220, 90);
  } else if (s[3] == 0 && s[4] == 0 && s[2] == 0) {
    move(220, 0);
  } else if (s[4] == 0) {
    move(220, -120);
  }
}

void teleguide(char cmd) {
  switch (cmd) {
    case 'F': move(255, 255); break;
    case 'B': move(-255, -255); break;
    case 'L': move(255, -255); break;
    case 'R': move(-255, 255); break;
    case 'G': move(255, 0); break;
    case 'I': move(0, 255); break;
    case 'S': move(0, 0); break;
    case 'v':
      gripperServo.write(180);
      Serial.println("Gripper: 0 deg");
      delay(300);
      liftServo.write(0);
      Serial.println("Lift: 40 deg");
      break;
    case 'V':
      gripperServo.write(0);
      Serial.println("Gripper: 180 deg");
      delay(300);
      liftServo.write(180);
      Serial.println("Lift: 180 deg");
      break;
  }
}

void eviteur() {
  df = gdf();
  dl = gdl();

  if (df < 15) {
    if (dl > 30) {
      move(200, -255);
    } else {
      move(-255, 200);
      delay(200);
    }
  } else {
    if (dl > 12) {
      move(200, 0);
    } else if (dl < 10) {
      move(0, 200);
    } else {
      move(200, 200);
    }
  }
}