智能車教程（紅外、藍牙、OpenMV追蹤）

動手製作一個智能小車教程未完成

能親手製作智能車、機器人，大概是我們每一位marker都想實現的事情，那麼今天我們就來看看如何製作一輛可以紅外、藍牙控制，並且可以用openmv攝像頭實時追蹤色塊的智能車吧。

---

1.小車功能設計

現在我們手上有一套智能車模型套件、紅外遙控器及傳感器、藍牙模塊、一塊OpenMV攝像頭模塊。

• 首先，要快速完成一輛小車的製作，我們可以選用智能車模型套件，直接拼裝成智能車，這可以節省我們不少設計的功夫，將精力放在功能實現上面。
• 其次，我們可以使用紅外遙控和紅外接收模塊，對小車進行紅外控制；
• 再次，我們可以通過藍牙模塊，通過手機app，藍牙遠程遙控智能車；
• 最後，車子要做到智能，那必不可少的就是圖像識別，我們可以使用OpenMV攝像頭對顏色進行實時識別和跟蹤。

那就讓我們開始實現以上的想法吧，JUST DO IT！

2.應用學到那些知識

做這個項目之前，你需要有：

• 一點基礎的編程語法知識（任何語言）
• 一顆耐心和創客的激情

做完這個項目，你可以掌握：

• 電機驅動板L298N的使用方法
• arduino的使用方法
• 紅外模塊的使用方法
• 藍牙模塊HC-05的調試方法
• 手機控制藍牙的方法
• OpenMV攝像頭模塊顏色識別的方法
• OpenMV與arduino板子通信的方法

3.材料準備

1.智能車部分：模型套件，如下圖

2.控制電路部分：如下表

材料	數量
arduino模塊	1塊
L298N電機驅動模塊	1塊
紅外接收模塊	1塊
紅外遙控器	1件
藍牙模塊	1塊
USB轉TTL模塊（可選）	1塊
OpenMV3 攝像頭模塊	1塊
9-12V電源（博主使用三節18560串聯，推薦）	1件
杜邦線	若干
螺絲、螺母、銅柱	若干
熱熔膠槍	1把
螺絲刀套件	1套
L298N電機驅動模塊	1塊
焊烙鐵（可選）	1件
剝線鉗（可選）	1把
電壓表（可選）	1件
android手機（使用軟件）	1部

3.攝像頭部分：OpenMV攝像頭模塊，如下圖

4.組裝小車

按照說明書裝配小車模型。
電子模塊我是如下圖安裝的，當然大家可以按照自己的習慣安裝。
注意：

• 導線可以先不接，之後可以根據每一次實現的功能再接
• 連接OpenMV的支架爲博主自行切割的，大家可以動手自己設計支架

小車各模塊安裝示意圖

---

小車各模塊實際安裝圖

5.製作紅外小車

準備工作都做好了，那我們開始動手製作最簡單的紅外小車吧。

1. 如下圖連接杜邦線
   

紅外小車連線圖

2. 電路原理
   1. 紅外接收器
   2. L298N電機驅動板
   3. 電源選擇
   4. Arduino UNO引腳分類與供電方式
3. 打開Arduino開發環境並添加庫文件，如未安裝，點擊這裏
4. 上傳代碼

//紅外小車代碼及解釋
#include <IRremote.h>//打開IDE：項目→加載庫→添加→搜索IRremote→安裝IRredmote庫
int RECV_PIN = 11;//紅外接收端口
IRrecv irrecv(RECV_PIN);
decode_results results;//結構聲明

//==============================
int Left_motor_back=3;     //左電機後退(IN1)
int Left_motor_go=4;     //左電機前進(IN2)

int Right_motor_go=9;    // 右電機前進(IN3)
int Right_motor_back=10;    // 右電機後退(IN4)

void setup()
{
  //初始化電機驅動IO爲輸出方式
  pinMode(Left_motor_go,OUTPUT); // PIN 5 (PWM)
  pinMode(Left_motor_back,OUTPUT); // PIN 6 (PWM)
  pinMode(Right_motor_go,OUTPUT);// PIN 9 (PWM) 
  pinMode(Right_motor_back,OUTPUT);// PIN 10 (PWM)
  pinMode(13, OUTPUT);////端口模式，輸出
  Serial.begin(9600);   //波特率9600
  irrecv.enableIRIn(); // Start the receiver
}
void run()     // 前進
{
  digitalWrite(Right_motor_go,HIGH);  // 右電機前進
  digitalWrite(Right_motor_back,LOW);     
  digitalWrite(Left_motor_go,HIGH);  // 左電機前進
  digitalWrite(Left_motor_back,LOW);
}

void brake()         //剎車，停車
{
  digitalWrite(Right_motor_go,LOW);
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,LOW);
  digitalWrite(Left_motor_back,LOW);
}

void left()         //左轉(左輪不動，右輪前進)
{
  digitalWrite(Right_motor_go,HIGH);    // 右電機前進
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,LOW);   //左輪不動
  digitalWrite(Left_motor_back,LOW);
}

void spin_left()         //左轉(左輪後退，右輪前進)
{
  digitalWrite(Right_motor_go,HIGH);    // 右電機前進
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,LOW);   //左輪後退
  digitalWrite(Left_motor_back,HIGH);
}

void right()        //右轉(右輪不動，左輪前進)
{
  digitalWrite(Right_motor_go,LOW);   //右電機不動
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,HIGH);//左電機前進
  digitalWrite(Left_motor_back,LOW);
}

void spin_right()        //右轉(右輪後退，左輪前進)
{
  digitalWrite(Right_motor_go,LOW);   //右電機後退
  digitalWrite(Right_motor_back,HIGH);
  digitalWrite(Left_motor_go,HIGH);//左電機前進
  digitalWrite(Left_motor_back,LOW);
}

void back()          //後退
{
  digitalWrite(Right_motor_go,LOW);  //右輪後退
  digitalWrite(Right_motor_back,HIGH);
  digitalWrite(Left_motor_go,LOW);  //左輪後退
  digitalWrite(Left_motor_back,HIGH);
}

//=============================================================================
//讀取各個按鍵需要用到這段代碼
//=============================================================================

void read_key()
{
    if(irrecv.decode(&results)){  //如果接收到信息
        Serial.print("code:");
        Serial.println(results.value,HEX);//results.value爲16進制,unsigned long
        Serial.print("bits:");
        Serial.println(results.bits);//輸出元位數
        irrecv.resume();
      } 
}



void loop()
{
  read_key();
  if(irrecv.decode(&results)){  //如果接收到信息
   switch(results.value){
     case 0xFF18E7:  //前,對應2
       run();
       break;
     case 0xFF4AB5:  //後，對應8
       back();
       break;
     case 0xFF10EF:  //左，對應4
       left();
       break;
     case 0xFF5AA5:  //右，對應6
       right();
       break;
     case 0xFF38C7:  //停止，對應5
       brake();
       break;
     default:
       break;
   }
  irrecv.resume();
  }
}

5. 測試成果
   

紅外小車實際控制

6.添加藍牙控制功能

1. 如下圖連接杜邦線
   

藍牙小車連線圖

2. 電路原理及注意
   1. HC-05藍牙模塊原理
   2. 藍牙模塊與Arduino交叉連接（即藍牙RX連接板子TX、藍牙TX連接板子RX）
   3. 此處紅外模塊無需拆下，紅外接線對藍牙功能沒有影響，正好同時使用兩種功能
3. 上傳代碼

//藍牙小車代碼及解釋
#include <IRremote.h>//打開IDE：項目→加載庫→添加→搜索IRremote→安裝IRredmote庫
int RECV_PIN = 11;//紅外接收端口
IRrecv irrecv(RECV_PIN);
decode_results results;//結構聲明

//==============================
int Left_motor_back=3;     //左電機後退(IN1)
int Left_motor_go=4;     //左電機前進(IN2)

int Right_motor_go=9;    // 右電機前進(IN3)
int Right_motor_back=10;    // 右電機後退(IN4)

void setup()
{
  //初始化電機驅動IO爲輸出方式
  pinMode(Left_motor_go,OUTPUT); // PIN 5 (PWM)
  pinMode(Left_motor_back,OUTPUT); // PIN 6 (PWM)
  pinMode(Right_motor_go,OUTPUT);// PIN 9 (PWM) 
  pinMode(Right_motor_back,OUTPUT);// PIN 10 (PWM)
  pinMode(13, OUTPUT);////端口模式，輸出
  Serial.begin(9600);   //波特率9600
  irrecv.enableIRIn(); // Start the receiver
}
void run()     // 前進
{
  digitalWrite(Right_motor_go,HIGH);  // 右電機前進
  digitalWrite(Right_motor_back,LOW);     
  digitalWrite(Left_motor_go,HIGH);  // 左電機前進
  digitalWrite(Left_motor_back,LOW);
}

void brake()         //剎車，停車
{
  digitalWrite(Right_motor_go,LOW);
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,LOW);
  digitalWrite(Left_motor_back,LOW);
}

void left()         //左轉(左輪不動，右輪前進)
{
  digitalWrite(Right_motor_go,HIGH);    // 右電機前進
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,LOW);   //左輪不動
  digitalWrite(Left_motor_back,LOW);
}

void spin_left()         //左轉(左輪後退，右輪前進)
{
  digitalWrite(Right_motor_go,HIGH);    // 右電機前進
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,LOW);   //左輪後退
  digitalWrite(Left_motor_back,HIGH);
}

void right()        //右轉(右輪不動，左輪前進)
{
  digitalWrite(Right_motor_go,LOW);   //右電機不動
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,HIGH);//左電機前進
  digitalWrite(Left_motor_back,LOW);
}

void spin_right()        //右轉(右輪後退，左輪前進)
{
  digitalWrite(Right_motor_go,LOW);   //右電機後退
  digitalWrite(Right_motor_back,HIGH);
  digitalWrite(Left_motor_go,HIGH);//左電機前進
  digitalWrite(Left_motor_back,LOW);
}

void back()          //後退
{
  digitalWrite(Right_motor_go,LOW);  //右輪後退
  digitalWrite(Right_motor_back,HIGH);
  digitalWrite(Left_motor_go,LOW);  //左輪後退
  digitalWrite(Left_motor_back,HIGH);
}

//=============================================================================
//讀取各個按鍵需要用到這段代碼
//=============================================================================

void read_key()
{
    if(irrecv.decode(&results)){  //如果接收到信息
        Serial.print("code:");
        Serial.println(results.value,HEX);//results.value爲16進制,unsigned long
        Serial.print("bits:");
        Serial.println(results.bits);//輸出元位數
        irrecv.resume();
      } 
}



void loop()
{
  read_key();
  if(irrecv.decode(&results)){  //如果接收到信息
   switch(results.value){
     case 0xFF18E7:  //前,對應2
       run();
       break;
     case 0xFF4AB5:  //後，對應8
       back();
       break;
     case 0xFF10EF:  //左，對應4
       left();
       break;
     case 0xFF5AA5:  //右，對應6
       right();
       break;
     case 0xFF38C7:  //停止，對應5
       brake();
       break;
     default:
       break;
   }
  irrecv.resume();
  }
  //藍牙控制
  if(Serial.available()>0){
      char ch = Serial.read();
      if(ch == '1'){
         //前進
         run();
         Serial.print("前進");
      }else if(ch == '2'){
         //後退
         back();
         Serial.print("後退");
      }else if(ch == '3'){
         //左轉
         left();
         Serial.print("左轉");
      }else if(ch == '4'){
        //右轉
        right();
        Serial.print("右轉");
      }else if(ch=='0'){
        //停車
        brake();
        Serial.print("停車");
      }
    }
}

4. 手機安裝藍牙控制app ¹，連接藍牙，控制小車
   1. 下載安裝軟件
   2. 小車通電
   3. 打開軟件，點擊左下角連接小車藍牙
   4. 控制小車
5. 測試成果
   

藍牙小車實際控制

7.添加OpenMV顏色追蹤功能

1. 如下圖連接杜邦線
   
2. 電路原理及注意
   1. OpenMV教程參考1、參考2（需要先學習）
   2. 此項目是將OpenMV3作爲從設備，Arduino作爲主設備控制電機的
3. 上傳代碼

---

Arduino代碼

   /*
 * 藍牙串口指令規定
 * 改變運行模式：紅外：I（默認）、藍牙：B、openmv：O
 * 藍牙控制參數設置：前進：1、後退：2、左轉：3、右轉：4、停車：0、油門：5、剎車：6
 */
#include <IRremote.h>
#include <Wire.h>
#define BAUD_RATE 9600
#define CHAR_BUF 128
float left_speed = 1.1;
float right_speed = 1.1;
char buff[CHAR_BUF] = {0};
int RECV_PIN = 11;//紅外接收端口
IRrecv irrecv(RECV_PIN);
decode_results results;//結構聲明
char mode = 'I';  //設置小車運行模式，默認紅外模式
int Left_motor_back=3;     //左電機後退(IN1)
int Left_motor_go=4;     //左電機前進(IN2)
int Right_motor_go=9;    // 右電機前進(IN3)
int Right_motor_back=10;    // 右電機後退(IN4)
int ENA = 5;      //PWM輸入A
int ENB = 6;      //PWM輸入B
int speed_default = 100;    //0-255之間,小車最低速度爲70，最佳速度爲100
char ch;
bool inverse_left=false;
bool inverse_right=false;

void setup()
{
  //初始化電機驅動IO爲輸出方式
  pinMode(Left_motor_go,OUTPUT); // PIN 5 (PWM)
  pinMode(Left_motor_back,OUTPUT); // PIN 6 (PWM)
  pinMode(Right_motor_go,OUTPUT);// PIN 9 (PWM) 
  pinMode(Right_motor_back,OUTPUT);// PIN 10 (PWM)
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  pinMode(13, OUTPUT);////端口模式，輸出
  Serial.begin(BAUD_RATE);   //波特率9600
  irrecv.enableIRIn(); // Start the receiver
  Wire.begin();
  delay(1000); // 給OpenMV一個啓動的時間
}
void run()     // 前進
{
  digitalWrite(Right_motor_go,HIGH);  // 右電機前進
  digitalWrite(Right_motor_back,LOW);     
  digitalWrite(Left_motor_go,HIGH);  // 左電機前進
  digitalWrite(Left_motor_back,LOW);
  analogWrite(ENA,speed_default);
  analogWrite(ENB,speed_default);
}

void brake()         //剎車，停車
{
  digitalWrite(Right_motor_go,LOW);
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,LOW);
  digitalWrite(Left_motor_back,LOW);
  analogWrite(ENA,speed_default);
  analogWrite(ENB,speed_default);
}

void left()         //左轉(左輪不動，右輪前進)
{
  digitalWrite(Right_motor_go,HIGH);    // 右電機前進
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,LOW);   //左輪不動
  digitalWrite(Left_motor_back,LOW);
  analogWrite(ENA,speed_default);
  analogWrite(ENB,speed_default);
}

void spin_left()         //左轉(左輪後退，右輪前進)
{
  digitalWrite(Right_motor_go,HIGH);    // 右電機前進
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,LOW);   //左輪後退
  digitalWrite(Left_motor_back,HIGH);
  analogWrite(ENA,speed_default);
  analogWrite(ENB,speed_default);
}

void right()        //右轉(右輪不動，左輪前進)
{
  digitalWrite(Right_motor_go,LOW);   //右電機不動
  digitalWrite(Right_motor_back,LOW);
  digitalWrite(Left_motor_go,HIGH);//左電機前進
  digitalWrite(Left_motor_back,LOW);
  analogWrite(ENA,speed_default);
  analogWrite(ENB,speed_default);
}

void spin_right()        //右轉(右輪後退，左輪前進)
{
  digitalWrite(Right_motor_go,LOW);   //右電機後退
  digitalWrite(Right_motor_back,HIGH);
  digitalWrite(Left_motor_go,HIGH);//左電機前進
  digitalWrite(Left_motor_back,LOW);
  analogWrite(ENA,speed_default);
  analogWrite(ENB,speed_default);
}

void back()          //後退
{
  digitalWrite(Right_motor_go,LOW);  //右輪後退
  digitalWrite(Right_motor_back,HIGH);
  digitalWrite(Left_motor_go,LOW);  //左輪後退
  digitalWrite(Left_motor_back,HIGH);
  analogWrite(ENA,speed_default);
  analogWrite(ENB,speed_default);
}

//=============================================================================
//讀取各個按鍵需要用到這段代碼
//=============================================================================

void read_key()
{
    if(irrecv.decode(&results)){  //如果接收到信息
        Serial.print("code:");
        Serial.println(results.value,HEX);//results.value爲16進制,unsigned long
        Serial.print("bits:");
        Serial.println(results.bits);//輸出元位數
        irrecv.resume();
      } 
}

//=============================================================================
//處理字符串buff
//============================================================================

void getCode(){    //buff經過傳輸，尾部有干擾，故用兩個空格分割
  String temp1,temp2;
  String string = String(buff);
  int postion = string.indexOf(" ");
  temp1 = string.substring(0,postion);
  string = string.substring(postion+1,string.length());
  postion = postion = string.indexOf(" ");
  temp2 = string.substring(0,postion);
  left_speed = temp1.toFloat();
  right_speed = temp2.toFloat();
}
//=============================================================================
//PWM模式的小車運動
//============================================================================
void openmvrun(){
  if(inverse_left) 
    left_speed=-left_speed;
  if(inverse_right)
    right_speed=-right_speed;

  int l_speed = abs(left_speed);
  int r_speed = abs(right_speed);
  

   if(left_speed<0){
     digitalWrite(Left_motor_go,LOW);   //左輪後退
     digitalWrite(Left_motor_back,HIGH);
   }else{
     digitalWrite(Left_motor_go,HIGH);//左電機前進
     digitalWrite(Left_motor_back,LOW);
   }
   analogWrite(ENA,l_speed);

   if(right_speed<0){
     digitalWrite(Right_motor_go,LOW);  //右輪後退
     digitalWrite(Right_motor_back,HIGH);
   }else{
     digitalWrite(Right_motor_go,HIGH);    // 右電機前進
     digitalWrite(Right_motor_back,LOW);
   }
   analogWrite(ENB,r_speed);
   
   Serial.print(l_speed);
   Serial.print(" ");
   Serial.print(r_speed);
}

void loop()
{
  
  if(Serial.available()>0){
      ch = Serial.read();
      if(ch == 'I'){
        //紅外模式
        mode = 'I';
      }else if(ch == 'B'){
        //藍牙模式
        mode = 'B';
      }else if(ch == 'O'){
        //openmv模式
        mode = 'O';
      }
  }
  if(mode == 'I'){     //紅外模式控制代碼
    //Serial.println("紅外模式");
    read_key();
    if(irrecv.decode(&results)){  //如果接收到信息
      Serial.println(results.value);
      switch(results.value){
       case 0xFF18E7:  //前,對應2
         run();
         break;
       case 0xFF4AB5:  //後，對應8
         back();
         break;
       case 0xFF10EF:  //左，對應4
         left();
         break;
       case 0xFF5AA5:  //右，對應6
         right();
         break;
       case 0xFF38C7:  //停止，對應5
         brake();
         break;
       default:
         break;
      }
      irrecv.resume();
    }
  }

  if(mode == 'B'){   //藍牙模式控制代碼
    //Serial.println("藍牙模式");
    char ch1 = '0';
    if(ch == '1'){
         //前進
         run();
         Serial.print("前進");
      }else if(ch == '2'){
         //後退
         back();
         Serial.print("後退");
      }else if(ch == '3'){
         //左轉
         left();
         Serial.print("左轉");
      }else if(ch == '4'){
        //右轉
        right();
        Serial.print("右轉");
      }else if(ch=='0'){
        //停車
        brake();
        Serial.print("停車");
      }else if(ch=='5'){
        speed_default +=5;
        ch = ch1;
      }else if(ch=='6'){
        speed_default -=5;
        ch = ch1;
      }
      ch1 = ch;
      Serial.println(speed_default);
  }

  if(mode == 'O'){      //openmv模式控制代碼
    //Serial.println("openmv模式");
    int32_t temp = 0; 
    Wire.requestFrom(0x12, 2);
    if (Wire.available() == 2) { // got length?  
      temp = Wire.read() | (Wire.read() << 8);
      delay(1); // Give some setup time... 
      Wire.requestFrom(0x12, temp);
      if (Wire.available() == temp) { // got full message? 
        temp = 0;
        while (Wire.available()) buff[temp++] = Wire.read();  
      } else {
        while (Wire.available()) Wire.read(); // Toss garbage bytes.
      }
    } else {
      while (Wire.available()) Wire.read(); // Toss garbage bytes.
    }
  
    //Serial.println(buff);
    getCode();
    //Serial.println(left_speed+"  "+"right_speed="+right_speed);
    //Serial.print(left_speed);
    //Serial.print(" ");
    //Serial.print(right_speed);
    openmvrun();
    delay(1); // Don't loop to quickly.
  }
}

OpenMV代碼

#car.py
# Arduino 作爲I2C主設備， OpenMV作爲I2C從設備。
#
# 請把OpenMV和Arduino按照下面連線：
#
# OpenMV Cam Master I2C Data  (P5) - Arduino Uno Data  (A4)
# OpenMV Cam Master I2C Clock (P4) - Arduino Uno Clock (A5)
# OpenMV Cam Ground                - Arduino Ground

import pyb, ustruct
import ujson
from pyb import Pin, Timer

text = "Hello World!\n"
data = ustruct.pack("<%ds" % len(text), text)
# 使用 "ustruct" 來生成需要發送的數據包
# "<" 把數據以小端序放進struct中
# "%ds" 把字符串放進數據流，比如："13s" 對應的 "Hello World!\n" (13 chars).
# 詳見 https://docs.python.org/3/library/struct.html

# READ ME!!!
#
# 請理解，當您的OpenMV攝像頭不是I2C主設備，所以不管是使用中斷回調，
# 還是下方的輪循，都可能會錯過響應發送數據給主機。當這種情況發生時，
# Arduino會獲得NAK，並且不得不從OpenMV再次讀數據。請注意，
# OpenMV和Arduino都不擅長解決I2C的錯誤。在OpenMV和Arduino中，
# 你可以通過釋放I2C外設，再重新初始化外設，來恢復功能。

# OpenMV上的硬件I2C總線都是2
bus = pyb.I2C(2, pyb.I2C.SLAVE, addr=0x12)
bus.deinit() # 完全關閉設備
bus = pyb.I2C(2, pyb.I2C.SLAVE, addr=0x12)
print("Waiting for Arduino...")



# 請注意，爲了正常同步工作，OpenMV Cam必須 在Arduino輪詢數據之前運行此腳本。
# 否則，I2C字節幀會變得亂七八糟。所以，保持Arduino在reset狀態，
# 直到OpenMV顯示“Waiting for Arduino...”。

def run(left_speed, right_speed):
    data = str(left_speed)+" "+str(right_speed)+" "
    try:
        #print(data)
        bus.send(ustruct.pack("<h", len(data)), timeout=10000) # 首先發送長度 (16-bits).
        try:
            bus.send(data, timeout=10000) # 然後發送數據
            print("Sent Data!") # 沒有遇到錯誤時，會顯示
        except OSError as err:
            pass # 不用擔心遇到錯誤，會跳過
            # 請注意，有3個可能的錯誤。 超時錯誤（timeout error），
            # 通用錯誤（general purpose error）或繁忙錯誤
            #（busy error）。 “err.arg[0]”的錯誤代碼分別
            # 爲116,5,16。
    except OSError as err:
        pass # 不用擔心遇到錯誤，會跳過
        # 請注意，有3個可能的錯誤。 超時錯誤（timeout error），
        # 通用錯誤（general purpose error）或繁忙錯誤
        #（busy error）。 “err.arg[0]”的錯誤代碼分別
        # 爲116,5,16。

#pid.py
from pyb import millis
from math import pi, isnan

class PID:
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1/(2 * pi * 20)
    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._last_derivative = float('nan')

    def get_pid(self, error, scaler):
        tnow = millis()
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt) / float(1000)
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                derivative = (error - self._last_error) / delta_time
            derivative = self._last_derivative + \
                                     ((delta_time / (self._RC + delta_time)) * \
                                        (derivative - self._last_derivative))
            self._last_error = error
            self._last_derivative = derivative
            output += self._kd * derivative
        output *= scaler
        if abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            if self._integrator < -self._imax: self._integrator = -self._imax
            elif self._integrator > self._imax: self._integrator = self._imax
            output += self._integrator
        return output
    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')

#main.py
# Blob Detection Example
#
# This example shows off how to use the find_blobs function to find color
# blobs in the image. This example in particular looks for dark green objects.

import sensor, image, time
import car
from pid import PID

# You may need to tweak the above settings for tracking green things...
# Select an area in the Framebuffer to copy the color settings.

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.

# For color tracking to work really well you should ideally be in a very, very,
# very, controlled enviroment where the lighting is constant...
green_threshold   = (42, 80, 28, 127, -22, 55)   # 顏色閾值，不同物體需要修改
size_threshold = 2000               #小球距離
x_pid = PID(p=0.1, i=0.2, imax=30)     # 方向參數p
h_pid = PID(p=0.01, i=0.1, imax=100)    # 速度參數p

def find_max(blobs):                #找到視野中最大的色塊，即最大的小球
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.

    blobs = img.find_blobs([green_threshold])
    if blobs:
        max_blob = find_max(blobs)
        x_error = max_blob[5]-img.width()/2    #色塊的外框的中心x座標blob[5]
        h_error = max_blob[2]*max_blob[3]-size_threshold
        #色塊的外框的寬度blob[2],色塊的外框的高度blob[3]
        print("x error: ", x_error)   #打印 x 軸誤差  用於轉彎
        print("h error: ", h_error)   #打印 距離誤差  用於速度
        '''
        for b in blobs:
            # Draw a rect around the blob.
            img.draw_rectangle(b[0:4]) # rect
            img.draw_cross(b[5], b[6]) # cx, cy
        '''
        img.draw_rectangle(max_blob[0:4]) # rect
        img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
        x_output=x_pid.get_pid(x_error,1)
        h_output=h_pid.get_pid(h_error,1)    #h_error調整後的值
        print("x_output",x_output)
        print("h_output",h_output)

        car.run(-h_output-x_output,-h_output+x_output)
        print(-h_output-x_output,-h_output+x_output)
    else:
        car.run(0,0)

4. 測試成果
   

OpenMV顏色追蹤小車實際控制

8.項目總結

---

1. 此款app是根據@單片機菜鳥(博哥)藍牙小車項目的app基礎上修改。博哥在創客、esp8266、軟件編程等領域都是令人欽佩的一位技術大拿，有開源精神，向他學習！ ↩︎