1. 自動運転の判断②

前回の表はあくまでイメージを掴む為の物でしたので、実際の制御に使う値に直したものを表1に示します。※3進数で処理し易いように水準も並びを変えました。

表の直した所の補足

〇センサーからのanalogRead値：3つの測距センサーからArduinoが取得する値(0～5Vを0～1023で表した値)で判定基準となる電圧の範囲

〇サーボへPWM出力する角度：障害物の距離に応じてステアリングの角度やスロットルアンプの開度を操作する基準となる値

　　　　　　　　　　　　　表1：自動運転の判断基準

項目	センサーからのanalogRead値 <=114(何もない)、 115～175(中距離に障害物が有る) 176<=(近くに障害物が有る)			サーボへPWM出力する角度ｽﾃｱﾘﾝｸﾞ:30(左),94(中),127(右) ｽﾛｯﾄﾙ：127(ﾊﾞｯｸ),94(止まる), 62(徐行),30(走る！)
ﾊﾟﾀｰﾝ番号	左	正面	右	ﾌﾛﾝﾄｽﾃｱ	ｽﾛｯﾄﾙ	ﾘｱｽﾃｱ
0	<=114	<=114	<=114	94	30	94
1	<=114	<=114	115～175	94	30	94
2	<=114	<=114	176<=	62	62	127
3	<=114	115～175	<=114	62	62	127
4	<=114	115～175	115～175	62	62	127
5	<=114	115～175	176<=	30	62	160
6	<=114	176<=	<=114	160	62	30
7	<=114	176<=	115～175	30	62	160
8	<=114	176<=	176<=	30	62	160
9	115～175	<=114	<=114	94	30	94
10	115～175	<=114	115～175	94	30	94
11	115～175	<=114	176<=	62	62	127
12	115～175	115～175	<=114	127	62	62
13	115～175	115～175	115～175	94	127	94
14	115～175	115～175	176<=	94	127	94
15	115～175	176<=	<=114	160	62	30
16	115～175	176<=	115～175	94	127	94
17	115～175	176<=	176<=	94	127	94
18	176<=	<=114	<=114	127	62	62
19	176<=	<=114	115～175	127	62	62
20	176<=	<=114	176<=	94	127	94
21	176<=	115～175	<=114	160	62	30
22	176<=	115～175	115～175	94	127	94
23	176<=	115～175	176<=	94	127	94
24	176<=	176<=	<=114	160	62	30
25	176<=	176<=	115～175	94	127	94
26	176<=	176<=	176<=	94	127	94

2. プログラム

という事で、表1の測距センサーからanalogReadした3つの値から0～26のパターンを判定し、そのパターンに応じたフロントステアリング・スロットル・リアステアリングへ送る値をSerial.printするプログラムを作ってみました！

byte FSOut[27] = {94, 94, 62, 62, 62, 30, 160, 30, 30, 94, 94, 62, 127, 94, 94, 160, 94, 94, 127, 127, 94, 160, 94, 94, 160, 94, 94}; //左30,中94,右160
byte THOut[27] = {30, 30, 62, 62, 62, 62, 62, 62, 62, 30, 30, 62, 62, 127, 127, 62, 127, 127, 62, 62, 127, 62, 127, 127, 62, 127, 127}; //前進30,停止94,バック160
byte RSOut[27] = {94, 94, 127, 127, 127, 160, 30, 160, 160, 94, 94, 127, 62, 94, 94, 30, 94, 94, 62, 62, 94, 30, 94, 94, 30, 94, 94}; //右30,中94,左160

void setup() {
  Serial.begin(9600);                  //9600bpsでシリアルポートを開く
}

void loop() {
  byte Pattern = State();            //State関数を実行し動作ﾊﾟﾀｰﾝ番号を取得
  Serial.print(FSOut[Pattern]);
  Serial.print(":");
  Serial.print(THOut[Pattern]);
  Serial.print(":");
  Serial.println(RSOut[Pattern]);  //iの値をシリアル出力します
  delay(1000);                           //1000ms(1秒)待ちます
}

byte State() { //3つのセンサーの状態から動作パターン番号を返す関数
  int L = 0, S = 0, R = 0, i = 0, A = 115, B = 175, C = 176;
  for (int i = 0; i < 9; i++) {           //測距ｾﾝｻｰの値を10回読み平均値を出す
    L = L + analogRead(0); S = S + analogRead(1); R = R + analogRead(2);
  }
  L = L / 10, S = S / 10, R = R / 10, i = 0;
  if (A <= L && L <= B) {
    i = i + 9; //左ｾﾝｻｰの状況判断
  } if (C <= L) {
    i = i + 18;
  }
  if (A <= S && S <= B) {
    i = i + 3; //正面ｾﾝｻｰの状況判断
  } if (C <= S) {
    i = i + 6;
  }
  if (A <= R && R <= B) {
    i = i + 1; //右ｾﾝｻｰの状況判断
  } if (C <= R) {
    i = i + 2;
  }
  return i;
}