國廠祥儀科技寶智慧手臂 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：自動追蹤的程式片斷
電控程式之主控板及開發環境建置
智慧手臂的控制板是 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 的程式碼摘列如下：
圖 4：控制板在 Arduino IDE V.1.8.9 的設定
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
}
電控程式之 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 所示。
緊接著，因為智慧手臂的 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
};
