Manually Controlled Trash Collecting Bot

#define BLYNK_TEMPLATE_ID "TMPL309wN1vU5"
#define BLYNK_TEMPLATE_NAME "TCB"
#define BLYNK_AUTH_TOKEN "A9pYeQZ2kYm3TK77dx80l2D-gc-BL5uo"

#define BLYNK_PRINT Serial

#include <WiFi.h>
#include <BlynkSimpleEsp32.h>
#include <ESP32Servo.h>

// Wi-Fi Credentials
char ssid[] = "YK";
char pass[] = "abcd1234";

// --- Motor Pin Assignments ---
#define RIGHT_RPWM 19
#define RIGHT_LPWM 21
#define LEFT_RPWM 5
#define LEFT_LPWM 18

// --- Servo Constants ---
#define CLOCKWISE 1300
#define COUNTERCLOCKWISE 1700
#define SERVO_STOP 1500
#define SERVO_DELAY 400

// --- Servo Pins ---
#define SERVO_A_PIN 13  // Arm Servo A
#define SERVO_B_PIN 12  // Arm Servo B
#define SERVO_C_PIN 27  // myServo - Arm Position
#define DUSTBIN_SERVO_PIN 14 // Dustbin Servo

// --- Servo Objects ---
Servo servoA;
Servo servoB;
Servo myServo;
Servo dustbinServo;

// --- Motor Control Variables ---
int x = 0, y = 0;
int rawSpeed = 512;
int mappedSpeed = 128;

void setup() {
  Serial.begin(115200);
  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);

  // --- Motor Pins ---
  pinMode(RIGHT_RPWM, OUTPUT);
  pinMode(RIGHT_LPWM, OUTPUT);
  pinMode(LEFT_RPWM, OUTPUT);
  pinMode(LEFT_LPWM, OUTPUT);

  // --- Servo Setup ---
  servoA.attach(SERVO_A_PIN);
  servoB.attach(SERVO_B_PIN);
  myServo.attach(SERVO_C_PIN);
  dustbinServo.attach(DUSTBIN_SERVO_PIN);

  // --- Initialize Servos ---
  servoA.writeMicroseconds(SERVO_STOP);
  servoB.writeMicroseconds(SERVO_STOP);
  myServo.write(90);
  dustbinServo.writeMicroseconds(SERVO_STOP);
}

// --- Joystick X (Forward/Backward) ---
BLYNK_WRITE(V0) {
  x = param.asInt();
}

// --- Joystick Y (Left/Right) ---
BLYNK_WRITE(V1) {
  y = param.asInt();
}

// --- Speed Slider ---
BLYNK_WRITE(V2) {
  rawSpeed = param.asInt();
  mappedSpeed = map(rawSpeed, 0, 1023, 0, 255);
}

// --- Arm Grab ---
BLYNK_WRITE(V3) {
  if (param.asInt() == 1) {
    Serial.println("Grabbing...");
    servoA.writeMicroseconds(CLOCKWISE);
    servoB.writeMicroseconds(COUNTERCLOCKWISE);
    delay(SERVO_DELAY);
    servoA.writeMicroseconds(SERVO_STOP);
    servoB.writeMicroseconds(SERVO_STOP);
  }
}

// --- Arm Release ---
BLYNK_WRITE(V4) {
  if (param.asInt() == 1) {
    Serial.println("Releasing...");
    servoA.writeMicroseconds(COUNTERCLOCKWISE);
    servoB.writeMicroseconds(CLOCKWISE);
    delay(SERVO_DELAY);
    servoA.writeMicroseconds(SERVO_STOP);
    servoB.writeMicroseconds(SERVO_STOP);
  }
}

// --- Arm Position Control ---
BLYNK_WRITE(V5) {
  if (param.asInt() == 1) {
    myServo.write(180);
    Serial.println("Arm → Collect (180°)");
  }
}

BLYNK_WRITE(V6) {
  if (param.asInt() == 1) {
    myServo.write(120);
    Serial.println("Arm → Check (120°)");
  }
}

BLYNK_WRITE(V8) {
  if (param.asInt() == 1) {
    myServo.write(60);
    Serial.println("Arm → Dump (60°)");
  }
}

BLYNK_WRITE(V9) {
  if (param.asInt() == 1) {
    myServo.write(90);
    Serial.println("Arm → Initial (90°)");
  }
}

// --- Dustbin Servo (Dump Open/Close) ---
BLYNK_WRITE(V7) {
  if (param.asInt() == 1) {
    Serial.println("Dustbin Dumping...");
    dustbinServo.writeMicroseconds(1300); // Open
    delay(1500);
    dustbinServo.writeMicroseconds(SERVO_STOP); // Stop
  } else {
    dustbinServo.writeMicroseconds(SERVO_STOP); // Ensure Stop
  }
}

void loop() {
  Blynk.run();

  // Motor Movement Handling
  if (x > 2) {
    // Forward
    analogWrite(RIGHT_RPWM, mappedSpeed);
    analogWrite(RIGHT_LPWM, 0);
    analogWrite(LEFT_RPWM, mappedSpeed);
    analogWrite(LEFT_LPWM, 0);
  } else if (x < -2) {
    // Backward
    analogWrite(RIGHT_RPWM, 0);
    analogWrite(RIGHT_LPWM, mappedSpeed);
    analogWrite(LEFT_RPWM, 0);
    analogWrite(LEFT_LPWM, mappedSpeed);
  } else if (y > 2) {
    // Right Turn
    analogWrite(RIGHT_RPWM, 0);
    analogWrite(RIGHT_LPWM, mappedSpeed);
    analogWrite(LEFT_RPWM, mappedSpeed);
    analogWrite(LEFT_LPWM, 0);
  } else if (y < -2) {
    // Left Turn
    analogWrite(RIGHT_RPWM, mappedSpeed);
    analogWrite(RIGHT_LPWM, 0);
    analogWrite(LEFT_RPWM, 0);
    analogWrite(LEFT_LPWM, mappedSpeed);
  } else {
    // Stop
    analogWrite(RIGHT_RPWM, 0);
    analogWrite(RIGHT_LPWM, 0);
    analogWrite(LEFT_RPWM, 0);
    analogWrite(LEFT_LPWM, 0);
  }
}