//The simulation is for 3-dof robotic arm and car.
//Servo motor was used to represent tt gear motors.
//The analog sticks in the project have bluetooth capability(dualshock ps3), here they are connected to the system with cables.                                                      
//In the project, the power is supplied by battery and distributed to the system by converter, here 5v pin of ESP 32 was used to supply power. 
#include <ESP32Servo.h>

// Servo pinleri
#define SHOULDER_SERVO_PIN 15
#define ELBOW_SERVO_PIN 16
#define BASE_SERVO_PIN 17
#define GRIPPER_SERVO_PIN 18
#define FRONT_RIGHT_WHEEL_PIN 19
#define FRONT_LEFT_WHEEL_PIN 21
#define REAR_RIGHT_WHEEL_PIN 22
#define REAR_LEFT_WHEEL_PIN 23

// Joystick pinleri
#define JOYSTICK_1_X_PIN 34  // Araba tekerlekleri için yatay eksen
#define JOYSTICK_1_Y_PIN 35  // Araba tekerlekleri için dikey eksen
#define JOYSTICK_2_X_PIN 32  // Robot kolu için yatay eksen
#define JOYSTICK_2_Y_PIN 33  // Robot kolu için dikey eksen

// Servo motorlar
Servo shoulderServo;
Servo elbowServo;
Servo baseServo;
Servo gripperServo;
Servo frontRightWheelServo;
Servo frontLeftWheelServo;
Servo rearRightWheelServo;
Servo rearLeftWheelServo;

void setup() {
  // Servo motorları başlat
  shoulderServo.attach(SHOULDER_SERVO_PIN);
  elbowServo.attach(ELBOW_SERVO_PIN);
  baseServo.attach(BASE_SERVO_PIN);
  gripperServo.attach(GRIPPER_SERVO_PIN);
  frontRightWheelServo.attach(FRONT_RIGHT_WHEEL_PIN);
  frontLeftWheelServo.attach(FRONT_LEFT_WHEEL_PIN);
  rearRightWheelServo.attach(REAR_RIGHT_WHEEL_PIN);
  rearLeftWheelServo.attach(REAR_LEFT_WHEEL_PIN);

  // Seri haberleşmeyi başlat
  Serial.begin(115200);
}

void loop() {
  // Joystick 1 (Araba Tekerlekleri) hareketlerini oku
  int joystick1X = analogRead(JOYSTICK_1_X_PIN);  // Yatay hareket (X)
  int joystick1Y = analogRead(JOYSTICK_1_Y_PIN);  // Dikey hareket (Y)

  // Joystick 2 (Robot Kol) hareketlerini oku
  int joystick2X = analogRead(JOYSTICK_2_X_PIN);  // Yatay hareket (X)
  int joystick2Y = analogRead(JOYSTICK_2_Y_PIN);  // Dikey hareket (Y)

  // Servo motorlarını kontrol et
  shoulderServo.write(map(joystick2Y, 0, 1023, 0, 180));  // Robot kolu omuz hareketi
  elbowServo.write(map(joystick2X, 0, 1023, 0, 180));    // Robot kolu dirsek hareketi
  baseServo.write(map(joystick1Y, 0, 1023, 0, 180));     // Araba tekerlekleri ön/arka hareketi
  gripperServo.write(map(joystick1X, 0, 1023, 0, 180));  // Araba tekerlekleri sağ/sol hareketi

  frontRightWheelServo.write(map(joystick1Y, 0, 1023, 0, 180));  // Sağ ön tekerlek
  frontLeftWheelServo.write(map(joystick1X, 0, 1023, 0, 180));   // Sol ön tekerlek
  rearRightWheelServo.write(map(joystick1Y, 0, 1023, 0, 180));   // Sağ arka tekerlek
  rearLeftWheelServo.write(map(joystick1X, 0, 1023, 0, 180));    // Sol arka tekerlek

  // Seri monitörde joystick değerlerini yazdır
  Serial.print("Joystick 1 X: ");
  Serial.print(joystick1X);
  Serial.print("\tJoystick 1 Y: ");
  Serial.println(joystick1Y);

  Serial.print("Joystick 2 X: ");
  Serial.print(joystick2X);
  Serial.print("\tJoystick 2 Y: ");
  Serial.println(joystick2Y);

  delay(20);  // Küçük bir gecikme
}