/* ステアリングを中央からa[度]だけ動かす
  <- - 0 + ->
      ||
      ||
 []---{}---[]
     / .\
*/
void steering(int a){
  steeringServo.write(90 + a);
}

/* ランスを中央からa[度]だけ動かす
  <- - 0 + ->
     / .\
    /  | \
 []+--{}--+[]
*/
void lance(int a){
  lanceServo.write(90 - a);
}

/* センサーのアレイの初期化 */
void sensorInit(){
  pinMode(PIN_SENSOR_0, INPUT);  //右端
  pinMode(PIN_SENSOR_1, INPUT);
  pinMode(PIN_SENSOR_2, INPUT);
  pinMode(PIN_SENSOR_3, INPUT);
  pinMode(PIN_SENSOR_4, INPUT);
  pinMode(PIN_SENSOR_5, INPUT);
  pinMode(PIN_SENSOR_6, INPUT);
  pinMode(PIN_SENSOR_7, INPUT);  //左端
}

/* センサーアレイの状態を見る*/
unsigned char sensorRead(){
  unsigned char b;
  //黒が1、白が0
  if(digitalRead(PIN_SENSOR_0)) b += 0b00000001;  //右端
  if(digitalRead(PIN_SENSOR_1)) b += 0b00000010;
  if(digitalRead(PIN_SENSOR_2)) b += 0b00000100;
  if(digitalRead(PIN_SENSOR_3)) b += 0b00001000;
  if(digitalRead(PIN_SENSOR_4)) b += 0b00010000;
  if(digitalRead(PIN_SENSOR_5)) b += 0b00100000;
  if(digitalRead(PIN_SENSOR_6)) b += 0b01000000;
  if(digitalRead(PIN_SENSOR_7)) b += 0b10000000;  //左端
  b ^= 0xff;  //今回ラインが白なので、反転して、ラインを1とする
  return sensor = b;
}

/* エンコーダーの割り込み処理 */
void rot(){
  distance++;
}

/* 起動音 */
void startBeep(){
  tone(PIN_BUZZER,2000);
  delay(100);
  tone(PIN_BUZZER,1000);
  delay(100);
  noTone(PIN_BUZZER);
}

/* 終了音 */
void stopBeep(){
  tone(PIN_BUZZER, 2000);
  delay(2000);
  noTone(PIN_BUZZER);
}