2022年11月13日 星期日

[科技宝, Cagebot, 8787, AI, Arduino] 國廠祥儀科技寶智慧手臂 3/3:電控程式設篇

 國廠祥儀科技寶智慧手臂 3/3:

電控程式設計篇


Nov. 13, 2022
[1]

  [2][3]

本系列第三篇文章延續了第一篇《國廠祥儀科技寶智慧手臂 1/3:機構設計篇(https://reurl.cc/Wq1aWZ)》的解析,本文將進一步地著墨在控制板(Arduino Nano)PCA9685  8787 電控程式設計上(圖 1 最右方塊)。

圖 1:機電整合專案三大設計

完整的電控程式

我們先來看看將控制板、PCA9685 與 8787 三者整合後的完整程式(https://reurl.cc/585AlM)(註:祥儀的 RD 是以 Performio(https://www.performio.com)這套 IDE 來開發智慧手臂的測試程式的,所以,它的主程式為 main.cpp。我們打開這隻程式細看,setup()、loop() 依舊是 Arduino IDE 的 C/ C++ 語法。因此,只要將智慧手臂的函式庫(https://reurl.cc/58paLz)複製到Arduino IDE 預設的函式庫存放路徑 C:\Program Files (x86)\Arduino\libraries 或 C:\Users\<user>\Documents\Arduino\libraries 之下即可順利編譯之。):

黃色指令為用來預測 8787 移動路徑的卡爾曼濾波器(Kalman filter)的程式碼;
紫色指令為 PCA9685 伺服馬達擴充板的控制程式碼;
綠色指令為 威盛 8787 AI 視覺感測器的控制程式碼。

#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include "PCA9685.h"
#include "SimpleKalmanFilter.h"
#include "Pixetto.h"

#define sevo1_pin 0
#define servo1_angle_min -90
#define servo1_angle_max 90
#define servo1_angle_init 0

#define sevo2_pin 1
#define servo2_angle_min -90
#define servo2_angle_max 90
#define servo2_angle_init 0

#define sevo3_pin 2
#define servo3_angle_min -90
#define servo3_angle_max 90
#define servo3_angle_init 0

#define sevo4_pin 3
#define servo4_angle_min -90
#define servo4_angle_max 90
#define servo4_angle_init 0

#define rxPin 11
#define txPin 9

Pixetto ss(rxPin, txPin);

PCA9685 driver;
// PCA9685 輸出 = 12 位 = 4096 步
// 20ms 的 2.5% = 0.5ms ; 20ms 的 12.5% = 2.5ms
// 4096 的 2.5% = 102 步;4096 的 12.5% = 512 步
PCA9685_ServoEval pwmServo1; // (0deg, 90deg, 180deg)
PCA9685_ServoEval pwmServo2; // (0deg, 90deg, 180deg)
PCA9685_ServoEval pwmServo3; // (0deg, 90deg, 180deg)
PCA9685_ServoEval pwmServo4; // (0deg, 90deg, 180deg)

SimpleKalmanFilter KalmanFilter_X(1, 1, 0.01);
SimpleKalmanFilter KalmanFilter_Y(1, 1, 0.01);

int servo1Pos, servo2Pos, servo3Pos; // 當前角度
int KalmanX, KalmanY;

void setup() {
  Wire.begin(); // Wire must be started first

  driver.resetDevices(); // Software resets all PCA9685 devices on Wire line
  driver.init(); // Address pins A5-A0 set to B000000
  driver.setPWMFreqServo(); // Set frequency to 50Hz

  driver.setChannelPWM(0, pwmServo1.pwmForAngle(servo1_angle_init));
  delay(10);
  driver.setChannelPWM(1, pwmServo2.pwmForAngle(servo2_angle_init));
  delay(10);
  driver.setChannelPWM(2, pwmServo3.pwmForAngle(servo3_angle_init));
  delay(10);
  driver.setChannelPWM(3, pwmServo4.pwmForAngle(servo4_angle_init));
  delay(10);

  ss.begin();
  ss.enableFunc(Pixetto::FUNC_COLOR_DETECTION);
  delay(10);
  Serial.begin(9600);
  
  servo1Pos = servo1_angle_init+90;
  servo2Pos = servo2_angle_init+90;
  servo3Pos = servo3_angle_init+90;
}

void loop() {
  if (ss.isDetected()) {
    Serial.print("x:");
    Serial.println(ss.getPosX());
    Serial.print("y:");
    Serial.println(ss.getPosY());
    Serial.print("w:");
    Serial.println(ss.getWidth());
    Serial.println("");
    KalmanX = KalmanFilter_X.updateEstimate(ss.getPosX());
    KalmanY = KalmanFilter_Y.updateEstimate(ss.getPosY());
    if (ss.getFuncID() == Pixetto::FUNC_COLOR_DETECTION) {
      if (ss.getTypeID() == Pixetto::COLOR_RED) {
        if (KalmanX >= 50) {
          servo1Pos -= 5;
          if (servo1Pos >= servo1_angle_max+90)
            servo1Pos = 180;
          driver.setChannelPWM(0, pwmServo1.pwmForAngle(servo1Pos-90));
        }
        else if (KalmanX <= 25) {
          servo1Pos += 5;
          if (servo1Pos <= servo1_angle_min+90)
            servo1Pos = 0;
          driver.setChannelPWM(0, pwmServo1.pwmForAngle(servo1Pos-90));
        }
        else if (KalmanY <= 17) {
          servo2Pos += 5;
          if (servo2Pos >= servo2_angle_max+70)
            servo2Pos = 160;
          driver.setChannelPWM(1, pwmServo2.pwmForAngle(servo2Pos-90));

        }
        else if (KalmanY >= 60) {
          servo2Pos -= 5;
          if (servo2Pos <= servo2_angle_min+90)
            servo2Pos = 0;
          driver.setChannelPWM(1, pwmServo2.pwmForAngle(servo2Pos-90));
        }

        if (servo2Pos <= 65) {
          servo3Pos = servo2Pos + 25;
          if (servo3Pos <= servo3_angle_min+90)
            servo3Pos = 0;
          driver.setChannelPWM(2, pwmServo3.pwmForAngle(servo3Pos-90));
        }
        else if (servo2Pos >= 85) {
          servo3Pos = servo2Pos + 10;
          if (servo3Pos >= servo3_angle_max+90)
            servo3Pos = 180;
          driver.setChannelPWM(2, pwmServo3.pwmForAngle(servo3Pos-90));
        }
        else {
          servo3Pos = servo3_angle_init;
          driver.setChannelPWM(2, pwmServo3.pwmForAngle(servo3Pos));
        }

      }
    }
  }
}

如圖 2 所示,電控程式是用來控制硬體運作的指令集合,因此,我們先要想像智慧手臂要如何動作,然後將之對應到伺服馬達群的一連串控制劇本(script)。例如,我們想要讓智慧手臂追蹤到紅球就伸爪去夾住它,將這段劇本翻譯成伺服馬達控制語言則變成:左右轉動伺服馬達 1 去追紅球,水平定位到之後再移動伺服馬達 3 重直定位到紅球蹤跡。接著,調整伺服馬達 2 調整紅球和 8787 遠近的深度(depth)。三軸鎖定紅球的位置後即可張爪夾球。

圖 2:電控程式學習拆解

圖 2 中最右邊的淺藍色方塊表示現今流行的伺服馬達控制程式界面概可分為積木程式、C/ C++ 和 MicroPython 的語法程式。
關於智慧手臂如何自動追蹤紅球,在 Notepad++ 文字編輯器中,我們將上述的程式碼打開,就能很清楚的看到圖 2 中紅框內所框選的即為實作的程式碼,它判斷的數據如圖 3 上半部圖示。此外,在追蹤移動時,為了讓 8787 永遠保持置中,圖 2 中的綠框處即為實作的程式碼,它的判斷數據如圖 3 的最下方圖示。

圖 2:自動追蹤的程式片斷

圖 3:自動追蹤紅球時 8787 的移動位置調整

電控程式之主控板及開發環境建置

智慧手臂的控制板是 Arduino Nano 的相容板,裝妥 CH340 驅動程式、從裝置管理員確認它被指派到的 COM 埠編號後,我們就可以使用 Arduino IDE (我們使用 V1.8.9(https://reurl.cc/06Xq3Y)版本,祥儀 RD 建議以 V1.8.X 版為佳──經我們測試,最新的 V2.0.x 版改幅度不小,不建議讀者們採用)來開啟祥儀研發的範例程式。其中,Board 設定為 Arduino Nano、Processor 設定為 ATmega32p (Old Bootloader)、Port 設定為裝置管理員中列出 CH34 的 COM 埠編號。
萬事皆備,我們就可以將 Arduino 界最著名的 Blink 範例程式(讓板載在 D13 腳位上的 SMD LED 閃爍)上傳到控制板來測試我們的開發環境是否全然準備妥當,如圖 4 所示。我們將 Blink 的程式碼摘列如下:

void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_BUILTIN, OUTPUT);
}

void loop() {
  digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
  delay(1000);                       // wait for a second
  digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW
  delay(1000);                       // wait for a second
}

圖 4:控制板在 Arduino IDE V.1.8.9 的設定

電控程式之 PCA9685

為了能準確的控制每一顆伺服馬達,我們應該要先弄清楚一顆伺服馬達是如何以指令程式達成控制的。從 Github 上取得 PCA9685 的控制程式碼 pwmtest.ino https://reurl.cc/06XNMo,我們將之調整成「單顆」伺服馬達的測試程式如下:

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  150
#define SERVOMAX  600
#define USMIN  600
#define USMAX  2400
#define SERVO_FREQ 50

uint8_t servonum = 0;

void setup() {
  Serial.begin(9600);
  Serial.println("8 channel Servo test!");

  pwm.begin();
 
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);

  delay(10);
}

void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  
  pulselength = 1000000;
  pulselength /= SERVO_FREQ;
  Serial.print(pulselength); Serial.println(" us per period"); 
  pulselength /= 4096;
  Serial.print(pulselength); Serial.println(" us per bit"); 
  pulse *= 1000000;
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);
}

void loop() {
  Serial.println(servonum);
  for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) {
    pwm.setPWM(servonum, 0, pulselen);
  }

  delay(500);
  for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) {
    pwm.setPWM(servonum, 0, pulselen);
  }

  delay(500);

  for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) {
    pwm.writeMicroseconds(servonum, microsec);
  }

  delay(500);
  for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {
    pwm.writeMicroseconds(servonum, microsec);
  }

  delay(500);

  if (servonum > 7) servonum = 0; // Testing the first 8 servo channels
}

現在,我們已經學會了單顆伺服馬達了,接下來就可以輕鬆地把像圖 2 提及的智慧手臂動作劇本轉換成對應的四顆伺服馬達控制指令了,讀者們可以自行挑戰看看!

此外,卡爾曼濾波器的測試程式可從此 https://reurl.cc/X5V05M 下載測試並從中了解這個數學模型的使用方式。

電控程式之 8787

首先,在 Pixtto 公用程式(Pixetto Utility)中設定使用 8787 內建的顏色偵測(color detection) AI 模型,如圖 5 所示。

圖 5:在 Pixetto 公用程式中指定顏色偵測的內建 AI 功能

緊接著,因為智慧手臂的 Github(https://reurl.cc/58paLz)中已含有 8787 V1.5.1 版的 Arduino 函式庫,所以,在 Arduino IDE 中可以使用下列的語法架構如來設計:

if (ss.isDetected()) {
    if (ss.getFuncID() == Pixetto::FUNC_COLOR_DETECTION) {
        if (ss.getTypeID() == Pixetto::COLOR_RED) {
        //...
    }
}
}

關於 8787 內建的 AI 模型詳列如下,有興趣的讀者們可詳參 https://reurl.cc/qZN7Mp 這隻 C 語言的標頭檔(header file):

enum EFunc
{
FUNC_COLOR_DETECTION = 1,
FUNC_COLOR_CODE_DETECTION = 2,
FUNC_SHAPE_DETECTION = 3,
FUNC_SPHERE_DETECTION = 4,
FUNC_TEMPLATE_MATCHING = 6,
FUNC_KEYPOINTS = 8,
FUNC_NEURAL_NETWORK = 9,
FUNC_APRILTAG = 10,
FUNC_FACE_DETECTION = 11,
FUNC_TRAFFIC_SIGN_DETECTION = 12,
FUNC_HANDWRITTEN_DIGITS_DETECTION = 13,
FUNC_HANDWRITTEN_LETTERS_DETECTION = 14,
FUNC_CLOUD_DETECTION = 15,
FUNC_LANES_DETECTION = 16,
FUNC_EQUATION_DETECTION = 17,
FUNC_SIMPLE_CLASSIFIER = 18,
FUNC_VOICE_COMMAND = 19
};

[科技宝, Cagebot, 8787, AI, Arduino] 國廠祥儀科技寶智慧手臂 2/3:機電整合設計篇

 國廠祥儀科技寶智慧手臂 2/3:

機電整合設計篇


Nov. 13, 2022
[1]

 [2][3]

本系列第二篇文章延續了前一篇《國廠祥儀科技寶智慧手臂 1/3:機構設計篇(https://reurl.cc/Wq1aWZ)》的解析,本文將進一步地著墨在機電整合設計上(圖 1 中間方塊),仔細地剖析控制板(Arduino Nano)伺服馬達 16 路伺馬達擴充板 PCA9685 的電路設計。這三者的接線圖如圖 3,電路圖如圖 4 所示。註:智慧手臂的控制板在出廠的接線已改為以 D11(8787 Grove 的黃線接做為接收 Rx 的控制板接腳) 和 D9(8787 Grove 的白線接做為傳送 Tx 的控制板接腳) 和 8787 做四線對接交握(handshaking)通信,請讀者自行調整圖 2 和圖 3 的接腳接線。 

圖 1:機電整合專案三大設計

圖 2:智慧手臂接線圖

圖 3:智慧手臂電路圖

另一方面,因為伺服馬達與 8787 都是吃大電流的電子元件,以「多電源共地」方式拉電源是比較安穩的供電方式(圖 2 是以二顆 18650 的 7.4 V 電池並聯供電) [4]:

電源供應 1:供應 5V 給 PCA 9685 去控四顆伺服馬達
電源供應 2:供應 5V 給 8787
電源供應 3:供應 5V 給控制板 

機電整合之 Arduino Nano

智慧手臂使用祥儀自家設計的科技寶(Cagebot)控制板(https://reurl.cc/X5jn1a),這是一塊 Arduino Nano 的相容板子,使用前要先掛上 CH340 驅動程式(https://reurl.cc/VRDVK6)。然後在裝置管理員(device manager)(以 Windows 為例,其他作業系統請自行尋找)中可查看到作業系統所指派的序列埠(COM port)編號──此號碼很重要,一定要學會隨時查看,因為我們隨後會以 Arduino IDE 開發工具透過此通道將程式上傳到控制板中。

機電整合之伺服馬達

伺服馬達採用了閉迴路(closed-loop)迴授(feedback)機制(https://reurl.cc/aaV0vZ),使之能以 0~180 度控制其轉動角度。至於馬達內的各部件(parts),強烈建議讀者以破壞式學習(https://reurl.cc/aaV0vZ),透過逐一拆解的過程去印證這些組成部件的關連性,以建立知識的直覺(intutition)

機電整合之 PCA9685

(伺服)馬逹是以電生磁,磁生力的電磁作用轉動的,它在啟動瞬間需要較大的啟動電流去推動,因此,切勿貪圖一時方便直接將之接到控制板的 GPIO(General Purpose Input/ Output)接腳(pin)上。雖然偶一為之看不出它對控制板的傷害,但隨著多次的積累使用,其實是在加速控制板的老化(burnout),就好像一直往存款有限的戶頭大量提款,這筆存款就會很「快」地被提領殆盡的。因此,我們選擇了 16 路四線 I2C 匯流排(bus)介面的馬達擴充板 PCA9685(https://reurl.cc/58pVn7)來趨動智慧手臂的四軸。

機電整合之 8787

8787 以 Grove 四線(Tx 黃、Rx 白、+5V 紅、GND 黑)和控制板的 D11(Rx)與 D9(Tx)透過軟體序列(software serial)進行交握式通信。兩者間連接方式如圖 5 所示。其中,8787 的 micro USB 接頭可供影像輸出校正監控之用。強烈建議元成校正後請保持 8787 獨立供電,以確保它能正常工作。

圖 5:8787 接線圖