現(xiàn)教程)
做六軸機械臂項目做到中期以后大家問得最多的往往不是“怎么讓舵機轉(zhuǎn)起來”而是“怎樣讓它按照我想要的軌跡動起來”。如果要靠運動學逆解去編寫一段手腕翻轉(zhuǎn)、抓取、回位的組合動作對數(shù)學和代碼能力的要求都不低但如果采用示教軌跡復(fù)現(xiàn)的方式整個過程就變成“人拖動小機械臂做一遍單片機記下來再讓執(zhí)行臂重放”工程門檻會一下子低很多。這一期教程圍繞六自由度機械臂的示教操作與軌跡復(fù)現(xiàn)展開控制核心使用 STM32F103 單片機。文章會先講清楚整體架構(gòu)和硬件接線再給出可落地的 PWM 舵機驅(qū)動、ADC 角度采集、軌跡記錄和回放狀態(tài)機設(shè)計思路最后補充常見故障排查和工程注意事項。無論你是在做課程設(shè)計、畢業(yè)設(shè)計還是自己組一套六軸機械臂玩這套思路都可以直接遷移。1. 為什么六軸機械臂要做“示教操作 軌跡復(fù)現(xiàn)”很多剛開始接觸六軸機械臂的同學第一反應(yīng)是讓機械臂完成“從 A 點抓取放到 B 點”這樣的任務(wù)。要做到這一點常規(guī)思路是給機械臂建立 DH 參數(shù)模型。標定六個關(guān)節(jié)的零位和運動范圍。寫出正運動學把關(guān)節(jié)角度換算成末端位姿。再寫逆運動學把目標位姿換算成六個關(guān)節(jié)角。規(guī)劃軌跡生成每個時刻的關(guān)節(jié)位置。最后讓控制器逐點執(zhí)行。這套流程當然是最接近工業(yè)機器人開發(fā)的但放在 DIY 六軸機械臂上問題也很明顯結(jié)構(gòu)件加工誤差、舵機安裝偏差、減速箱回差都會讓理論模型和實際運動對不上。如果每一段動作都想靠代碼去“算”出來調(diào)試周期會非常長。示教操作則繞開了復(fù)雜的運動學推導。它的核心思想是由人直接拖動一套與執(zhí)行機械臂結(jié)構(gòu)相似的小機械臂各關(guān)節(jié)的角度傳感器實時檢測關(guān)節(jié)位置主控把傳感器數(shù)據(jù)轉(zhuǎn)換為舵機 PWM 信號驅(qū)動另一條機械臂同步跟隨。“拖動小機械臂”是人工示教比較常用的一種做法。好處有三個不需要逆解操作人員無需關(guān)心關(guān)節(jié)角度只要用手把教臂擺成目標姿態(tài)即可。動作直觀教臂轉(zhuǎn)多少執(zhí)行臂就按預(yù)設(shè)比例轉(zhuǎn)多少效果所見即所得。可以快速產(chǎn)生軌跡把示教過程中每個時刻的六個關(guān)節(jié)角度按固定時間間隔保存下來就能形成一條完整軌跡需要復(fù)現(xiàn)時單片機按同樣的時間間隔把角度點回放出來即可。因此本文要做的事情并不復(fù)雜讀取教臂角度、實時驅(qū)動執(zhí)行臂、定時保存軌跡點、按記錄順序回放。這正好是 STM32F103 這類資源不多、但控制能力足夠的 MCU 所擅長的任務(wù)。2. 整體方案與工作流程2.1 系統(tǒng)由哪幾部分組成整套系統(tǒng)可以劃分為四個部分模塊作用典型器件示教小機械臂人手拖動提供六個關(guān)節(jié)的目標角度六軸串聯(lián)結(jié)構(gòu)各關(guān)節(jié)安裝角度傳感器角度采集電路把機械角度變成 MCU 可讀的電信號旋轉(zhuǎn)電位器或 AS5600 等磁編碼器主控板讀取角度、處理按鍵、記錄和回放軌跡STM32F103C8T6 最小系統(tǒng)板執(zhí)行機械臂按主控給出的 PWM 信號運動六個 PWM 數(shù)字舵機 結(jié)構(gòu)件這里要說明一下很多同學會問為什么不讓執(zhí)行臂直接被手拖動對于普通舵機驅(qū)動的機械臂舵機內(nèi)部有減速齒輪組通電后反向拖動很容易導致舵機堵轉(zhuǎn)、發(fā)熱甚至燒毀。因此更安全的 DIY 示教方案是采用“主從式”結(jié)構(gòu)人拖動的是無動力或輕負載的教臂執(zhí)行臂只在復(fù)現(xiàn)階段承擔運動任務(wù)。如果想讓執(zhí)行臂本身支持直接拖動示教需要特殊設(shè)計的電機抱閘釋放機構(gòu)和絕對值角度反饋復(fù)雜度會高很多。2.2 數(shù)據(jù)流向用一句話概括系統(tǒng)流程人手拖動教臂 - 六個關(guān)節(jié)電位器產(chǎn)生模擬電壓 - STM32F103 的 ADC 采樣 - 轉(zhuǎn)換為舵機脈寬 - 輸出 PWM 驅(qū)動執(zhí)行臂 - 同時按時間間隔保存當前角度 - 按鍵切換后按保存順序回放在實時跟隨模式下STM32F103 每 20ms 左右讀取一次六路 ADC 值并把這組值映射成 PWM 脈寬輸出。這個循環(huán)不斷重復(fù)看起來就是執(zhí)行臂跟隨教臂同步運動。在記錄模式下單片機除了實時跟隨還會把每一次的 PWM 脈寬保存到軌跡數(shù)組中。停止記錄后數(shù)組里保存的就是這段時間的完整動作序列。在回放模式下單片機從軌跡數(shù)組第一個點開始每隔固定時間間隔輸出一組 PWM 脈寬直到全部回放結(jié)束。3. 本期硬件組裝與接線要點3.1 材料清單第二期既然要接入示教與復(fù)現(xiàn)功能硬件上需要增加“教臂”部分。由于材料包的螺絲、鋁件尺寸不一樣下面只給出通用清單類別說明執(zhí)行機械臂結(jié)構(gòu)底座、腰部、大臂、小臂、腕部等六軸結(jié)構(gòu)件教臂結(jié)構(gòu)件與執(zhí)行臂關(guān)節(jié)數(shù)量一致的小型串聯(lián)臂可以用 3D 打印也可以用型材搭建角度傳感器每個教臂關(guān)節(jié)一個旋轉(zhuǎn)電位器共 6 個舵機每個執(zhí)行關(guān)節(jié)一個 PWM 舵機共 6 個主控板STM32F103C8T6 最小系統(tǒng)板或類似兼容板電源舵機獨立電源例如 2S 鋰電池加穩(wěn)壓模塊5V 主控供電按鍵與指示燈用于切換“記錄 / 回放 / 停止”接線材料杜邦線或者排線、銅柱、扎帶等如果你手里的執(zhí)行臂是“總線舵機”版本則舵機接口不再是普通 PWM通信協(xié)議不同本文示例代碼需要對應(yīng)替換以下內(nèi)容以最適合入門學習的 PWM 舵機為例。3.2 示教臂角度傳感器裝配對于 DIY 主從示教旋轉(zhuǎn)電位器是最容易獲取的角度傳感器。裝配時有幾點需要注意電位器要和關(guān)節(jié)旋轉(zhuǎn)軸同心否則轉(zhuǎn)動時會憋住。每個關(guān)節(jié)最好有機械限位避免電位器超過有效電氣轉(zhuǎn)角。電位器兩端分別接 3.3V 和 GND中間抽頭接 STM32 的 ADC 引腳。如果某個關(guān)節(jié)方向反了可以在軟件里做反向映射不一定要重新改結(jié)構(gòu)。在功能上教臂并不需要很大的力矩關(guān)節(jié)轉(zhuǎn)動越順滑越好。如果結(jié)構(gòu)件轉(zhuǎn)動不順暢首先檢查軸承或軸套的安裝同軸度。3.3 執(zhí)行臂舵機與供電六自由度機械臂的六個舵機在啟動和換向瞬間電流較大最忌諱的是用 STM32 最小系統(tǒng)板的 USB 口直接給舵機供電。更合理的供電方式是電池或電源適配器 - 穩(wěn)壓/降壓模塊 - 舵機電源正負極 - 舵機信號線接 STM32 PWM 引腳還需要特別注意舵機電源地、STM32 系統(tǒng)板地必須共地。很多“舵機不轉(zhuǎn)、舵機亂抖、單片機復(fù)位”的問題本質(zhì)都是沒有共地。舵機的信號線是高電平脈沖信號STM32 的 PWM 引腳輸出就可以驅(qū)動。接線時不需要把所有舵機電源并到主控板上正確的電源走線是直接從電源模塊分出來再讓主控板單獨取電。3.4 引腳規(guī)劃與避免 IO 沖突STM32F103C8T6 的引腳資源有限。如果六個舵機 PWM 和六路 ADC 都要占用引腳直接隨便接很容易沖突。一個比較直觀的分配方案如下功能引腳對應(yīng)外設(shè)舵機 0~3 PWMPA0、PA1、PA2、PA3TIM2_CH1~CH4舵機 4、5 PWMPA8、PA9TIM1_CH1、TIM1_CH2教臂 ADC 通道 0~5PA4、PA5、PA6、PA7、PB0、PB1ADC1_IN4~IN9這樣安排后PWM 引腳和 ADC 引腳不重疊六個舵機和六路電位器都能接入。如果你手里的 PCB 已經(jīng)定義好其他引腳以你手上的原理圖為準重點是把引腳配置集中到宏定義或配置文件中方便后續(xù)調(diào)整。4. STM32F103 核心驅(qū)動代碼解析4.1 舵機 PWM 控制原理常見的模擬或數(shù)字舵機控制信號是周期約 20ms 的脈沖。脈沖寬度不同舵機輸出的角度不同脈寬典型角度0.5ms0° 附近1.5ms中間位置2.5ms最大角度附近不同廠家舵機的脈寬范圍有差異有的實際范圍是 0.5ms~2.5ms有的只有 1ms~2ms。建議先查舵機數(shù)據(jù)手冊然后在代碼里把范圍定義成宏方便標定。在 STM32CubeMX 中將定時器時鐘配置成 72MHz預(yù)分頻器設(shè)為 71則計數(shù)器頻率為72MHz / 72 1MHz也就是計數(shù)器每計數(shù)一次為 1 微秒。如果把自動重載值 ARR 設(shè)為 19999則 PWM 周期為(19999 1) * 1us 20000us 20ms這正好符合舵機的 20ms 周期要求。此時設(shè)置 CCR 比較值CCR 值脈寬500500us15001500us25002500us因此代碼里不需要寫很復(fù)雜的數(shù)學公式直接“往 CCR 寄存器寫入微秒值”即可。4.2 六路舵機 PWM 輸出代碼假設(shè)你在 STM32CubeMX 中已經(jīng)生成了工程定時器句柄分別是htim1和htim2?？梢詫懸粋€統(tǒng)一的舵機設(shè)置接口// servo.c #include main.h #include servo.h #define PULSE_MIN_US 500 #define PULSE_MAX_US 2500 void Servo_SetPulse(uint8_t idx, uint16_t pulse_us) { if (pulse_us PULSE_MIN_US) pulse_us PULSE_MIN_US; if (pulse_us PULSE_MAX_US) pulse_us PULSE_MAX_US; switch (idx) { case 0: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_1, pulse_us); break; case 1: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_2, pulse_us); break; case 2: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_3, pulse_us); break; case 3: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_4, pulse_us); break; case 4: __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, pulse_us); break; case 5: __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_2, pulse_us); break; default: break; } } void Servo_InitAll(void) { HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_2); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_4); /* TIM1 是高級定時器需要使能主輸出 */ __HAL_TIM_MOE_ENABLE(htim1); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_2); for (uint8_t i 0; i 6; i) { Servo_SetPulse(i, 1500); } }代碼里的“1500”表示舵機先回到 1.5ms 脈寬中位。實際項目中如果機械臂結(jié)構(gòu)安裝時零位并不在舵機中位這里需要按你的機械零位修改。4.3 六路 ADC 角度讀取如果教臂使用電位器ST 的 ADC1 可以依次掃描多個通道。為了減少抖動我習慣對每個通道連續(xù)讀幾次然后取平均。下面是一個輕量實現(xiàn)// teach_adc.c #include main.h #include teach_adc.h static uint16_t adc_channel_map[6] { ADC_CHANNEL_4, // PA4 ADC_CHANNEL_5, // PA5 ADC_CHANNEL_6, // PA6 ADC_CHANNEL_7, // PA7 ADC_CHANNEL_8, // PB0 ADC_CHANNEL_9 // PB1 }; uint16_t ReadOneChannel(uint16_t channel) { ADC_ChannelConfTypeDef sConfig {0}; uint16_t value 0; sConfig.Channel channel; sConfig.Rank 1; sConfig.SamplingTime ADC_SAMPLETIME_239CYCLES_5; HAL_ADC_ConfigChannel(hadc1, sConfig); HAL_ADC_Start(hadc1); HAL_ADC_PollForConversion(hadc1, 10); value HAL_ADC_GetValue(hadc1); HAL_ADC_Stop(hadc1); return value; } void TeachArm_ReadAll(uint16_t *raw) { uint32_t sum 0; for (uint8_t ch 0; ch 6; ch) { sum 0; for (uint8_t sample 0; sample 5; sample) { sum ReadOneChannel(adc_channel_map[ch]); } raw[ch] sum / 5; } }這里之所以每讀一個通道都執(zhí)行HAL_ADC_Stop()是為了讓下一次切換通道時不要沿用上一次的配置。在 6 個電位器、采樣率要求不高的場景下這種寫法簡單可靠。5. 示教、記錄與回放軟件設(shè)計5.1 狀態(tài)機設(shè)計機械臂示教不只是一個“循環(huán)讀角度”的過程還需要考慮“什么時候開始記錄”“什么時候停止”“什么時候回到軌跡首點”“什么時候開始回放”。使用狀態(tài)機會清晰很多?？梢远x這樣幾種模式模式含義MODE_FOLLOW實時跟隨教臂運動但不記錄MODE_RECORD實時跟隨同時按周期保存軌跡點MODE_BACK_TO_START從當前姿態(tài)平滑回到軌跡首點MODE_PLAYBACK逐點回放軌跡按鈕的功能可以這樣設(shè)計按鍵 1進入記錄。如果已經(jīng)在記錄則停止記錄并進入“待機”或“跟隨”模式。按鍵 2開始回放。如果采集到的軌跡點數(shù)為 0則按下無效。按鍵 3急?；驈?fù)位所有舵機輸出中位脈寬。采用這種劃分后主循環(huán)的邏輯會很清楚。5.2 軌跡數(shù)據(jù)結(jié)構(gòu)使用 STM32F103C8T6 時RAM 只有約 20KB。如果六個關(guān)節(jié)都保存為uint16_t每個軌跡點需要6 * 2 12 字節(jié)如果保存 800 個點總?cè)萘繛?9600 字節(jié)約占 RAM 的一半。示例中先按這個規(guī)模設(shè)計// trajectory.h #define SERVO_NUM 6 #define MAX_TRAJ_POINT 800 typedef struct { uint16_t pulse[SERVO_NUM]; } TrajPoint; extern TrajPoint g_traj[MAX_TRAJ_POINT]; extern volatile uint16_t g_traj_len;當然800 個點通常不夠保存幾分鐘的長軌跡。如果希望斷電保存或保存更長時間可以外接 SPI Flash、SD 卡或者用串口把數(shù)據(jù)發(fā)送到上位機保存。這里先把 RAM 方案跑通再考慮擴展存儲。5.3 實時跟隨與軌跡記錄下面這段代碼是核心循環(huán)邏輯// main.c 中的循環(huán)片段 void MainLoop_Tick(void) { static uint32_t last_record_tick 0; static uint32_t last_follow_tick 0; uint32_t now HAL_GetTick(); if (g_mode MODE_FOLLOW || g_mode MODE_RECORD) { if (now - last_follow_tick 20) { uint16_t raw[6]; TeachArm_ReadAll(raw); for (uint8_t i 0; i 6; i) { g_current_pulse[i] MapRawToPulse(i, raw[i]); Servo_SetPulse(i, g_current_pulse[i]); } last_follow_tick now; } } if (g_mode MODE_RECORD) { if (now - last_record_tick STM32六軸機械臂示教操作與軌跡復(fù)現(xiàn)教程
做六軸機械臂項目做到中期以后大家問得最多的往往不是“怎么讓舵機轉(zhuǎn)起來”而是“怎樣讓它按照我想要的軌跡動起來”。如果要靠運動學逆解去編寫一段手腕翻轉(zhuǎn)、抓取、回位的組合動作對數(shù)學和代碼能力的要求都不低但如果采用示教軌跡復(fù)現(xiàn)的方式整個過程就變成“人拖動小機械臂做一遍單片機記下來再讓執(zhí)行臂重放”工程門檻會一下子低很多。這一期教程圍繞六自由度機械臂的示教操作與軌跡復(fù)現(xiàn)展開控制核心使用 STM32F103 單片機。文章會先講清楚整體架構(gòu)和硬件接線再給出可落地的 PWM 舵機驅(qū)動、ADC 角度采集、軌跡記錄和回放狀態(tài)機設(shè)計思路最后補充常見故障排查和工程注意事項。無論你是在做課程設(shè)計、畢業(yè)設(shè)計還是自己組一套六軸機械臂玩這套思路都可以直接遷移。1. 為什么六軸機械臂要做“示教操作 軌跡復(fù)現(xiàn)”很多剛開始接觸六軸機械臂的同學第一反應(yīng)是讓機械臂完成“從 A 點抓取放到 B 點”這樣的任務(wù)。要做到這一點常規(guī)思路是給機械臂建立 DH 參數(shù)模型。標定六個關(guān)節(jié)的零位和運動范圍。寫出正運動學把關(guān)節(jié)角度換算成末端位姿。再寫逆運動學把目標位姿換算成六個關(guān)節(jié)角。規(guī)劃軌跡生成每個時刻的關(guān)節(jié)位置。最后讓控制器逐點執(zhí)行。這套流程當然是最接近工業(yè)機器人開發(fā)的但放在 DIY 六軸機械臂上問題也很明顯結(jié)構(gòu)件加工誤差、舵機安裝偏差、減速箱回差都會讓理論模型和實際運動對不上。如果每一段動作都想靠代碼去“算”出來調(diào)試周期會非常長。示教操作則繞開了復(fù)雜的運動學推導。它的核心思想是由人直接拖動一套與執(zhí)行機械臂結(jié)構(gòu)相似的小機械臂各關(guān)節(jié)的角度傳感器實時檢測關(guān)節(jié)位置主控把傳感器數(shù)據(jù)轉(zhuǎn)換為舵機 PWM 信號驅(qū)動另一條機械臂同步跟隨。“拖動小機械臂”是人工示教比較常用的一種做法。好處有三個不需要逆解操作人員無需關(guān)心關(guān)節(jié)角度只要用手把教臂擺成目標姿態(tài)即可。動作直觀教臂轉(zhuǎn)多少執(zhí)行臂就按預(yù)設(shè)比例轉(zhuǎn)多少效果所見即所得。可以快速產(chǎn)生軌跡把示教過程中每個時刻的六個關(guān)節(jié)角度按固定時間間隔保存下來就能形成一條完整軌跡需要復(fù)現(xiàn)時單片機按同樣的時間間隔把角度點回放出來即可。因此本文要做的事情并不復(fù)雜讀取教臂角度、實時驅(qū)動執(zhí)行臂、定時保存軌跡點、按記錄順序回放。這正好是 STM32F103 這類資源不多、但控制能力足夠的 MCU 所擅長的任務(wù)。2. 整體方案與工作流程2.1 系統(tǒng)由哪幾部分組成整套系統(tǒng)可以劃分為四個部分模塊作用典型器件示教小機械臂人手拖動提供六個關(guān)節(jié)的目標角度六軸串聯(lián)結(jié)構(gòu)各關(guān)節(jié)安裝角度傳感器角度采集電路把機械角度變成 MCU 可讀的電信號旋轉(zhuǎn)電位器或 AS5600 等磁編碼器主控板讀取角度、處理按鍵、記錄和回放軌跡STM32F103C8T6 最小系統(tǒng)板執(zhí)行機械臂按主控給出的 PWM 信號運動六個 PWM 數(shù)字舵機 結(jié)構(gòu)件這里要說明一下很多同學會問為什么不讓執(zhí)行臂直接被手拖動對于普通舵機驅(qū)動的機械臂舵機內(nèi)部有減速齒輪組通電后反向拖動很容易導致舵機堵轉(zhuǎn)、發(fā)熱甚至燒毀。因此更安全的 DIY 示教方案是采用“主從式”結(jié)構(gòu)人拖動的是無動力或輕負載的教臂執(zhí)行臂只在復(fù)現(xiàn)階段承擔運動任務(wù)。如果想讓執(zhí)行臂本身支持直接拖動示教需要特殊設(shè)計的電機抱閘釋放機構(gòu)和絕對值角度反饋復(fù)雜度會高很多。2.2 數(shù)據(jù)流向用一句話概括系統(tǒng)流程人手拖動教臂 - 六個關(guān)節(jié)電位器產(chǎn)生模擬電壓 - STM32F103 的 ADC 采樣 - 轉(zhuǎn)換為舵機脈寬 - 輸出 PWM 驅(qū)動執(zhí)行臂 - 同時按時間間隔保存當前角度 - 按鍵切換后按保存順序回放在實時跟隨模式下STM32F103 每 20ms 左右讀取一次六路 ADC 值并把這組值映射成 PWM 脈寬輸出。這個循環(huán)不斷重復(fù)看起來就是執(zhí)行臂跟隨教臂同步運動。在記錄模式下單片機除了實時跟隨還會把每一次的 PWM 脈寬保存到軌跡數(shù)組中。停止記錄后數(shù)組里保存的就是這段時間的完整動作序列。在回放模式下單片機從軌跡數(shù)組第一個點開始每隔固定時間間隔輸出一組 PWM 脈寬直到全部回放結(jié)束。3. 本期硬件組裝與接線要點3.1 材料清單第二期既然要接入示教與復(fù)現(xiàn)功能硬件上需要增加“教臂”部分。由于材料包的螺絲、鋁件尺寸不一樣下面只給出通用清單類別說明執(zhí)行機械臂結(jié)構(gòu)底座、腰部、大臂、小臂、腕部等六軸結(jié)構(gòu)件教臂結(jié)構(gòu)件與執(zhí)行臂關(guān)節(jié)數(shù)量一致的小型串聯(lián)臂可以用 3D 打印也可以用型材搭建角度傳感器每個教臂關(guān)節(jié)一個旋轉(zhuǎn)電位器共 6 個舵機每個執(zhí)行關(guān)節(jié)一個 PWM 舵機共 6 個主控板STM32F103C8T6 最小系統(tǒng)板或類似兼容板電源舵機獨立電源例如 2S 鋰電池加穩(wěn)壓模塊5V 主控供電按鍵與指示燈用于切換“記錄 / 回放 / 停止”接線材料杜邦線或者排線、銅柱、扎帶等如果你手里的執(zhí)行臂是“總線舵機”版本則舵機接口不再是普通 PWM通信協(xié)議不同本文示例代碼需要對應(yīng)替換以下內(nèi)容以最適合入門學習的 PWM 舵機為例。3.2 示教臂角度傳感器裝配對于 DIY 主從示教旋轉(zhuǎn)電位器是最容易獲取的角度傳感器。裝配時有幾點需要注意電位器要和關(guān)節(jié)旋轉(zhuǎn)軸同心否則轉(zhuǎn)動時會憋住。每個關(guān)節(jié)最好有機械限位避免電位器超過有效電氣轉(zhuǎn)角。電位器兩端分別接 3.3V 和 GND中間抽頭接 STM32 的 ADC 引腳。如果某個關(guān)節(jié)方向反了可以在軟件里做反向映射不一定要重新改結(jié)構(gòu)。在功能上教臂并不需要很大的力矩關(guān)節(jié)轉(zhuǎn)動越順滑越好。如果結(jié)構(gòu)件轉(zhuǎn)動不順暢首先檢查軸承或軸套的安裝同軸度。3.3 執(zhí)行臂舵機與供電六自由度機械臂的六個舵機在啟動和換向瞬間電流較大最忌諱的是用 STM32 最小系統(tǒng)板的 USB 口直接給舵機供電。更合理的供電方式是電池或電源適配器 - 穩(wěn)壓/降壓模塊 - 舵機電源正負極 - 舵機信號線接 STM32 PWM 引腳還需要特別注意舵機電源地、STM32 系統(tǒng)板地必須共地。很多“舵機不轉(zhuǎn)、舵機亂抖、單片機復(fù)位”的問題本質(zhì)都是沒有共地。舵機的信號線是高電平脈沖信號STM32 的 PWM 引腳輸出就可以驅(qū)動。接線時不需要把所有舵機電源并到主控板上正確的電源走線是直接從電源模塊分出來再讓主控板單獨取電。3.4 引腳規(guī)劃與避免 IO 沖突STM32F103C8T6 的引腳資源有限。如果六個舵機 PWM 和六路 ADC 都要占用引腳直接隨便接很容易沖突。一個比較直觀的分配方案如下功能引腳對應(yīng)外設(shè)舵機 0~3 PWMPA0、PA1、PA2、PA3TIM2_CH1~CH4舵機 4、5 PWMPA8、PA9TIM1_CH1、TIM1_CH2教臂 ADC 通道 0~5PA4、PA5、PA6、PA7、PB0、PB1ADC1_IN4~IN9這樣安排后PWM 引腳和 ADC 引腳不重疊六個舵機和六路電位器都能接入。如果你手里的 PCB 已經(jīng)定義好其他引腳以你手上的原理圖為準重點是把引腳配置集中到宏定義或配置文件中方便后續(xù)調(diào)整。4. STM32F103 核心驅(qū)動代碼解析4.1 舵機 PWM 控制原理常見的模擬或數(shù)字舵機控制信號是周期約 20ms 的脈沖。脈沖寬度不同舵機輸出的角度不同脈寬典型角度0.5ms0° 附近1.5ms中間位置2.5ms最大角度附近不同廠家舵機的脈寬范圍有差異有的實際范圍是 0.5ms~2.5ms有的只有 1ms~2ms。建議先查舵機數(shù)據(jù)手冊然后在代碼里把范圍定義成宏方便標定。在 STM32CubeMX 中將定時器時鐘配置成 72MHz預(yù)分頻器設(shè)為 71則計數(shù)器頻率為72MHz / 72 1MHz也就是計數(shù)器每計數(shù)一次為 1 微秒。如果把自動重載值 ARR 設(shè)為 19999則 PWM 周期為(19999 1) * 1us 20000us 20ms這正好符合舵機的 20ms 周期要求。此時設(shè)置 CCR 比較值CCR 值脈寬500500us15001500us25002500us因此代碼里不需要寫很復(fù)雜的數(shù)學公式直接“往 CCR 寄存器寫入微秒值”即可。4.2 六路舵機 PWM 輸出代碼假設(shè)你在 STM32CubeMX 中已經(jīng)生成了工程定時器句柄分別是htim1和htim2?？梢詫懸粋€統(tǒng)一的舵機設(shè)置接口// servo.c #include main.h #include servo.h #define PULSE_MIN_US 500 #define PULSE_MAX_US 2500 void Servo_SetPulse(uint8_t idx, uint16_t pulse_us) { if (pulse_us PULSE_MIN_US) pulse_us PULSE_MIN_US; if (pulse_us PULSE_MAX_US) pulse_us PULSE_MAX_US; switch (idx) { case 0: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_1, pulse_us); break; case 1: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_2, pulse_us); break; case 2: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_3, pulse_us); break; case 3: __HAL_TIM_SET_COMPARE(htim2, TIM_CHANNEL_4, pulse_us); break; case 4: __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, pulse_us); break; case 5: __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_2, pulse_us); break; default: break; } } void Servo_InitAll(void) { HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_2); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(htim2, TIM_CHANNEL_4); /* TIM1 是高級定時器需要使能主輸出 */ __HAL_TIM_MOE_ENABLE(htim1); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_1); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_2); for (uint8_t i 0; i 6; i) { Servo_SetPulse(i, 1500); } }代碼里的“1500”表示舵機先回到 1.5ms 脈寬中位。實際項目中如果機械臂結(jié)構(gòu)安裝時零位并不在舵機中位這里需要按你的機械零位修改。4.3 六路 ADC 角度讀取如果教臂使用電位器ST 的 ADC1 可以依次掃描多個通道。為了減少抖動我習慣對每個通道連續(xù)讀幾次然后取平均。下面是一個輕量實現(xiàn)// teach_adc.c #include main.h #include teach_adc.h static uint16_t adc_channel_map[6] { ADC_CHANNEL_4, // PA4 ADC_CHANNEL_5, // PA5 ADC_CHANNEL_6, // PA6 ADC_CHANNEL_7, // PA7 ADC_CHANNEL_8, // PB0 ADC_CHANNEL_9 // PB1 }; uint16_t ReadOneChannel(uint16_t channel) { ADC_ChannelConfTypeDef sConfig {0}; uint16_t value 0; sConfig.Channel channel; sConfig.Rank 1; sConfig.SamplingTime ADC_SAMPLETIME_239CYCLES_5; HAL_ADC_ConfigChannel(hadc1, sConfig); HAL_ADC_Start(hadc1); HAL_ADC_PollForConversion(hadc1, 10); value HAL_ADC_GetValue(hadc1); HAL_ADC_Stop(hadc1); return value; } void TeachArm_ReadAll(uint16_t *raw) { uint32_t sum 0; for (uint8_t ch 0; ch 6; ch) { sum 0; for (uint8_t sample 0; sample 5; sample) { sum ReadOneChannel(adc_channel_map[ch]); } raw[ch] sum / 5; } }這里之所以每讀一個通道都執(zhí)行HAL_ADC_Stop()是為了讓下一次切換通道時不要沿用上一次的配置。在 6 個電位器、采樣率要求不高的場景下這種寫法簡單可靠。5. 示教、記錄與回放軟件設(shè)計5.1 狀態(tài)機設(shè)計機械臂示教不只是一個“循環(huán)讀角度”的過程還需要考慮“什么時候開始記錄”“什么時候停止”“什么時候回到軌跡首點”“什么時候開始回放”。使用狀態(tài)機會清晰很多?？梢远x這樣幾種模式模式含義MODE_FOLLOW實時跟隨教臂運動但不記錄MODE_RECORD實時跟隨同時按周期保存軌跡點MODE_BACK_TO_START從當前姿態(tài)平滑回到軌跡首點MODE_PLAYBACK逐點回放軌跡按鈕的功能可以這樣設(shè)計按鍵 1進入記錄。如果已經(jīng)在記錄則停止記錄并進入“待機”或“跟隨”模式。按鍵 2開始回放。如果采集到的軌跡點數(shù)為 0則按下無效。按鍵 3急?；驈?fù)位所有舵機輸出中位脈寬。采用這種劃分后主循環(huán)的邏輯會很清楚。5.2 軌跡數(shù)據(jù)結(jié)構(gòu)使用 STM32F103C8T6 時RAM 只有約 20KB。如果六個關(guān)節(jié)都保存為uint16_t每個軌跡點需要6 * 2 12 字節(jié)如果保存 800 個點總?cè)萘繛?9600 字節(jié)約占 RAM 的一半。示例中先按這個規(guī)模設(shè)計// trajectory.h #define SERVO_NUM 6 #define MAX_TRAJ_POINT 800 typedef struct { uint16_t pulse[SERVO_NUM]; } TrajPoint; extern TrajPoint g_traj[MAX_TRAJ_POINT]; extern volatile uint16_t g_traj_len;當然800 個點通常不夠保存幾分鐘的長軌跡。如果希望斷電保存或保存更長時間可以外接 SPI Flash、SD 卡或者用串口把數(shù)據(jù)發(fā)送到上位機保存。這里先把 RAM 方案跑通再考慮擴展存儲。5.3 實時跟隨與軌跡記錄下面這段代碼是核心循環(huán)邏輯// main.c 中的循環(huán)片段 void MainLoop_Tick(void) { static uint32_t last_record_tick 0; static uint32_t last_follow_tick 0; uint32_t now HAL_GetTick(); if (g_mode MODE_FOLLOW || g_mode MODE_RECORD) { if (now - last_follow_tick 20) { uint16_t raw[6]; TeachArm_ReadAll(raw); for (uint8_t i 0; i 6; i) { g_current_pulse[i] MapRawToPulse(i, raw[i]); Servo_SetPulse(i, g_current_pulse[i]); } last_follow_tick now; } } if (g_mode MODE_RECORD) { if (now - last_record_tick