ㄆㄜㄊㄧㄡㄙˋ: ESP8285 兩軸伺服馬達雲台運動控制學習套件｛4 of 4｝雲台自穩與跟隨運動控制

網頁最後修改時間：2019/05/17

這篇是 ESP8285 兩軸伺服馬達雲台運動控制學習套件中的第四個部分：雲台的跟隨和自穩（鎖向）運動控制。

套件所包含的學習部分可由下面這張圖看出。

其中，本篇網頁主要說明的是圖中 雲台自穩、雲台跟隨 （中間上方，橘色線和框）的部分：

說明兩軸雲台如何根據 MPU6050 的轉動，讓自己動作在自穩（鎖向）或跟隨的模式。

*********************************************************************************
學習套件可至下面網址購買：

ESP8285（相容ESP8266）兩軸伺服馬達雲台運動控制學習套件（IMU 自穩、跟隨，手機 APP 控制等...）

*********************************************************************************

【接線圖與燒錄說明】

關於學習套件的接線與燒錄在前一篇網頁已做過詳細的說明，請自行跳轉至該網頁閱覽，這裡不再贅述！

【程式架構說明】

下圖是程式的架構示意圖，大致可看出整個執行的流程，只要與程式碼相對照，不難了解其中的流程與原理。

程式架構示意圖，點擊看超大圖

上面的程式架構與網頁提供的展示影片有所不同，此處程式已捨棄掉該無線網路組態的方式，改直接連線到 ESP8285 的 SoftAP，不過展示影片還是保留其無線網路組態的部分（因為已經拍了，只是後來改了）。

這裡說明一下整個程式的流程。

開機之後，會先等待 3 秒才繼續接下來的動作。若有使用串列埠軟體，則會看到類似下面的輸出，直到 Waiting self-calibration process... 後面出現 done! 之前，不要動模組！

出現 done! 之後，連線至ESP8285 的 SoftAP espPanTilt（密碼 123456789），此時 ESP8285 已經自行建立了一個 Web Server 等待遠方連線，此網址為 192.168.4.1，可打開瀏覽器進入。

程式執行時的 Serial 輸出

雲台模式設定網頁（下圖左）有跟隨以及自穩兩個選項可以選擇，選擇完畢按下 "上傳設定"　按鈕，一但 ESP8285 確實收到，那就會回傳下圖中或下圖右的訊息在最下面的那個欄位處；若沒有變化就是沒收到，那就是斷線！

雲台模式設定網頁，以及控制模式設定後回傳訊息

網頁的功能就只是讓用戶做雲台運動模式設定而已，所以我覺得不需要太複雜的前置步驟，因此為了開機之後能快速做設定，所以才就捨棄了無線網路設定的部分，改用 SoftAP 來取代。

/*-/--*-*/*/*/*/***//-*-*-**-*/*-*-/*/*/*-*-/-////--/**/**--**/--///--//**----**//--**//**----***//*-**//*
有購買商品的使用者，網頁中所需相關資料已放置於雲端硬碟，請用裡面的資料對照這裡的說明。
/*-/--*-*/*/*/*/***//-*-*-**-*/*-*-/*/*/*-*-/-////--/**/**--**/--///--//**----**//--**//**----***//*-**//*

【程式碼重點說明】

程式主要的架構，來自於上一篇的程式，但：保留 DMP 設定與輸出的部分、移除掉不需要的程式碼、最後加入網路和雲台運動控制部分結合而成。

前幾篇已說明過的部分，在這裡不再贅述，下面僅說明該程式重點的部分，其他的請自行看其裡面的註解。

/*--*//**---/*///**---*-*////***--*/*///***----*///--*/*///**--*/*//**--**/*//
* 雲台各軸轉動角度定義與 MPU6050 模組坐標系定義：

下圖是預設的 MPU6050 座標與雲台轉動角度之間的定義。

Pan Servo Motor 繞著 MPU6050 的 Z 軸轉動，輸出為 yaw = ypr[0]；Tilt Servo Motor 繞著 MPU6050 的 Y 軸轉動，輸出為 pitch = ypr[1]。

雲台轉動角度定義與 MPU6050 座標系

運動控制首先就是要先找出雲台對應的 ypr 各軸的輸出（要啟用 OUTPUT_YPR；關閉 ENABLE_SERVO）。例如，左右轉動 MPU6050 的 Z 軸時，要找出 ypr[0] 所對應的輸出角度範圍 [min, max]，然後將這個角度對應到 Pan Servo Motor 的 [0, 180]；Tilt Servo Motor 的處理方式也是一樣。

跟隨和自穩運動的控制方式可以看該程式的原始註解，這裡就不多加說明了。

/*--*//**---/*///**---*-*////***--*/*///***----*///--*/*///**--*/*//**--**/*//
* 全局變數部分：

全局變數裡面有兩個定義是關於數據輸出和控制伺服馬達動作的，：

line 2：OUTPUT_YPR
這個選項可以幫助用戶確認 MPU6050 轉動時所輸出的角度值為何？
如果你（妳）的 MPU6050 模組安裝的方向跟預設的不一樣，那就需要開啟這個選項以得知輸出的角度值範圍，然後才能將其轉換為伺服馬達的轉動角度。
line 4：ENABLE_SERVO
這個選項決定伺服馬達是否受控？
沒有開啟這個選項，雲台初始化之後就會停在原處，不會被任何模式所控制。
line 6...7：MPU6050 I2C 通訊接腳在 ESP8285 的定義。
line 8：MPU6050 中斷接腳在 ESP8285 的定義。

ESP_IMU_Gimbal_v012.ino, 全局變數, 部分程式碼

/// 是否輸出 YAW / PITCH / ROLL 數據
//#define OUTPUT_YPR
/// 是否致能 RC Servos
#define ENABLE_SERVO
// MPU6050 通訊接腳
const int PIN_SDA       = 10;    // Use pin 10 (IO10) on ESP8285
const int PIN_SCL       =  9;    // USe pin 9 (IO9)  on ESP8285
const int PIN_INTERRUPT =  4;    // Use pin 4 (IO4) on ESP8285

/*--*//**---/*///**---*-*////***--*/*///***----*///--*/*///**--*/*//**--**/*//
* WEBSERVER(80) 事件處理的部分：

重點在兩個 URI 網址的事件處理：雲台模式設定網頁的建立和上傳模式設定的處理。

** /settings：

由網頁中按下 "上傳設定" 後所發送的不是 m=follow，就是 m=stable。ESP8285 只要比對出來後沒問題，就會回覆一段 HTML 語法，表示雲台現在設定模式為何。

ESP_IMU_Gimbal_v012.ino, setup(), 部分程式碼

   /** 雲台模式處理 */
   WEBSERVER.on( "/settings", HTTP_POST, [](){
      if( WEBSERVER.hasArg( "m" ) ) {         
         gimbalmode = FOLLOW;
         String mode = WEBSERVER.arg( "m" );
         if( mode == "follow" ) {
            gimbalmode = FOLLOW;
            WEBSERVER.send( 200, "text/plain", 
                  mpu6050ready + "<p>Gimbal mode: follow</p>");
            Serial.println( F("Set gimbal to folllow mode") );
         }
         else if( mode == "stable" ) {
            gimbalmode = STABLE;
            WEBSERVER.send( 200, "text/plain", 
                  mpu6050ready + "<p>Gimbal mode: self-stability</p>");
            Serial.println( F("Set gimbal to self-stability mode") );
         }
      }     
   });

除了直接在網頁測試之外，也可以在 postman 如下面一樣的輸入做測試。

postman 測試 http://192.168.4.1/settings

** /：

當用戶輸入 192.168.4.1 連線至 espPanTilt 時，下面這一行程式就會被觸發，然後跳到 handleconfig() 函式準備網頁輸出的動作。

ESP_IMU_Gimbal_v012.ino, setup(), 部分程式碼

   /** 雲台組態畫面 */
   WEBSERVER.on( "/", handleconfig );

網頁主要由下面兩行構成，分隔開是為了要在其中插入 MPU6050 狀態的訊息。

** 未經處理過的原始網頁檔案，請看資料夾裡面的 index.html

ESP_IMU_Gimbal_v012.ino, 全局變數, 部分程式碼

// web page
const char HTML_PAGE_HEAD[] PROGMEM = "<!DOCTYPE HTML><HTML><HEAD> ...省略... </HEAD><BODY> ...省略...";
const char HTML_PAGE_END[] PROGMEM = "...省略... </BODY></HTML>";

插入 MPU6050 狀態訊息由下面這幾行程式碼來完成。

ESP_IMU_Gimbal_v012.ino, handleconfig()

void handleconfig() {
   //** 創建 config web page
   String page = FPSTR( HTML_PAGE_HEAD );          // 01
   page += mpu6050ready + "<br/>";                 // 02
   page += FPSTR( HTML_PAGE_END );                 // 03
   WEBSERVER.send( 200, "text/html", page );
}

網頁由 HTML + JavaScript + CSS 三個部分組成，原格式如下示意程式碼。

<!DOCTYPE HTML>
<HTML>
   <HEAD>
      <STYLE>...</STYLE>
      <SCRIPT>...</SCRIPT>
   </HEAD>
   <BODY>...</BODY>
</HTML>

完成各部分的撰寫和測試之後，我這裡採用的是將其最小化（也就是移除程式碼中不必要的字元，將其合為一行）的方法，把中間需要插入的部分再將其分開，程式碼中再加以組合後輸出。

<!DOCTYPE HTML><HTML><HEAD><STYLE>...</STYLE><SCRIPT>...</SCRIPT></HEAD><BODY>...</BODY></HTML>

若要在 ESP8285 輸出的網頁使用中文，那麼所有的中文都需要事先編碼（請自行 Google 找工具），而且輸出要像下面這種格式，否則在瀏覽器開啟就是亂碼一片！

例如：雲台模式設定 編碼後等於

雲台模式設定

/*--*//**---/*///**---*-*////***--*/*///***----*///--*/*///**--*/*//**--**/*//
* 雲台運動控制部分：

這部分修改自 MPU6050_DMP6_ESPWiFi 範例程式中的 mpu_loop() 函式，並易名為 mpu6050_dmp_loop()，是整個運動控制的中心。

為方便說明，程式分隔為兩段，僅說明加入的程式碼部分，運動控制部分的程式碼不提供。

這裡的程式碼沒變，只增加了 line 2...3 這兩個靜態變數：

line 2：紀錄開始輸出前的 yaw 偏移量；
line 3：紀錄開始輸出前忽略的數據輸出次數；

ESP_IMU_Gimbal_v012.ino, mpu6050_dmp_loop(), 1 of 2

void mpu6050_dmp_loop() {
   static float yaw_correct = 0;
   static   int correct_count = 0;

   // if programming failed, don't try to do anything
   if (!dmpReady) return;

   // wait for MPU interrupt or extra packet(s) available
   if (!MPU_INTERRUPT && fifoCount < packetSize) return;

   // reset interrupt flag and get INT_STATUS byte
   MPU_INTERRUPT = false;
   mpuIntStatus = mpu.getIntStatus();

   // get current FIFO count
   fifoCount = mpu.getFIFOCount();

   // check for overflow (this should never happen unless our code is too inefficient)
   if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount == 1024) {
      // reset so we can continue cleanly
      mpu.resetFIFO();
      Serial.println(F("FIFO overflow!"));

      // otherwise, check for DMP data ready interrupt (this should happen frequently)
   } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
      // wait for correct available data length, should be a VERY short wait
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

      // read a packet from FIFO
      mpu.getFIFOBytes(fifoBuffer, packetSize);

      // track FIFO count here in case there is > 1 packet available
      // (this lets us immediately read more without waiting for an interrupt)
      fifoCount -= packetSize;

      // Get Yaw / Pitch / Roll values
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

對於雲台的運動控制，所需要的只有 dmp_loop() 函式裡的 OUTPUT_READABLE_YAWPITCHROLL 的部分，其他的 #ifdef ... #endif 都可以移除掉。

      #ifdef OUTPUT_READABLE_YAWPITCHROLL
         // display Euler angles in degrees
         mpu.dmpGetQuaternion(&q, fifoBuffer);
         mpu.dmpGetGravity(&gravity, &q);
         mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
         Serial.print("ypr\t");
         Serial.print(ypr[0] * 180/M_PI);
         Serial.print("\t");
         Serial.print(ypr[1] * 180/M_PI);
         Serial.print("\t");
         Serial.println(ypr[2] * 180/M_PI);
      #endif

其中，ypr[0]、ypr[1] 和 ypr[2] 分別代表 Yaw、Pitch 和 Roll 未轉換為角度的數據。

除了一開始的前三行保留為 line 37...39 之外，移除其它並改寫如下：

line 45...55：忽略前 300 （0...299）個數據讀值（line 45），將第 301 個讀值（line 46）作為 ypr[0] 之後讀取需要減掉的偏移值 yaw_correct（line 48），然後將 mpu6050ready 設為準備就緒（line 50），最後輸出 done! （line 51）提示用戶裝置就緒。前三百次，每讀取一次 correct_count 加一。
line 55...85：開始處理 DMP 數據；

line 57：ypr[0] 讀取之後需要先減掉 yaw_correct 後才能用；
line 59...61：轉換 ypr 陣列值為角度形式；

line 63...70：輸出 ypr 陣列裡的值到 Serial Port，但需先在全局變數處使 OUTPUT_YPR 有效；

ESP_IMU_Gimbal_v012.ino, mpu6050_dmp_loop(), 2 of 2

      // 
      // MPU6050 DMP 完成後，會有一段期間還是處於自我校正，所以要忽略掉這段時間所讀取到的數據
      // 順便做等待，要忽略多久可以再自行設定。
      // 
      if(correct_count <= 300) {
         if(correct_count == 300) {
            // 取得第 301 的讀值
            yaw_correct = ypr[0];
            /// MPU6050 初始化與 DMP 設定成功
            mpu6050ready = "MPU6050 READY!<br/>";
            Serial.println( F("done!") );
         }
         correct_count++;
         
      } else {
         // 將 ypr[0] 設定為 0 度；
         ypr[0] = ypr[0] - yaw_correct;
         // 轉換 Yaw, Pitch, Roll 為角度值
         ypr[0] = ypr[0] * 180 / M_PI;
         ypr[1] = ypr[1] * 180 / M_PI;
         ypr[2] = ypr[2] * 180 / M_PI;

         #ifdef OUTPUT_YPR
            Serial.print("ypr\t");
            Serial.print(ypr[0]);
            Serial.print("\t");
            Serial.print(ypr[1]);
            Serial.print("\t");
            Serial.println(ypr[2]);
         #endif

         #ifdef ENABLE_SERVO
            int pan, tilt;
            switch( gimbalmode ) {
               case FOLLOW:
                  //...省略...看雲端資料夾程式碼中的註解
                  break;
               case STABLE:
                  //...省略...看雲端資料夾程式碼中的註解
                  break;
            }
            // 輸出雲台轉動值
            gimbal_move( pan, tilt );
         #endif
      }  // end of if(correct_count <= 300)
   }  // end of else if

}  // end of mpu6050_dmp_setup()

line 72...85：根據雲台模式的設定，對各軸指定相對應的轉動角度。

【展示說明影片】

下面有兩個展示影片：第一個是關於無線網路組態的設定；第二個就是連線到雲台模式設定網頁之後的雲台動作的展示。

現在網頁用的程式就不需要第一個影片中的操作步驟，電腦或手機直接連線至 SoftAP 後，打開瀏覽器輸入 192.168.4.1 取代第二個影片中的 IP 地址，動作就都一模一樣了！

【結論】

這個程式主要的重點與學習的地方，就是我們在程式碼中提到的那幾個部分：

從 ESP8285 回傳含中文內文的網頁；
ESP Web Server 怎麼與網頁互傳資料；
跟隨與自穩的控制方式差異（看雲端資料夾程式碼中的註解）；

到這裡，學習套件的部分就先告一段落了，有任何問題歡迎來信討論！

<< 部落格相關文章 >>

ㄆㄜㄊㄧㄡㄙˋ

2019年5月18日星期六

ESP8285 兩軸伺服馬達雲台運動控制學習套件｛4 of 4｝雲台自穩與跟隨運動控制

沒有留言:

張貼留言

2019年5月18日 星期六

ESP8285 兩軸伺服馬達雲台運動控制學習套件｛4 of 4｝雲台自穩與跟隨運動控制

沒有留言:

張貼留言

2019年5月18日星期六