プロジェクト12 超音波追従タンク

説明

プロジェクト11では、障害物回避カーを作成しました。実は、テストコードを少し変更するだけで、障害物回避カーを追従カーに変換することができます。このレッスンでは、超音波追従ロボットを作成します。超音波センサーはスマートカーと障害物の間の距離を検出し、タンクカーを動かします。

超音波追従ロボットの具体的なロジックは以下の通りです：

検出	前方障害物の測定距離	距離（単位：cm）
設定	サーボ角度90°
	8X16 LEDパネルに「V」アイコンを表示
条件	20≤ 距離 ≤60
状態	前進（PWMを200に設定）
条件	10<距離＜20
	距離＞60
状態	停止
条件	距離 ≤10
状態	後退（PWMを200に設定）

フローチャート

接続図

配線に関する注意：

1.8x16 LEDパネル		V5 センサーシールド
GND	→	-（GND）
VCC	→	+（VCC）
SDA	→	SDA
SCL	→	SCL

テストコード

 /*
 keyestudio Mini Tank Robot V2.1
 lesson 12
 ultrasonic follow tank
 http://www.keyestudio.com
*/ 
// 配列、パターンのデータを保存するために使用されます。自分で計算するか、モジュラスツールから取得できます
unsigned char start01[] = {0x01,0x02,0x04,0x08,0x10,0x20,0x40,0x80,0x80,0x40,0x20,0x10,0x08,0x04,0x02,0x01};
unsigned char front[] = {0x00,0x00,0x00,0x00,0x00,0x24,0x12,0x09,0x12,0x24,0x00,0x00,0x00,0x00,0x00,0x00};
unsigned char back[] = {0x00,0x00,0x00,0x00,0x00,0x24,0x48,0x90,0x48,0x24,0x00,0x00,0x00,0x00,0x00,0x00};
unsigned char left[] = {0x00,0x00,0x00,0x00,0x00,0x00,0x44,0x28,0x10,0x44,0x28,0x10,0x44,0x28,0x10,0x00};
unsigned char right[] = {0x00,0x10,0x28,0x44,0x10,0x28,0x44,0x10,0x28,0x44,0x00,0x00,0x00,0x00,0x00,0x00};
unsigned char STOP01[] = {0x2E,0x2A,0x3A,0x00,0x02,0x3E,0x02,0x00,0x3E,0x22,0x3E,0x00,0x3E,0x0A,0x0E,0x00};
unsigned char clear[] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
#define SCL_Pin  A5  // クロックピンをA5に設定
#define SDA_Pin  A4  // データピンをA4に設定

#define ML_Ctrl 13  // 左モーターの方向制御ピンを定義
#define ML_PWM 11   // 左モーターのPWM制御ピンを定義
#define MR_Ctrl 12  // 右モーターの方向制御ピンを定義
#define MR_PWM 3   // 右モーターのPWM制御ピンを定義
#define Trig 5  // 超音波トリガーピン
#define Echo 4  // 超音波エコーピン
int distance;
int pulsewidth;
#define servoPin 9  // サーボピン
void setup(){
  Serial.begin(9600);
  pinMode(SCL_Pin,OUTPUT);
  pinMode(SDA_Pin,OUTPUT);
  matrix_display(clear); // ディスプレイをクリア
  matrix_display(start01);  // スタートパターンを表示
  pinMode(servoPin, OUTPUT);
  procedure(90); // サーボを90°に設定
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
  pinMode(ML_Ctrl, OUTPUT);
  pinMode(ML_PWM, OUTPUT);
  pinMode(MR_Ctrl, OUTPUT);
  pinMode(MR_PWM, OUTPUT);
}
void loop(){
  distance = checkdistance();  // 超音波センサーで検出した距離をdistanceに代入
  if (distance >= 20 && distance <= 60) // 前進する範囲
  {
    Car_front();
  }
  else if (distance > 10 && distance < 20)  // 停止する範囲
  {
    Car_Stop();
  }
  else if (distance <= 10)  // 後退する範囲
  {
    Car_back();
  }
  else  // その他の状況では停止
  {
    Car_Stop();
  }
}
/***********モーター動作用の関数****************/
void Car_front()
{
  digitalWrite(MR_Ctrl,LOW);
  analogWrite(MR_PWM,200);
  digitalWrite(ML_Ctrl,LOW);
  analogWrite(ML_PWM,200);
}
void Car_back()
{
  digitalWrite(MR_Ctrl,HIGH);
  analogWrite(MR_PWM,200);
  digitalWrite(ML_Ctrl,HIGH);
  analogWrite(ML_PWM,200);
}
void Car_left()
{
  digitalWrite(MR_Ctrl,LOW);
  analogWrite(MR_PWM,200);
  digitalWrite(ML_Ctrl,HIGH);
  analogWrite(ML_PWM,200);
}
void Car_right()
{
  digitalWrite(MR_Ctrl,HIGH);
  analogWrite(MR_PWM,200);
  digitalWrite(ML_Ctrl,LOW);
  analogWrite(ML_PWM,200);
}
void Car_Stop()
{
  digitalWrite(MR_Ctrl,LOW);
  analogWrite(MR_PWM,0);
  digitalWrite(ML_Ctrl,LOW);
  analogWrite(ML_PWM,0);
}

/******************ドットマトリックス********************/
// ドットマトリックス表示用の関数
void matrix_display(unsigned char matrix_value[])
{
  IIC_start(); // データ送信開始の関数を呼び出し
  IIC_send(0xc0);  // アドレスを選択
  
  for(int i = 0;i < 16;i++) // パターンデータは16ビット
  {
     IIC_send(matrix_value[i]); // パターンを伝達するデータ
  }

  IIC_end();   // データパターン伝達を終了
  
  IIC_start();
  IIC_send(0x8A);  // パルス幅4/16を選択し、表示を制御
  IIC_end();
}

// データ送信開始の条件
void IIC_start()
{
  digitalWrite(SCL_Pin,HIGH);
  delayMicroseconds(3);
  digitalWrite(SDA_Pin,HIGH);
  delayMicroseconds(3);
  digitalWrite(SDA_Pin,LOW);
  delayMicroseconds(3);
}

// データを送信
void IIC_send(unsigned char send_data)
{
  for(char i = 0;i < 8;i++)  // 各バイトは8ビット
  {
      digitalWrite(SCL_Pin,LOW);  // クロックピンSCL_Pinを下げてSDAの信号を変更      
delayMicroseconds(3);
      if(send_data & 0x01)  // 各ビットの1または0に応じてSDA_Pinの高低レベルを設定
      {
        digitalWrite(SDA_Pin,HIGH);
      }
      else
      {
        digitalWrite(SDA_Pin,LOW);
      }
      delayMicroseconds(3);
      digitalWrite(SCL_Pin,HIGH); // クロックピンSCL_Pinを上げてデータ送信を停止
      delayMicroseconds(3);
      send_data = send_data >> 1;  // ビットごとに検出するため、データを1ビット右にシフト
  }
}
// データ送信終了の合図
void IIC_end()
{
  digitalWrite(SCL_Pin,LOW);
  delayMicroseconds(3);
  digitalWrite(SDA_Pin,LOW);
  delayMicroseconds(3);
  digitalWrite(SCL_Pin,HIGH);
  delayMicroseconds(3);
  digitalWrite(SDA_Pin,HIGH);
  delayMicroseconds(3);
}
/***************ドットマトリックス表示終了******************/
// サーボを制御する関数
void procedure(int myangle) {
  for (int i = 0; i <= 50; i = i + (1)) {
    pulsewidth = myangle * 11 + 500;
    digitalWrite(servoPin,HIGH);
    delayMicroseconds(pulsewidth);
    digitalWrite(servoPin,LOW);
    delay((20 - pulsewidth / 1000));
  }}
// 超音波センサー制御関数 超音波を制御する関数
float checkdistance() {
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  float distance = pulseIn(Echo, HIGH) / 58.20;  // 58.20、つまり、2*29.1=58.2
  delay(10);
  return distance;
}
// ****************************************************************

テスト結果

コードをアップロードしました。DIPスイッチを右端に切り替え、サーボが90°に回転し、8X16 LEDパネルに「V」が表示され、スマートカーが障害物に応じて動きます。