// The servo motor don't work
// https://forum.arduino.cc/t/the-servo-motor-dont-work/1325342


// #include <Wire.h>
// #include <Adafruit_PWMServoDriver.h>

#include <Servo.h>

// 定义PCA9685的地址，默认为0x40
// #define PCA9685_ADDRESS 0x40

// 定义伺服电机脉冲宽度范围
#define SERVOMIN  400   // 最小脉冲宽度计数（对应0度）
#define SERVOMAX  2400  // 最大脉冲宽度计数（对应180度）
#define SERVO_CENTER ((SERVOMAX - SERVOMIN) / 2 + SERVOMIN) // 中心位置脉冲宽度计数（对应90度）


// 定义伺服电机通道
#define SERVO_1_Pin 2
#define SERVO_2_Pin 3
#define SERVO_3_Pin 4
#define SERVO_4_Pin 5
#define SERVO_5_Pin 8

Servo servo_1;
Servo servo_2;
Servo servo_3;
Servo servo_4;
Servo servo_5;

int servo_1_Pos;
int servo_2_Pos;
int servo_3_Pos;
int servo_4_Pos;
int servo_5_Pos;

int TURN = 1;

int a = SERVO_CENTER;
int b = SERVO_CENTER;
int c = SERVO_CENTER;
int d = SERVO_CENTER;
int e = SERVOMAX;

// 初始化PCA9685对象
// Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// Arduino pin numbers
const int joystick_1_Sel_Pin =  7; // digital pin connected to switch output
const int joystick_1_X_Pin   = A0; // analog pin connected to X output
const int joystick_1_Y_Pin   = A1; // analog pin connected to Y output
const int joystick_2_Sel_Pin =  6; // digital pin connected to switch output
const int joystick_2_X_Pin   = A2; // analog pin connected to X output
const int joystick_2_Y_Pin   = A3; // analog pin connected to Y output


int joystick_1_X = 0;
int joystick_1_Y = 0;
int digitalSW1 = 0;
int joystick_2_X = 0;
int joystick_2_Y = 0;
int digitalSW2 = 0;

void setup() {
  // 初始化PCA9685
  // pwm.begin();
  Serial.begin(115200);

  pinMode(joystick_1_Sel_Pin, INPUT_PULLUP);
  pinMode(joystick_2_Sel_Pin, INPUT_PULLUP);

  // pwm.setOscillatorFrequency(27000000);
  // pwm.setPWMFreq(20); // 设置PWM频率为50Hz

  // 将所有伺服电机设置到90度位置
  servo_1.attach(SERVO_1_Pin);
  servo_2.attach(SERVO_2_Pin);
  servo_3.attach(SERVO_3_Pin);
  servo_4.attach(SERVO_4_Pin);
  servo_5.attach(SERVO_5_Pin);
  motor();
}

void loop() {
  joystick_1_X =  analogRead(joystick_1_X_Pin);
  joystick_1_Y =  analogRead(joystick_1_Y_Pin);
  digitalSW1   = !digitalRead(joystick_1_Sel_Pin);
  joystick_2_X =  analogRead(joystick_2_X_Pin);
  joystick_2_Y =  analogRead(joystick_2_Y_Pin);
  digitalSW2   = digitalRead(joystick_2_Sel_Pin);

  rebuild();
  motor();

  Serial.print("Sw1: ");
  Serial.print(digitalSW1);
  Serial.print("; ");
  Serial.print("a: ");
  Serial.print(a);
  Serial.print("; b: ");
  Serial.print(b);
  Serial.print("; c: ");
  Serial.print(c);
  Serial.print("; d: ");
  Serial.print(d);
  Serial.print("; e: ");
  Serial.println(e);
}

void motor() {
  servo_1_Pos = a;
  servo_2_Pos = b;
  servo_3_Pos = c;
  servo_4_Pos = d;
  servo_5_Pos = e;

  // 将所有伺服电机设置到90度位置
  servo_1.writeMicroseconds(servo_1_Pos);
  servo_2.writeMicroseconds(servo_2_Pos);
  servo_3.writeMicroseconds(servo_3_Pos);
  servo_4.writeMicroseconds(servo_4_Pos);
  servo_5.writeMicroseconds(servo_5_Pos);
}

void rebuild() {
  //tern left
  if (joystick_1_X > 1000 && joystick_1_Y > 400 && joystick_1_Y < 600) {
    a = a + 20;
    a = min(a, SERVOMAX);
  }

  //turn right
  if (joystick_1_X < 100 && joystick_1_Y > 400 && joystick_1_Y < 600) {
    a = a - 20;
    a = max(SERVOMIN, a);
  }

  //1st big arm turn down
  if (joystick_1_Y < 100 && joystick_1_X > 400 && joystick_1_X < 600) {
    c = c + 20;
    c = min(SERVOMAX, c);
  }

  //1st big arm turn up
  if (joystick_1_Y > 1000 && joystick_1_X > 400 && joystick_1_X < 600) {
    c = c - 20;
    c = max(c, SERVOMIN);
  }

  //2nd forearm turn up
  if (joystick_2_Y < 100 && joystick_2_X > 400 && joystick_2_X < 600) {
    b = b + 20;
    b = min(SERVOMAX, b);
  }

  //2nd forearm turn down
  if (joystick_2_Y > 1000 && joystick_2_X > 400 && joystick_2_X < 600) {
    b = b - 20;
    b = max(b, SERVOMIN);
  }

  if (joystick_2_X > 1000 && joystick_2_Y > 400 && joystick_2_Y < 600) {
    d = d + 20;
    d = min(d, SERVOMAX);
  }

  //turn right
  if (joystick_2_X < 100 && joystick_2_Y > 400 && joystick_2_Y < 600) {
    d = d - 20;
    d = max(SERVOMIN, d);
  }

  e = SERVOMAX - (SERVOMAX - SERVOMIN) * digitalSW1;

}