2019年5月18日 星期六

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

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

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

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

其中,本篇網頁主要說明的是圖中 雲台自穩、雲台跟隨 (中間上方,橘色線和框)的部分:
說明兩軸雲台如何根據 MPU6050 的轉動,讓自己動作在自穩(鎖向)或跟隨的模式。

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

【接線圖與燒錄說明】

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

【程式架構說明】

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

上面的程式架構與網頁提供的展示影片有所不同,此處程式已捨棄掉該無線網路組態的方式,改直接連線到 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 2OUTPUT_YPR
    這個選項可以幫助用戶確認 MPU6050 轉動時所輸出的角度值為何?
    如果你(妳)的 MPU6050 模組安裝的方向跟預設的不一樣,那就需要開啟這個選項以得知輸出的角度值範圍,然後才能將其轉換為伺服馬達的轉動角度。
  • line 4ENABLE_SERVO
    這個選項決定伺服馬達是否受控?
    沒有開啟這個選項,雲台初始化之後就會停在原處,不會被任何模式所控制。
  • line 6...7:MPU6050 I2C 通訊接腳在 ESP8285 的定義。
  • line 8:MPU6050 中斷接腳在 ESP8285 的定義。
ESP_IMU_Gimbal_v012.ino, 全局變數, 部分程式碼
1
2
3
4
5
6
7
8
/// 是否輸出 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 找工具),而且輸出要像下面這種格式,否則在瀏覽器開啟就是亂碼一片!

例如:雲台模式設定 編碼後等於
&#38642;&#21488;&#27169;&#24335;&#35373;&#23450;
/*--*//**---/*///**---*-*////***--*/*///***----*///--*/*///**--*/*//**--**/*//
* 雲台運動控制部分:

這部分修改自 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
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
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 57ypr[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
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
      // 
      // 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 怎麼與網頁互傳資料;
  • 跟隨與自穩的控制方式差異(看雲端資料夾程式碼中的註解);
到這裡,學習套件的部分就先告一段落了,有任何問題歡迎來信討論!


<< 部落格相關文章 >>


沒有留言:

張貼留言