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

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

/* センサーのアレイの初期化 */
void sensorInit(){
  for(int i=0; i<numSensors; i++){
    pinMode(sensorPin[i], INPUT);
  }
}

/* センサーアレイの状態を見る */
unsigned char sensorRead(){
  unsigned char b;
  //黒が1、白が0
  for(int i=0; i<numSensors; i++){
    if(digitalRead(sensorPin[i])) b += 1<<i;  //右端から順番に左端へ
  }
  b ^= 0xff;  //今回ラインが白なので、反転して、ラインを1とする
  return sensor = b;
}

/* センサーを読んでラインの位置を返す */
int scanSensor(){
  int pos = 0;
  switch(sensor&lineSensorMask){
  case 0b01000000:
    pos = -5;
    break;
  case 0b01100000:
    pos = -4;
    break;
  case 0b00100000:
    pos = -3;
    break;
  case 0b00110000:
    pos = -2;
    break;
  case 0b00010000:
    pos = -1;
    break;
  case 0b00011000:
    pos = 0;
    break;
  case 0b00001000:
    pos = 1;
    break;
  case 0b00001100:
    pos = 2;
    break;
  case 0b00000100:
    pos = 3;
    break;
  case 0b00000110:
    pos = 4;
    break;
  case 0b00000010:
    pos = 5;
    break;
  default:
    errorCount++;
    return 127;
  }
  
  errorCount = 0;
  
  static int forDiv16;
  if(forDiv16<16){
    lineDifferential += pos - linePosition;
    forDiv16++;
  }else{
    lineDifferential16 = lineDifferential;
    lineDifferential = 0;
    forDiv16 = 0;
  }
  linePosition = pos;
  lineIntegral += pos;
  return pos;
}

// 右のモーターと左のモーターの速度差 : (右のモーター)-(左のモーター)
int speedPIDControl(){
  return speedKP*linePosition + speedKI*lineIntegral + speedKD*lineDifferential16/16;
}

// ステアリングの角度(度)
int steeringPIDControl(){
  return steeringKP*linePosition + steeringKI*lineIntegral + steeringKD*lineDifferential16/16;
}

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

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

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