hanzo/hanzo4_1Utils.ino

/* ステアリングを中央から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<NUM_SENSORS; i++){
    pinMode(PIN_SENSOR[i], INPUT);
  }
}

/* センサーアレイの状態を見る*/
unsigned char sensorRead(){
  unsigned char b;
  //黒が1、白が0
  for(int i=0; i<NUM_SENSORS; i++){
    if(digitalRead(PIN_SENSOR[i])) b += 1<<i;  //右端-左端
  }
  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);
}