用joystick控制2自由度舵机运台2

#include <Servo.h>

#define SERVO1_DPIN 8
#define SERVO2_DPIN 9
#define JOYSTICK_KEY_DPIN 7
#define JOYSTICK_X_APIN A0
#define JOYSTICK_Y_APIN A1


Servo servo1,servo2;
int oriX,oriY;
int minX,maxX,minY,maxY;
int stepX, stepY;    // servo motor increment in degrees
int thresholdX, thresholdY;
bool servo_on;


void setup()
{
    servo_on=true;
    stepX=1;
    stepY=1;
    oriX = 0;
    oriY = 0;
    thresholdX=10;
    thresholdY=10;

    Serial.begin(9600);
    pinMode(JOYSTICK_KEY_DPIN, INPUT);
    servo1.attach(SERVO1_DPIN);
    servo2.attach(SERVO2_DPIN);
    servo1.write(90);
    servo2.write(90);
}

void loop()
{
      int X,Y;
      float r;
      int deg;
      // ------------------------------------------
      // check joystick key
      // ------------------------------------------
      int key_down = digitalRead(JOYSTICK_KEY_DPIN);
      if(key_down == 0)
      {
          delay(100);
          key_down = digitalRead(JOYSTICK_KEY_DPIN);
          if(key_down==0){
              servo_on=!servo_on;
              Serial.println("Joystick KEY pressed");
          }
      }

     // ------------------------------------------
     // check joystick X value
     // ------------------------------------------
     int sensorValue = analogRead(JOYSTICK_X_APIN);
     if(oriX == 0)
     {
         oriX = sensorValue;
         minX=0-oriX;
         maxX=1023-oriX;
         Serial.print("oriX = ");
         Serial.println(oriX);
     }

     X=sensorValue - oriX;
     Serial.print("X = ");
     Serial.println(X);

     if(servo_on){
         deg=servo1.read();
         if(X>thresholdX){
             r=(float)X/(float)maxX;
             servo1.write(deg+stepX);
             delay(15);
         }else if(X<-thresholdX){
             r=(float)X/(float)minX;
             servo1.write(deg-stepX);
             delay(15);
         }
     }

     // ------------------------------------------
     // check joystick Y value
     // ------------------------------------------
     sensorValue = analogRead(JOYSTICK_Y_APIN);
     if(oriY == 0)
     {
         oriY = sensorValue;
         minY=0-oriY;
         maxY=1023-oriY;
         Serial.print("oriY = ");
         Serial.println(oriY);
     }

     Y=sensorValue - oriY;
     Serial.print("Y = ");
     Serial.println(Y);

     if(servo_on){
         deg=servo2.read();
         if(Y>thresholdY){
             r=(float)Y/(float)maxY;
             servo2.write(deg+stepY);
             delay(15);
         }else if(Y<-thresholdY){
             r=(float)Y/(float)minY;
             servo2.write(deg-stepY);
             delay(15);
         }
     }

}