目標
做出一台可以遙控的機械手臂車,遙控器與車輛間採用類 Wi-Fi 通訊(ESP-NOW),方便後續接入網路成為 AIoT 設備。
成員
411214306 林致宇
411214307 胡朝凱
車輛與機械臂
1*ESP32
5*伺服馬達 (機械臂)
1*伺服馬達 (車輛轉向)
1*馬達
2*OLED 顯示器
遙控器
1*ESP32
2*雙軸搖桿
6*開關與按鈕
1*可變電組
1*OLED 顯示器
測量各物件的實際尺寸,使用 Shapr3D 進行所需要的建模。
將建模好的檔案傳入切片軟體,切片完成後傳輸至 3D 列印機以 PLA 材料進行 3D 列印。
專案程式碼較多,使用 ArduinoIDE 開發不便,所以採用 PlatformIO 作為開發框架。
車上的 ESP32 開發板在某次的程式上傳後就突然不會動了,有連上電腦但又無法上傳。Bootloader 刷不進去,ESP32 模組變很燙,推測 ESP32 壞了。
需要一片新的開發板以解決此問題。
[platformio]
default_envs = vehicle_node32s ; 預設編譯環境 (可改)
; --- 共用設定 (Common Settings) ---
[env]
platform = espressif32
framework = arduino
monitor_speed = 115200
build_flags =
-I include ; 確保編譯器找得到 common.h
; --- 環境 1: 車輛端 (Vehicle) ---
[env:vehicle_node32s]
board = nodemcu-32s
; 關鍵指令:只編譯 src/Vehicle 下的檔案,排除 Remote
src_filter = +<Vehicle/> -<Remote/>
lib_deps =
madhephaestus/ESP32Servo @ ^1.1.0
fastled/FastLED @ ^3.6.0
; --- 環境 2: 遙控器端 (Remote) ---
[env:remote_lolin32]
board = lolin32_lite
; 關鍵指令:只編譯 src/Remote 下的檔案,排除 Vehicle
src_filter = +<Remote/> -<Vehicle/>
lib_deps =
adafruit/Adafruit SSD1306 @ ^2.5.9
adafruit/Adafruit GFX Library @ ^1.11.9
adafruit/Adafruit BusIO @ ^1.15.0
// 檔案名稱: common.h
#ifndef COMMON_H
#define COMMON_H
#include <Arduino.h>
// 燈光狀態位元定義
#define LIGHT_LEFT 0x01
#define LIGHT_RIGHT 0x02
#define LIGHT_HAZARD 0x04
#define LIGHT_HEAD 0x08
#define LIGHT_RESERVED 0x10
// 遙控器 -> 車輛 (控制指令)
typedef struct {
int16_t lx; // 左搖桿 X (-512 ~ 512)
int16_t ly; // 左搖桿 Y
int16_t rx; // 右搖桿 X
int16_t ry; // 右搖桿 Y
int16_t pot; // 旋鈕 (0 ~ 4095)
uint8_t mode; // 0: 車輛模式, 1: 手臂模式
uint8_t light_bits; // 燈光狀態
} ControlPacket;
// 車輛 -> 遙控器 (回傳數據)
typedef struct {
float rpm; // 車速
bool arm_active; // 確認手臂是否激活
} TelemetryPacket;
#endif
// 車輛端 Firmware (NodeMCU-32S)
#include <Arduino.h>
#include <ESP32Servo.h>
#include <FastLED.h>
#include <WiFi.h>
#include <esp_now.h>
#include "common.h"
// --- MAC 設定 ---
uint8_t remoteMac[] = {0x08, 0x3A, 0xF2, 0xA9, 0x1E, 0x18};
// --- 硬體定義 ---
#define PIN_MOTOR_EN 4
#define PIN_MOTOR_PWM 16
#define PIN_MOTOR_DIR 17
#define PIN_ENC_A 34
#define PIN_ENC_B 35
#define PIN_LED_STRIP 13
#define NUM_LEDS 22
// Servo Pins: Steer, Base, Shoulder, Elbow, Wrist, Grip
const int SERVO_PINS[] = {18, 19, 26, 27, 23, 25};
Servo servos[6];
// --- 全域變數 ---
ControlPacket rxData;
CRGB leds[NUM_LEDS];
unsigned long lastRecvTime = 0;
bool isConnected = false;
// 平滑控制變數
float currentServoPos[6] = {90, 90, 90, 90, 90, 90};
// 測速變數
volatile long encoderCount = 0;
unsigned long lastRpmTime = 0;
float currentRpm = 0;
// 燈光計時
unsigned long blinkTimer = 0;
bool blinkState = false;
static void selfTestLights() {
fill_solid(leds, NUM_LEDS, CRGB::Red);
FastLED.show();
delay(300);
fill_solid(leds, NUM_LEDS, CRGB::Green);
FastLED.show();
delay(300);
fill_solid(leds, NUM_LEDS, CRGB::Blue);
FastLED.show();
delay(300);
fill_solid(leds, NUM_LEDS, CRGB::Black);
FastLED.show();
}
static inline int16_t clampStick(int16_t v) {
if (v > 512) return 512;
if (v < -512) return -512;
return v;
}
static inline int16_t clampPot(int16_t v) {
if (v < 0) return 0;
if (v > 4095) return 4095;
return v;
}
// --- 中斷:編碼器讀取 ---
void IRAM_ATTR readEncoder() { encoderCount++; }
// --- 燈光控制邏輯 ---
void updateLights() {
// 斷線保護
if (!isConnected) {
if ((millis() / 200) % 2)
fill_solid(leds, NUM_LEDS, CRGB::Red);
else
fill_solid(leds, NUM_LEDS, CRGB::Black);
FastLED.show();
return;
}
fill_solid(leds, NUM_LEDS, CRGB::Black); // 清除背景
uint8_t lights = rxData.light_bits;
// 閃爍計時 (300ms)
if (millis() - blinkTimer > 300) {
blinkTimer = millis();
blinkState = !blinkState;
}
// 大燈 (0-10 前白, 11-21 後微紅)
if (lights & LIGHT_HEAD) {
for (int i = 0; i <= 10; i++) leds[i] += CRGB(150, 150, 150);
for (int i = 11; i < 22; i++) leds[i] += CRGB(30, 0, 0);
}
// 煞車燈 (油門歸零時全亮紅)
int16_t throttle = clampStick(rxData.ly);
if (rxData.mode == 0 && abs(throttle) < 20) {
for (int i = 11; i < 22; i++) leds[i] = CRGB::Red;
}
// 方向燈 (優先權:雙閃 > 方向燈)
CRGB sigColor = CRGB::Orange;
if (lights & LIGHT_HAZARD) {
if (blinkState) fill_solid(leds, NUM_LEDS, sigColor);
} else {
// 左轉 (假設 0-2 為左前, 19-21 為左後)
if ((lights & LIGHT_LEFT) && blinkState) {
leds[0] = sigColor;
leds[1] = sigColor;
leds[20] = sigColor;
leds[21] = sigColor;
}
// 右轉 (假設 8-10 為右前, 11-13 為右後)
if ((lights & LIGHT_RIGHT) && blinkState) {
leds[9] = sigColor;
leds[10] = sigColor;
leds[11] = sigColor;
leds[12] = sigColor;
}
}
// 預留功能 (藍光特效)
if (lights & LIGHT_RESERVED) {
if (blinkState) fill_solid(leds, NUM_LEDS, CRGB::Blue);
}
FastLED.show();
}
// --- 運動控制 (含平滑演算法) ---
void updateMotion() {
// 斷線強制停車
if (!isConnected) {
digitalWrite(PIN_MOTOR_EN, LOW);
return;
}
// 手臂模式:強制停車 (避免用 ly 控肩膀時車亂跑)
if (rxData.mode == 1) {
digitalWrite(PIN_MOTOR_EN, LOW);
ledcWrite(0, 255);
} else {
// 1. 馬達控制 (反相PWM: 255=停, 0=全速)
int16_t throttle = clampStick(rxData.ly);
int pwmVal = map(abs(throttle), 0, 512, 0, 255);
pwmVal = constrain(pwmVal, 0, 255);
if (abs(throttle) < 20) {
digitalWrite(PIN_MOTOR_EN, LOW);
ledcWrite(0, 255);
} else {
digitalWrite(PIN_MOTOR_EN, HIGH);
digitalWrite(PIN_MOTOR_DIR,
throttle > 0 ? HIGH : LOW); // 若方向相反請改 LOW:HIGH
ledcWrite(0, 255 - pwmVal);
}
}
// 2. Servo 目標計算
int target[6];
int16_t lx = clampStick(rxData.lx);
int16_t ly = clampStick(rxData.ly);
int16_t rx = clampStick(rxData.rx);
int16_t ry = clampStick(rxData.ry);
int16_t pot = clampPot(rxData.pot);
if (rxData.mode == 0) { // 車輛模式
target[0] = map(rx, -512, 512, 0, 180); // 轉向
for (int i = 1; i <= 4; i++)
target[i] = (int)currentServoPos[i]; // 手臂鎖定
target[5] = map(pot, 0, 4095, 0, 180); // 夾頭
} else { // 手臂模式
target[0] = 90; // 轉向回正
target[1] = map(lx, -512, 512, 0, 180); // Base
target[2] = map(ly, -512, 512, 0, 180); // Shoulder
target[3] = map(ry, -512, 512, 0, 180); // Elbow
target[4] = map(rx, -512, 512, 0, 180); // Wrist
target[5] = map(pot, 0, 4095, 0, 180); // Grip
}
// 3. 平滑執行
float k_fast = 0.6; // 轉向與夾頭反應快
float k_slow = 0.05; // 手臂反應慢(防甩)
for (int i = 0; i < 6; i++) {
float k = (i == 0 || i == 5) ? k_fast : k_slow;
currentServoPos[i] = (currentServoPos[i] * (1 - k)) + (target[i] * k);
servos[i].write((int)currentServoPos[i]);
}
}
// --- RPM 計算與回傳 ---
void handleTelemetry() {
// 計算 RPM (每 100ms)
if (millis() - lastRpmTime > 100) {
// 假設編碼器 PPR=360 (請依規格調整)
currentRpm = (encoderCount / 360.0) * 600.0; // 轉換為 RPM
encoderCount = 0;
lastRpmTime = millis();
}
// 發送回傳 (每 200ms)
static unsigned long lastSend = 0;
if (isConnected && millis() - lastSend > 200) {
TelemetryPacket packet;
packet.rpm = currentRpm;
packet.arm_active = (rxData.mode == 1);
esp_now_send(remoteMac, (uint8_t*)&packet, sizeof(packet));
lastSend = millis();
}
}
// --- ESP-NOW Callback ---
void OnDataRecv(const uint8_t* mac, const uint8_t* incomingData, int len) {
if (len != sizeof(rxData)) return;
memcpy(&rxData, incomingData, sizeof(rxData));
lastRecvTime = millis();
isConnected = true;
}
void setup() {
Serial.begin(115200);
Serial.println("[VEHICLE] boot");
// Motor
pinMode(PIN_MOTOR_EN, OUTPUT);
pinMode(PIN_MOTOR_DIR, OUTPUT);
ledcSetup(0, 20000, 8);
ledcAttachPin(PIN_MOTOR_PWM, 0);
// Encoder
pinMode(PIN_ENC_A, INPUT);
pinMode(PIN_ENC_B, INPUT);
attachInterrupt(digitalPinToInterrupt(PIN_ENC_A), readEncoder, RISING);
// Servos
for (int i = 0; i < 6; i++) servos[i].attach(SERVO_PINS[i]);
// LEDs
pinMode(PIN_LED_STRIP, OUTPUT);
FastLED.addLeds<WS2812B, PIN_LED_STRIP, GRB>(leds, NUM_LEDS);
FastLED.setBrightness(100);
selfTestLights();
// WiFi
WiFi.mode(WIFI_STA);
if (esp_now_init() != ESP_OK) return;
esp_now_register_recv_cb(OnDataRecv);
// Register Peer (Remote)
esp_now_peer_info_t peerInfo;
memset(&peerInfo, 0, sizeof(peerInfo));
memcpy(peerInfo.peer_addr, remoteMac, 6);
peerInfo.channel = 0;
peerInfo.encrypt = false;
peerInfo.ifidx = WIFI_IF_STA;
esp_err_t addStatus = esp_now_add_peer(&peerInfo);
if (addStatus != ESP_OK) {
Serial.print("ESP-NOW add peer failed: ");
Serial.println(addStatus);
} else {
Serial.println("ESP-NOW peer added");
}
}
void loop() {
if (millis() - lastRecvTime > 500) isConnected = false;
// 方便現場除錯:每秒印一次連線/燈光狀態
static unsigned long lastLog = 0;
if (millis() - lastLog > 1000) {
Serial.print("[VEHICLE] link=");
Serial.print(isConnected ? "OK" : "--");
Serial.print(" mode=");
Serial.print(rxData.mode);
Serial.print(" light_bits=0x");
Serial.print(rxData.light_bits, HEX);
// LED buffer 狀態(用來判斷是硬體不亮,還是程式沒把 LED 設成亮)
uint16_t nonBlack = 0;
for (int i = 0; i < NUM_LEDS; i++) {
if (leds[i].r || leds[i].g || leds[i].b) nonBlack++;
}
Serial.print(" leds_on=");
Serial.print(nonBlack);
Serial.print(" led0=");
Serial.print(leds[0].r);
Serial.print(",");
Serial.print(leds[0].g);
Serial.print(",");
Serial.print(leds[0].b);
Serial.print(" led11=");
Serial.print(leds[11].r);
Serial.print(",");
Serial.print(leds[11].g);
Serial.print(",");
Serial.println(leds[11].b);
lastLog = millis();
}
updateMotion();
updateLights();
handleTelemetry();
delay(10);
}
// 遙控器端 Firmware (Lolin32 Lite)
#include <Adafruit_SSD1306.h>
#include <Arduino.h>
#include <WiFi.h>
#include <Wire.h>
#include <esp_now.h>
#include "common.h"
// --- MAC 設定 ---
uint8_t vehicleMac[] = {0xE0, 0xE2, 0xE6, 0xD0, 0xB9, 0xEC};
// --- 硬體定義 ---
#define PIN_LX 34
#define PIN_LY 35
#define PIN_RX 32
#define PIN_RY 33
#define PIN_POT 39
#define BTN_MODE 22
#define BTN_L 18
#define BTN_R 5
#define BTN_HAZ 17
#define BTN_HEAD 16
#define BTN_RES 4
#define I2C_SDA 23
#define I2C_SCL 19
// --- 全域變數 ---
ControlPacket txData;
TelemetryPacket rxTelem;
Adafruit_SSD1306 display(128, 64, &Wire, -1);
bool isCarConnected = false;
unsigned long lastTelemTime = 0;
// 按鈕狀態結構
struct Button {
int pin;
bool lastState;
bool toggleState;
};
Button btns[6] = {{BTN_MODE, HIGH, false}, {BTN_L, HIGH, false},
{BTN_R, HIGH, false}, {BTN_HAZ, HIGH, false},
{BTN_HEAD, HIGH, false}, {BTN_RES, HIGH, false}};
// --- 按鈕處理 ---
void updateButtons() {
for (int i = 0; i < 6; i++) {
bool reading = digitalRead(btns[i].pin);
if (reading == LOW && btns[i].lastState == HIGH) { // 按下瞬間
btns[i].toggleState = !btns[i].toggleState;
delay(20); // 簡單防彈跳
}
btns[i].lastState = reading;
}
// 互斥邏輯 (L/R)
if (digitalRead(btns[1].pin) == LOW)
btns[2].toggleState = false; // 按 L 關 R
if (digitalRead(btns[2].pin) == LOW)
btns[1].toggleState = false; // 按 R 關 L
}
// --- ESP-NOW Callback ---
void OnDataRecv(const uint8_t* mac, const uint8_t* incomingData, int len) {
memcpy(&rxTelem, incomingData, sizeof(rxTelem));
lastTelemTime = millis();
isCarConnected = true;
}
void setup() {
Serial.begin(115200);
// Pins
pinMode(BTN_MODE, INPUT_PULLUP);
pinMode(BTN_L, INPUT_PULLUP);
pinMode(BTN_R, INPUT_PULLUP);
pinMode(BTN_HAZ, INPUT_PULLUP);
pinMode(BTN_HEAD, INPUT_PULLUP);
pinMode(BTN_RES, INPUT_PULLUP);
// Display
Wire.begin(I2C_SDA, I2C_SCL);
if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) Serial.println("OLED Failed");
display.clearDisplay();
// WiFi
WiFi.mode(WIFI_STA);
WiFi.disconnect(false, true);
if (esp_now_init() != ESP_OK) return;
esp_now_register_recv_cb(OnDataRecv);
// Register Peer (Vehicle)
esp_now_peer_info_t peerInfo = {};
memcpy(peerInfo.peer_addr, vehicleMac, 6);
peerInfo.channel = 0;
peerInfo.encrypt = false;
peerInfo.ifidx = WIFI_IF_STA;
esp_err_t addStatus = esp_now_add_peer(&peerInfo);
if (addStatus != ESP_OK) {
Serial.print("esp_now_add_peer failed: ");
Serial.println(addStatus);
}
}
void loop() {
// 1. 讀取輸入
updateButtons();
// 2. 打包資料
// ADC(0~4095) -> 搖桿期望範圍(-512~512)
txData.lx = (analogRead(PIN_LX) - 2048) / 4;
txData.ly = (analogRead(PIN_LY) - 2048) / 4;
txData.rx = (analogRead(PIN_RX) - 2048) / 4;
txData.ry = (analogRead(PIN_RY) - 2048) / 4;
txData.pot = analogRead(PIN_POT); // 0-4095
// 死區修正
if (abs(txData.lx) < 40) txData.lx = 0;
if (abs(txData.ly) < 40) txData.ly = 0;
if (abs(txData.rx) < 40) txData.rx = 0;
if (abs(txData.ry) < 40) txData.ry = 0;
txData.mode = btns[0].toggleState ? 1 : 0; // 0=CAR, 1=ARM
txData.light_bits = 0;
if (btns[1].toggleState) txData.light_bits |= LIGHT_LEFT;
if (btns[2].toggleState) txData.light_bits |= LIGHT_RIGHT;
if (btns[3].toggleState) txData.light_bits |= LIGHT_HAZARD;
if (btns[4].toggleState) txData.light_bits |= LIGHT_HEAD;
if (btns[5].toggleState) txData.light_bits |= LIGHT_RESERVED;
// 3. 發送資料
esp_now_send(vehicleMac, (uint8_t*)&txData, sizeof(txData));
// 4. 連線狀態檢查
if (millis() - lastTelemTime > 1000) isCarConnected = false;
// 5. 更新螢幕
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.print("MODE: ");
display.println(txData.mode ? "ARM" : "CAR");
display.setCursor(64, 0);
display.print(isCarConnected ? "LINK: OK" : "LINK: --");
display.setCursor(0, 16);
display.print("RPM: ");
display.print(rxTelem.rpm, 0);
// 左右轉燈:移到 RPM 右側同行
display.setCursor(100, 16);
if (btns[1].toggleState)
display.print("<<");
else if (btns[2].toggleState)
display.print(">>");
else
display.print(" ");
// HEAD/HAZ + P:P 值移到 HAZ 右側
display.setCursor(0, 32);
display.print(btns[4].toggleState ? "[HEAD]" : "[ ]");
display.print(" ");
display.print(btns[3].toggleState ? "[HAZ]" : "[ ]");
display.print(" P:");
display.print(txData.pot);
// 下方兩行:分別顯示左右搖桿 XY
display.setCursor(0, 48);
display.print("Lx:");
display.print(txData.lx);
display.print(" Ly:");
display.print(txData.ly);
display.setCursor(0, 56);
display.print("Rx:");
display.print(txData.rx);
display.print(" Ry:");
display.print(txData.ry);
display.display();
delay(50); // Loop Rate ~20Hz
}
影片中展示了各項設計功能,遙控器的操作、車輛的機械結構等。
依 3D 建模輸出的檔案切片後列印出的成品。
以上學期的材料拆下後重複利用,以馬達帶動導輪帶動皮帶傳輸動力到輪軸。
以 NodeMCU-32s 開發板(中左上方)串接起各個電子零件。
轉向結構則使用遙控車常見的結構。
一大臂一小臂加上轉向底座的結構,前端還設計有一夾子(開發板毀損無法測試未實裝)。
以 LOLIN32 Lite 開發板為中心,接上一個 OLED 螢幕、兩個搖桿、一個可變電組及六顆按鍵。
胡朝凱:這次專題是延伸上次自平衡自行車的專題,整體一樣以搖桿控制遙控車前後左右,主要差別在於少了計算控制平衡的步驟,多了一個機械手臂。過程中都還算順利,測試階段遙控車和機械手臂也都可以正常作動,但可惜在收尾階段修改程式時,因為ESP32開發板莫名損壞,導致車輛和機械手臂無法操作,我們也沒有多餘的材料能夠更換,最後只能以半成品的方式結束這個專題。
林致宇:本專案以 ESP32 為核心,開發具備雙向通訊功能之機械手臂遙控車。透過 ESP-NOW 協定實現低延遲控制與即時數據回傳,並運用平滑演算法消除機械抖動。過程中克服底層通訊配置與電源管理等硬體挑戰,成功整合六軸手臂、馬達驅動與燈光系統。最終實現了精確操作與狀態監控功能,並深刻體悟嵌入式系統軟硬整合之實務精髓。雖然開發板突然壞了,但還是學到很多。