#include "Servo.h"
#define M1  9
#define M2  10
#define M3  11
#define TOTAL_MOTORS 2
Servo motor[TOTAL_MOTORS];
Servo base;
int mmax[TOTAL_MOTORS] = {180, 180};
int mmin[TOTAL_MOTORS] = {0, 0};
int current_degree[TOTAL_MOTORS] = {90, 90};

void setup() {
  Serial.begin(9600);
  pinMode(2, INPUT);
  pinMode(3, INPUT);
  motor[0].attach(M2);
  motor[1].attach(M3);
  base.attach(M1);
  base.write(90);
  delay(300);
  for(int i=0; i<TOTAL_MOTORS; i++) {
    motor[i].write(90); delay(200);
    current_degree[i] = 90;
  }
}
// 此函數用來一次把3個伺服馬達調整到指定的角度（degree陣列）
void arm_move_all_to(int degree[]) {
  int diff[TOTAL_MOTORS];
  int max_diff_degree=0;  // 最大的差異角度數
  int max_index=0;        // 最大差異角度的那個伺服馬達
  int remap[TOTAL_MOTORS][180];      // 重新對應角度用的陣列
  // 以下的迴圈用來找出哪一個伺服器的差異角度最多
  for(int i=0; i<TOTAL_MOTORS; i++) {
    diff[i] = degree[i] - current_degree[i] ;
    if(abs(diff[i])>max_diff_degree) {
      max_index = i;
      max_diff_degree = abs(diff[i]);
    }
  }
  // 以下重新對應要調整的角度陣列
  for(int i=0; i<=max_diff_degree; i++) {
    for(int motor_no=0; motor_no<TOTAL_MOTORS; motor_no++) {
      remap[motor_no][i] = current_degree[motor_no] +
        (int) (((float)diff[motor_no]/(float)max_diff_degree)*i);
      //disp_var(remap[motor_no][i]);
    }
  }
  // 以下開始讓伺服馬達調整角度
  for(int i=0; i<=max_diff_degree; i++) {
    for(int motor_no=0; motor_no<TOTAL_MOTORS; motor_no++) {
      current_degree[motor_no] = remap[motor_no][i];
      motor[motor_no].write(remap[motor_no][i]);
    }
    delay(20);
    //disp_current_degree();
  }
  //delay(1000);
}
int count=0;
void loop() {
  int pin2, pin3;
  int d[2];
  d[0]=170; d[1]=20;
  arm_move_all_to(d);
  d[0]=60; d[1]=180;
  arm_move_all_to(d);
  pin2 = digitalRead(2);
  pin3 = digitalRead(3);
  if(++count<5) {
    base.write(80);
    delay(300);
  } else {
    base.write(100);
    delay(300);
    if(count>10) count=0;
  }
    base.write(90);
    delay(300);
}