automatic_spray.ino

#include <Servo.h>
#define loopInterval 33  // loop関数のループ間隔[msec]
#define echoPin 2  // 超音波センサーからの入力ピン
#define trigPin 4  // 超音波センサーへのトリガ出力ピン
#define servoPin1 3  // 1つ目のサーボモーターを制御するピン
#define servoPin2 5  // 2つ目のサーボモーターを制御するピン
#define sonicSpeed 340 // 音速[m/sec]
#define sensorFilterLength 30 // 超音波センサー入力の平滑化フィルタの長さ
#define switchOnDistance 1.0 // サーボモータの角度をONに切り替える基準とする距離[cm]
#define switchOffDistance 6.0  // サーボモータの角度をOFFに切り替える基準とする距離[cm]
#define servo1OnDegree 0 // サーボモータONのときの角度[deg]
#define servo1OffDegree 180  // サーボモータOFFのときの角度[deg]
#define servo2OnDegree 180 // サーボモータONのときの角度[deg]
#define servo2OffDegree 0  // サーボモータOFFのときの角度[deg]

double duration = 0; //受信した間隔
double distance = 0; //距離
double distanceBuffer[sensorFilterLength];  // 超音波センサーから取得した値にノイズ除去フィルタ処理をかけるために用意するバッファ。
Servo gripServo1;  // 1つ目のサーボモーターのインスタンス
Servo gripServo2;  // 2つ目のサーボモーターのインスタンス

// 超音波センサーから取得した値を保持しておくバッファの初期化処理。
void initBuffer() {
  int i = 0;
  for (i = 0; i < sensorFilterLength; i++) {
    distanceBuffer[i] = 0.0;
  }
}

// 超音波センサーから取得した値を保持しておくバッファに、新たに値を1つ追加する。
// キューの要領で最も古い値を1つ破棄し、最新の値を1つ追加する。
void pushBuffer(double d) {
  int i = 0;
  for(i = sensorFilterLength - 2; i <= 0; i--) {
    distanceBuffer[i + 1] = distanceBuffer[i];
  }
  distanceBuffer[0] = d;
}

// 超音波センサーからの入力に平滑化フィルターをかけた後の値を取得する。
double getFilteredValue() {
  int i = 0;
  double output = 0.0;
  for(i = 0; i < sensorFilterLength; i++) {
    output += distanceBuffer[i];
  }
  output /= sensorFilterLength;
  return output;
}

// 初期設定処理
void setup() {
  Serial.begin( 9600 );
  initBuffer();
  pinMode( echoPin, INPUT );
  pinMode( trigPin, OUTPUT );
  gripServo1.attach(servoPin1);
  gripServo2.attach(servoPin2);
}

// メインの処理
void loop() {
  //超音波を出力するトリガ信号を超音波センサーに送信する。
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); //
  digitalWrite(trigPin, LOW);
  
  // 超音波センサーからの入力を取得する。
  duration = pulseIn(echoPin, HIGH);
  
  // 超音波センサーの距離計測結果が「0より大きな距離」だった場合
  if (duration > 0) {
    duration = duration / 2; //センサーから対象物までの片道経過時間
    distance = duration * sonicSpeed * 100 / 1000000; // 経過時間を距離に換算する。
    
    // 平滑化フィルタをかけるためのバッファにセンサー値を追加する。
    pushBuffer(distance);
    
    //Serial.print(distance);
    //Serial.println(" cm");
    
    // センサー値に平滑化フィルタをかけた値を取得する。
    double filteredDistance = getFilteredValue();
    Serial.print(filteredDistance);
    Serial.print(" cm : ");

    // 距離が<switchDistance>cm未満である場合
    if (filteredDistance < switchOnDistance) {
      // スプレーを吹きかける。
      gripServo1.write(servo1OnDegree);
      gripServo2.write(servo2OnDegree);
      Serial.println("on");
    }
    if(filteredDistance > switchOffDistance) {
      // スプレーを吹きかけない。（スプレーのハンドルを元の位置に戻す）
      gripServo1.write(servo1OffDegree);
      gripServo2.write(servo2OffDegree);
      Serial.println("off");
    }
  }
  
  // 一定時間待つ。
  delay(loopInterval);
}