Arduinoでセンサで値を取得した入力に対して、閾値以上なら出力を一定時間常に遅らせるプログラムを書きたい

###前提・実現したいこと
Arduinoを用いて、「センサで200ms毎に取得した値が、閾値以上ならモータを動かし、以下ならモータを停止させる」動作を取得から0.5秒ずらして行いたいです。
###発生している問題・エラーメッセージ
millis()関数を用いて試みたのですが、モータの動作がセンサとずれず、うまくいきませんでした。
また、現状のプログラムだと閾値以上の値が続く場合もその都度停止してしまうため困っています。

###該当のソースコード

int val=0;
int maxV=0;
int minV=0;
int start_time=0;

void setup() {
   pinMode(11,OUTPUT);
   Serial.begin(9600);
}

void loop() {
 
  val = analogRead(0);
  Serial.println(val);

  minV = 340;//閾値


  if(val > minV){
   if(start_time < 0){
    start_time = millis();
   }
   else{
    if (millis() - start_time > 500){
     analogWrite(11,HIGH);
     }
   }
  }

  else{
    analogWrite(11,LOW);
  }
  delay(200);

}

###試したこと
最初はdelay関数を用いることを考えたのですが、210msごとにセンサからデータを取得し続ける必要があるため、millis()関数を用いるべきではないかと考えました。

行動規範の内容に同意します

回答1件

ベストアンサー

意外と面倒でした。
まず、ループ中にdelayを入れると反応（処理の分解能）が悪くなるため、入れないようにします。
次に、データ取得と遅延動作処理は、それぞれ別の時刻値変数を用いて発火を管理します。

今回、データ取得間隔200msと遅延時間500msより、最大３個分の過去の状態変化を保持しておく必要があります。そのために簡易的なキューを用います。

以下は概念コードです。実機上では試していないので、適宜修正してください。

C++
1int last_time = -1; // 前回の状態取得時刻
2int last_state = -1;// 前回の状態
3
4// 簡易的なキュー
5// 過去３回分の状態の変化と時刻を記録
6int queTime[3];
7int queState[3];
8int que_cnt = 0;
9
10// キューに時刻と状態を追加
11void push_que( int time, int state)
12{
13    if( que_cnt >= 3) return;
14
15    queTime[que_cnt] = time;
16    queState[que_cnt] = state;
17    que_cnt++;
18}
19// 先頭要素の時刻を返す
20int head_que( void)
21{
22    if( que_cnt <= 0) return -1;
23    return queTime[0];
24}
25// 先頭要素の状態を返して削除
26int pop_que( void)
27{
28    if( que_cnt <= 0) return -1;
29
30    int state = queState[0];
31
32    // 要素を先頭にずらす
33    for( int i = 0; i < que_cnt-1; i++){
34        queTime[i] = queTime[i+1];
35        queState[i] = queState[i+1];
36    }
37    queTime[que_cnt-1] = -1;
38    queState[que_cnt-1] = -1;
39    que_cnt--;
40
41    return state;
42}
43
44int loop( void) {
45
46    int cur_time = millis();
47
48    // 状態を取得
49    if( (cur_time - last_time) >= 200){
50        int val = analogRead(0);
51        int cur_state = (int)(val > minV);
52
53        // 状態が変化 : キューに追加
54        if( cur_state != last_state){
55        //  Serial.println(val);
56            push_que( cur_time, cur_state);
57            last_state = cur_state;
58        }
59        last_time = cur_time;
60    }
61
62    // 溜まった状態を処理
63    int head_time = head_que();
64    if( head_time > 0 && (cur_time - head_time) >= 500){
65        int state = pop_que();
66        if( state){
67            analogWrite(11,HIGH);
68        }
69        else{
70            analogWrite(11,LOW);
71        }
72    }
73    // delay(1);    // 不要。入れる場合もできるだけ短くすること
74}