//----------------------------------
//    シッポシールドのためのスケッチ    
//----------------------------------

#include <Servo.h>

//////// 変数の宣言 ////////
//------ 加速度入力用
int inX, inY, inZ ;

//------ シッポの運動用
float x, y, vx, vy ;

//------ サーボ用
Servo servoX, servoY ; // サーボのオブジェクト
int  centerX = 80 ;    // X方向サーボの中央値（要調整）
int  centerY = 90 ;    // Y方向サーボの中央値（要調整）

//------ タイミング制御用
unsigned long msec ;

void setup() {
  //////// A/Dのリファレンス電圧の設定 ////////
  analogReference(EXTERNAL) ;

  //////// サーボの設定 ////////
  servoX.attach(9) ;
  servoY.attach(10) ;

  //////// シッポの運動の変数を初期化
  x = y = vx = vy = 0 ;
  
  //////// タイミングを初期化 ////////
  msec = millis() ;
}

void loop() {
  //////// 処理のタイミングを揃える ////////
  while(millis() < msec+20) ;
  msec = millis() ;  

  //////// 加速度を読む ////////
  inX = analogRead(0) - 510 ;
  inY = analogRead(1) - 510 ;
  inZ = analogRead(2) - 510 ;

  //////// シッポの移動目標値を算出（テキトウ） ////////
  int targetX, targetY ;
  targetX = inX * 2 ;
  targetY = inY * 1.5 + inZ * 2 ;

  //////// シッポの運動 ////////
  float sens = 0.01 ;  // 敏感具合
  float damp = 0.92 ;  // 減速具合

  //------ 速度を算出
  vx = vx + (targetX - x) * sens ;
  vy = vy + (targetY - y) * sens ;

  //------ 変位を算出
  x = x + vx ;
  y = y + vy ;

  //------ 減速
  vx *= damp ;
  vy *= damp ;

  //////// サーボの制御 ////////
  //------ 起動直後の2秒間はニュートラル位置にしておく
  if (millis() < 2000) { 
    servoX.write(centerX) ;
    servoY.write(centerY) ;
  }
  //------ 通常動作。可動範囲を制限してサーボ出力 
  else {
    servoX.write(constrain( (int)(x) + centerX, 20, 140) ) ;
    servoY.write(constrain( (int)(y) + centerY, 40, 140) ) ;
  }
}