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

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

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


// 定义伺服电机通道
#define SERVO_CHANNEL_1 0
#define SERVO_CHANNEL_2 1
#define SERVO_CHANNEL_3 2
#define SERVO_CHANNEL_4 3
#define SERVO_CHANNEL_5 4


int SERVO1;
int SERVO2;
int SERVO3;
int SERVO4;
int SERVO5;
int TURN=1;
int a=SERVO_CENTER;
int b=SERVO_CENTER;
int c=SERVO_CENTER;
int d=SERVO_CENTER;
int e=600;

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

// Arduino pin numbers
const int SW_1 = 7; // digital pin connected to switch output
const int X_1 = A0; // analog pin connected to X output
const int Y_1 = A1; // analog pin connected to Y output
const int SW_2 = 6; // digital pin connected to switch output
const int X_2 = A2; // analog pin connected to X output
const int Y_2 = A3; // analog pin connected to Y output


int a_1=0;
int b_1=0;
int digitalSW1=0;
int a_2=0;
int b_2=0;
int digitalSW2=0;

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

  pinMode(SW_1, INPUT);
  digitalWrite(SW_1, HIGH);
  pinMode(SW_2, INPUT);
  digitalWrite(SW_2, HIGH);

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

  // 将所有伺服电机设置到90度位置
  pwm.setPWM(SERVO_CHANNEL_1, 0, SERVO_CENTER);
  pwm.setPWM(SERVO_CHANNEL_2, 0, SERVO_CENTER);
  pwm.setPWM(SERVO_CHANNEL_3, 0, SERVO_CENTER);
  pwm.setPWM(SERVO_CHANNEL_4, 0, SERVO_CENTER);
  pwm.setPWM(SERVO_CHANNEL_5, 0, SERVO_CENTER);

}

void loop() {
  a_1=analogRead(X_1);
  b_1=analogRead(Y_1);
  digitalSW1=!digitalRead(SW_1);
  a_2=analogRead(X_2);
  b_2=analogRead(Y_2);
  digitalSW2=digitalRead(SW_2);

  rebuild();
  motor();

  Serial.print("a");
  Serial.print(a);
  Serial.print(" ; ");
  Serial.print("b");
  Serial.print(b);
  Serial.print(" ; ");
  Serial.print("c");
  Serial.print(c);
  Serial.print(" ; ");
  Serial.print("d");
  Serial.print(d);
  Serial.print(" ; ");
  Serial.print("e");
  Serial.println(e);
 



 


  
}

void motor(){
  SERVO1=a;
  SERVO2=b;
  SERVO3=c;
  SERVO4=d;
  SERVO5=e;
    // 将所有伺服电机设置到90度位置
  pwm.setPWM(SERVO_CHANNEL_1, 0, SERVO1);
  pwm.setPWM(SERVO_CHANNEL_2, 0, SERVO2);
  pwm.setPWM(SERVO_CHANNEL_3, 0, SERVO3);
  pwm.setPWM(SERVO_CHANNEL_4, 0, SERVO4);
  pwm.setPWM(SERVO_CHANNEL_5, 0, SERVO5);
}

void rebuild(){
  //tern left
  if(a_1>1000&&b_1>400&&b_1<600){
    a=a+20;
    a=min(a,480);


  }
  //turn right
  if(a_1<100&&b_1>400&&b_1<600){
    a=a-20;
    a=max(260,a);

  }
  //1st big arm turn down
  if(b_1<100&&a_1>500&&a_1<600){
    c=c+20;
    c=min(475,c);


  }
  //1st big arm turn up
  if(b_1>1000&&a_1>500&&a_1<600){
    c=c-20;
    c=max(c,195);

  }
  //2nd forearm turn up
  if(b_2<100&&a_1>500&&a_1<600){
    b=b+20;
    b=min(480,b);


  }
  //2nd forearm turn down
  if(b_2>1000&&a_2>500&&a_2<600){
    b=b-20;
    b=max(b,170);

  }
  if(a_2>1000&&b_2>400&&b_2<600){
    d=d+20;
    d=min(d,480);


  }
  //turn right
  if(a_2<100&&b_2>400&&b_2<600){
    d=d-20;
    d=max(260,d);

  }


  e=600-375*digitalSW1;
  

}