hanzo/hanzo4_1Utils.ino

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

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

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

/* センサーアレイの状態を見る */
unsigned char scanSensor(){
  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;
}

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

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

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