サーボーモータで迷路をリモコン操作

#include ＜Servo.h＞
#include ＜Wire.h＞
#define STICK_ADDRESS 0x52

int posX = 0;
int posY = 0;
byte x = 0 ;
byte y = 0 ;
byte b = 0 ;
int xp = 0 ;
int yp = 0 ;

Servo servoX;  // servo object to control servo-X
Servo servoY;  // servo object to control servo-Y

void setup() {
  Wire.begin(); // マスタとしてI2Cバスに接続する
  servoX.attach(9); 	// X軸用のサーボを9番ピンに割り当て
  servoY.attach(10);	// Y軸用のサーボを10番ピンに割り当て
}

void loop() {
  // Joystickの読み取り
  // 3Byteでx軸(0-255),y軸(0-255),ボタン(0-1)を返す
  Wire.requestFrom(STICK_ADDRESS,3);
  while(Wire.available()) {
    x = Wire.read() ;
    y = Wire.read() ;
    b = Wire.read() ;
  }

  // 入力に変化があった時だけサーボの回転位置を再計算
  // 細かくやるとプルプル震えるので大雑把に16段階にする
  if (xp != x / 16 || yp != y / 16) {
    xp = x / 16 ;
    yp = y / 16 ;
    posX = 11.25 * xp ; // 180度を16段階で表現 
    posY = 11.25 * yp ; // 精密操作する訳じゃないので誤差は気にしない
  }

  servoX.write(posX); X軸用サーボ操作
  servoY.write(posY); Y軸用サーボ操作
  delay(15);  // waits 15ms for the servo to reach the position
}

#include ＜Wire.h＞
#define STICK_ADDRESS 0x52
#define ServoX 9
#define ServoY 10
int rX = 0;
int rY = 0;
byte x = 0 ;
byte y = 0 ;
byte b = 0 ;
int xp = 0 ;
int yp = 0 ;
long mts = 0 ;
long mtp = 0 ;

void setup() {
  Wire.begin(); // マスタとしてI2Cバスに接続する

  pinMode(ServoX, OUTPUT);
  pinMode(ServoY, OUTPUT);

  mts = micros() ;  // 経過時間計測基準点
}

void loop() {
  // Joystickの読み取り
  // 3Byteでx軸(0-255),y軸(0-255),ボタン(0-1)を返す
  Wire.requestFrom(STICK_ADDRESS,3);
  while(Wire.available()) {
    x = Wire.read() ;
    y = Wire.read() ;
    b = Wire.read() ;
  }

  // 入力に変化があった時だけサーボの回転位置を再計算
  // 細かくやるとプルプル震えるので大雑把に16段階にする
  if (xp != x / 16 || yp != y / 16) {
    xp = x / 16 ;
    yp = y / 16 ;
    rX = 125 * xp + 500 ;
    rY = 125 * yp + 500 ;
  }

  mtp = micros() ;  // 経過時間
  if (mtp - mts ＞= 20000) {
    // 20m秒経過毎にＬチカ
    mts = mtp ; // 経過時間再設定

    // X軸サーボのポジション設定
    digitalWrite(ServoX, HIGH );
    delayMicroseconds(rX) ;
    digitalWrite(ServoX, LOW );
  
    // Y軸サーボのポジション設定
    digitalWrite(ServoY, HIGH );
    delayMicroseconds(rY) ;
    digitalWrite(ServoY, LOW );
  }
}