2021-工訓搬運機器人-行進方案(步進電機+麥克納姆輪)
備賽2021工訓物流搬運小車許久，和其他組不同，我們組採用
步進電機+麥克納姆輪
的驅動方案，就圖一個簡單。（
步進電機精度高，無需調編碼器，無需PID
）
秉承以往文章的意志，一方面用於比賽記錄，一方面希望能給小白提供一套參考方案，以期拋磚引玉。
一、方案選擇
工訓2021物流搬運機器人賽項的地圖如下所示，小車的
行進方向主要為前後左右，以及左上左下右上右下。
為實現小車在各個方向上的快速切換與移動，
麥克納姆輪或全向輪
皆為不錯的選擇。其中麥克納姆輪的使用更為常見，遂選用兩對麥克納姆輪。
二、麥克納姆輪安裝使用
麥輪是個神奇的傢伙，可以
透過控制一組麥輪中若干麥輪的轉向，實現整體行進方向的改變。
我們選擇常見的長方形安裝方式，長方形的四個角各一個麥輪，
麥輪旋向為“X”
，如下圖所示。
相同旋向的麥輪對角安裝。“X”
我們透過
控制
上圖所示
AXYZ四處麥輪的轉向（即對應電機的正反轉）
，即可實現小車整體行進方向的改變。
四處麥輪的轉向與整體行進方向的關係參考下圖。
具體安裝方式可自行選擇，轉向和行進方向的關係不變。
小車整體 前/後/左/右方向行進時，麥輪的轉向。 （基本行進方向）
前後左右方向行進時，麥輪的轉向。
2. 小車整體 順時針/逆時針行進時，麥輪的轉向。 （巡線/矯正車姿常用）
3. 小車整體 左上/左下/右上/右下行進時，麥輪的轉向。 （小車走斜線更快）
三、步進電機驅動控制
為實現上述多方向的運動，離不開對四個步進電機的運動控制。
1、步進電機驅動模組
1）如果要求一般可選用 A4988/DRV8825 。
效果：噪聲明顯，整車極限速度中低檔（帶散熱片）。
其中drv8825效果明顯優於a4988。
此處的極限速度：約
10kg
小車的跑完
賽道2趟
，驅動晶片發熱情況還能接受——稍微有些燙手
我們的小車測試時，使用drv8825的極限速度是a4988的一倍多。
（驅動額定電流1.7a的42步進電機）
tb可買的驅動板如下圖所示。
這裡放上
a4988/drv8825調電流
、以及
簡化版的4路驅動板
的參考文章：
Poao：42步進電機驅動—A4988/DRV8825調節
Poao：四路步進電機驅動板（工訓小車）— A4988/DRV8825
2）EXTEND:
如果要求較高，想要那種
靜音
的驅動模組，推薦
TMC2208
（驅動能力弱了點）；如果希望
靜音而且驅動能力強
，那就選擇
TMC2209。
2、步進電機控制程式
步進電機的驅動程式，很大部分我也是參考的別人的。只是在小車整體的行進方向上，需要進行一些方向修正。
這裡我將把步進電機驅動的程式
，籠統的過一遍，方便各位快速的有一個概念。
1）太極創客官方的教程
2）當時參考的csdn上一位博主的程式
後文的意義其實也就在於，
帶各位快速理解這位博主步進電機驅動的程式
。 而無需硬啃半天。（俺當初）
tips：
四個電機以 X Y Z A 表示，安裝位置同上文。
程式中的變數名、函式等內容中包含的XYZA與對應電機掛鉤。
a、步進電機控制宏定義
1) 控制每個電機，需要1個DIR 1個STEP
2) 此外，還需1個控制四個電機的 EN
#include
/*————————————————————————*
* 步進電機埠宏定義（2*4杜邦線）
*————————————————————————*/
// 奇數為 STEP 偶數為 Dir
#define En_Pin 40
// 步進電機使能端
#define X_Step 32
// X 電機步進引腳
#define X_Dir 33
// X 電機步方向進引腳
#define Y_Step 34
#define Y_Dir 35
#define Z_Step 36
#define Z_Dir 37
#define A_Step 38
#define A_Dir 39
/*——————————————————————*
* 步進電機控制物件定義
*——————————————————————*/
AccelStepper
motora
（
1
，
A_Step
，
A_Dir
）；
//定義步進電機控制物件a
AccelStepper
motorx
（
1
，
X_Step
，
X_Dir
）；
AccelStepper
motory
（
1
，
Y_Step
，
Y_Dir
）；
AccelStepper
motorz
（
1
，
Z_Step
，
Z_Dir
）；
b、步進電機引數定義
由於該控制程式實現的
效果
是
：
【
直接控制小車往指定方向運動指定距離】。
所以，需要進行相關引數的修正（根據自己的實際情況來）。註釋都挺詳細的，應該都可以看明白。
需要修正： 車輪直徑、驅動板的實際細分數 與 對應的一圈所需脈衝數。
/*——————————————————————*
* 步進電機引數定義
*——————————————————————*/
// 全步進- 1。8° | 半步進- 0。9° | 1/4步進- 0。45° | 1/8步進- 0。225° —— 配置跳線帽
// 一圈： 200步 | 400步 | 800步 | 1600步
#define D_Wheel 0。0762
//車輪的直徑 單位：m （三英寸麥輪=7。62cm）
#define AllStep 200
//步進電機全步進一圈步數為200
//#define MicroStep 4 //步進電機細分數（短接帽設定）
//#define TotalStep 800 //4細分下每圈步數：1600步
#define MicroStep 8
//步進電機細分數（短接帽設定）
#define TotalStep 1600
//8細分下每圈步數：1600步
// 下邊的都不用管，保留即可。
#define ClockWise HIGH
//順時針轉動
#define AntiClockWise LOW
//逆時針轉動
#define NotSendPluse false
//不傳送脈衝
#define SendPluse true
//傳送脈衝
#define StartUse true
//開始啟用
#define StopUse false
//停止啟用步進電機
/*——————————————————————-*
* 比率變數的設定
*——————————————————————-*/
const
double
C_Wheel
=
M_PI
*
D_Wheel
；
//車輪的周長 單位：m （M_PI——3。1415。。。）
const
double
StepOfMeter
=
TotalStep
/
C_Wheel
；
// 8細分時 步數/米
const
double
CarMaxSpeed
=
6000
；
// 最大步進速度
const
double
KMaxFre
=
6000
；
// 每秒最大步數 單位：步/秒
// 下邊這個俺沒用。
// 暫未明白該變數 設取 v=wr ，取angle_l = r = 410 mm = 0。410 m 90？
const
double
angle_L
=
0。410
/
90
；
// MAYBE？
// const double angle_L=0。323/90； // 將設定的小車線速度轉化為整體角速度
c、步進電機移動方向宏定義
/*——————————————————————*
* 步進電機移動方向宏定義
*——————————————————————*/
#define MFord 1
//移動方向 ：前、後、左、右
#define MBack 2
#define MLeft 3
#define MRight 4
#define RLeft 5
// 用於尋跡時修正方向
#define RRight 6
#define MZX 7
// 對角線方向走 // 右上
#define MAY 8
// 右下
#define MXZ 9
// 左下
#define MYA 10
// 左上
d、不用管的步進電機初始化
這個部分的內容完全不用管，直接copy就可。
/*——————————————————————-*
* 步進電機的初始設定方向-次
*——————————————————————-*/
void
defaultDir
（）{
// 正反轉 、 步進與否 、 是否啟用 （maybe）
// 初始化正轉（順時針）、不發生脈衝步進、啟用
motorx
。
setPinsInverted
（
ClockWise
，
NotSendPluse
，
StartUse
）；
//步進電機初始正轉向為順時針，是否傳送步進脈衝步進，是否啟用步進電機
motory
。
setPinsInverted
（
ClockWise
，
NotSendPluse
，
StartUse
）；
motorz
。
setPinsInverted
（
ClockWise
，
NotSendPluse
，
StartUse
）；
motora
。
setPinsInverted
（
ClockWise
，
NotSendPluse
，
StartUse
）；
}
/*——————————————————————-*
* 步進電機的最大速度-次
*——————————————————————-*/
void
defaultMaxSpeed
（）{
motorx
。
setMaxSpeed
（
CarMaxSpeed
）；
motory
。
setMaxSpeed
（
CarMaxSpeed
）；
motorz
。
setMaxSpeed
（
CarMaxSpeed
）；
motora
。
setMaxSpeed
（
CarMaxSpeed
）；
}
/*——————————————————————-*
* 步進電機的初始化
*——————————————————————-*/
void
intiMotors
（）{
//四個電機共用一個EN，任取其一控制即可
motorx
。
setEnablePin
（
En_Pin
）；
//設定步進電機使能埠
defaultDir
（）；
//設定步進電機初始方向
defaultMaxSpeed
（）；
//設定步進電機最大速度
motorx
。
enableOutputs
（）；
//步進埠使能
}
/*————————————————————-*
* 步進電機運動到設定的速度
*————————————————————-*/
inline
void
runToSpeed
（）{
// 。setSpeed（Val） 之後呼叫。
motorx
。
runSpeed
（）；
// 勻速運動 —— 開始運動
motory
。
runSpeed
（）；
motorz
。
runSpeed
（）；
motora
。
runSpeed
（）；
}
e、小車運動方向設定（步進電機）
這裡需要
調整一下 v0 的方向。
（根據前文中 xyza 四個電機的運動方向調整）
調整好後，該函式就不用管了。
/*——————————————————————*
* 步進電機速度設定（運動方向）
*——————————————————————*/
// dir-運動方向（前後左右） vxy-xy方向行進速度（直行） vw-斜著走
inline
void
xySetVal
（
double
vxy
，
double
vw
，
int
dir
）{
// 步進電機以“步”驅動。
double
v0
=
vxy
*
StepOfMeter
；
//直行速度轉化為步數的速率 —— 【m/s * 步/m = 步/s】
double
v1
=
vw
*
StepOfMeter
；
//轉動的速率轉化為步數的速率 —— 斜向行進所需，用於修正偏移
switch
（
dir
）{
// 設定行進方向集步進數
case
MFord
：
// 1 前——>x軸正向： X+ Y+ Z+ A+
motorx
。
setSpeed
（
v0
）；
motory
。
setSpeed
（
v0
）；
motorz
。
setSpeed
（
v0
）；
motora
。
setSpeed
（
v0
）；
break
；
case
MBack
：
// 2 後——>x軸負向： X- Y- Z- A-
motorx
。
setSpeed
（
-
v0
）；
motory
。
setSpeed
（
-
v0
）；
motorz
。
setSpeed
（
-
v0
）；
motora
。
setSpeed
（
-
v0
）；
break
；
case
MLeft
：
// 3 左——>y軸正向： X- Y+ Z- A+
motorx
。
setSpeed
（
-
v0
）；
motory
。
setSpeed
（
v0
）；
motorz
。
setSpeed
（
-
v0
）；
motora
。
setSpeed
（
v0
）；
break
；
case
MRight
：
// 4 右——>y軸負向： X+ Y- Z+ A-
motorx
。
setSpeed
（
v0
）；
motory
。
setSpeed
（
-
v0
）；
motorz
。
setSpeed
（
v0
）；
motora
。
setSpeed
（
-
v0
）；
break
；
// 用於尋跡時修正方向
case
RLeft
：
// 5 左偏調整——>順時針： X+ Y- Z- A+
motorx
。
setSpeed
（
v1
）；
motory
。
setSpeed
（
-
v1
）；
motorz
。
setSpeed
（
-
v1
）；
motora
。
setSpeed
（
v1
）；
break
；
case
RRight
：
// 6 右偏調整——>逆時針： X- Y+ Z+ A-
motorx
。
setSpeed
（
-
v1
）；
motory
。
setSpeed
（
v1
）；
motorz
。
setSpeed
（
v1
）；
motora
。
setSpeed
（
-
v1
）；
break
；
case
MZX
：
// 7 Z——>X調整——>右上↗： X0 Y+ Z0 A+
motorx
。
setSpeed
（
0
）；
motory
。
setSpeed
（
v1
）；
motorz
。
setSpeed
（
0
）；
motora
。
setSpeed
（
v1
）；
break
；
case
MAY
：
// 8 A——>Y調整——>右下↘： X+ Y0 Z+ A0
motorx
。
setSpeed
（
v1
）；
motory
。
setSpeed
（
0
）；
motorz
。
setSpeed
（
v1
）；
motora
。
setSpeed
（
0
）；
break
；
case
MXZ
：
// 9 X——>Z調整——>左下↙： X0 Y- Z0 A-
motorx
。
setSpeed
（
0
）；
motory
。
setSpeed
（
-
v1
）；
motorz
。
setSpeed
（
0
）；
motora
。
setSpeed
（
-
v1
）；
break
；
case
MYA
：
// 10 Y——>A調整——>左上↖： X- Y0 Z- A0
motorx
。
setSpeed
（
-
v1
）；
motory
。
setSpeed
（
0
）；
motorz
。
setSpeed
（
-
v1
）；
motora
。
setSpeed
（
0
）；
break
；
default
：
break
；
}
}
/*——————————————————————-*
* 暫停（速度為零）
*——————————————————————-*/
void
stopHalSec
（）{
xySetVal
（
0
，
0
，
0
）；
runToSpeed
（）；
}
f、步進電機移動固定距離 （重點！！！）
這個重點在於
如何呼叫
該函式
moveTo（dxy， dw， dir）；
引數 dxy 為移動的距離（m）；dw 不使用；
dir 為移動的方向。
dir - 參考【
c、步進電機移動方向宏定義】
moveTo（0。24， 0， 1）； // X+移動 0。24m
moveTo（0。24， 0， 2）； // X- 移動 0。24m
moveTo（0。24， 0， 3）； // Y+移動 0。24m 。。。
/*————————————————————-*
* 步進電機移動固定距離-主
*————————————————————*/
void
moveTo
（
double
dxy
，
double
dw
，
int
dir
）{
if
（（
dxy
==
0
）
&&
（
dw
==
0
））
return
；
double
t1
=
dxy
/
linear_v
；
// m/（m/s） = s — 計算需要持續執行的時間
if
（
t1
==
0
）
return
；
// 防止發生不可預料的錯誤
// 由於實際使用時並未使用第二個引數 dw，故將dw的計算去除。（可自行加入）
// dw=angle_L*dw； // 將角度運算轉化為長度執行 ？
// double t2 = dw/linear_v； // m/（m/s） = s — 計算需要持續執行的時間
// double t_last = max（t1，t2）； // dw=0時，t_last = t1。
// if（t_last==0）return； // 防止發生不可預料的錯誤
xySetVal
（
linear_v
，
linear_v
，
dir
）；
// 運動速度一般固定，只是調整移動距離
// 步進電機持續轉動時間【t_last*1000*1。03】
unsigned
long
delta_t
=
millis
（）
+
t_last
*
1000
*
1。03
；
// millis（）-ms-1000 1。03-修正係數
while
（
millis
（）
<
delta_t
）{
runToSpeed
（）；
// 步進電機開始轉動。（前邊設定步進及速度）
}
stopHalSec
（）；
// 速度為零
}
參考程式
該程式可實現基本的跑圖功能，是當時在家中除錯時的備份程式（21年2-3月）。
可參考其中的【
步進電機驅動程式&基本循跡思路
】
click here or 手機掃碼
後續文章彙總
一、方案設計（完結）
搬運小車方案總述：
Poao：2021-工訓搬運機器人-搬運小車方案簡述
1）行進驅動方案： Poao：2021-工訓搬運機器人-行進方案（步進電機+麥克納姆輪）
2）循跡定位方案： Poao：2021-工訓搬運機器人-循跡程式設計思路
3）機械臂動作順序： Poao：2021-工訓搬運機器人-機械臂動作順序
4）識別方案（二維碼+物料）： Poao：2021-工訓搬運機器人-openmv識別
二、器件除錯（一半）
1）步進電機驅動：
Poao：四路步進電機驅動板（工訓小車）— A4988/DRV8825
Poao：42步進電機驅動—A4988/DRV8825調節
2）HWT101測姿態角—矯正車姿：
Poao：2021-工訓搬運機器人-HWT101
3）串列埠屏—顯示任務碼及除錯資訊：
4）機械臂控制板：
三、程式框架（待更新）
賽前程式框架介紹
1）循跡計數詳講
2）步進電機行程序序
3）機械臂動作順序解算
4）機械臂功能函式封裝