可以控制12軸的MGR996，單邊的測試腳部已經準備好了，就該來準備產生步態的部分了，產生步態的部分，直接用DrGuero2001公開的core.cpp內容。
因為之前練習控制MGR996的範例都是python，這些範例也會用到。所以決定把core.cpp內部產生步態的部分，用python重寫，應該只是格式不同，現在也不用顧慮效率。
不過因為python和C++其實都還不熟，就是持續兩邊查教學資料，邊寫python版本。說難倒是不難，單是無法理解C++的意思，與不知道python哪裡寫錯了，就要卡上不少時間
重寫後的python code如下，基本上把core.cpp逐行改寫，原本的日文註解也移過來了，外加我自己的註解，用 " #XD " 開頭做區分。
#!/usr/bin/env python # -*- coding: utf-8 -*- #Libraries import time #https://docs.python.org/fr/3/library/time.html from adafruit_servokit import ServoKit #https://circuitpython.readthedocs.io/projects/servokit/en/latest/ #/#把math加進來 import math #Constants nbPCAServo=16 LEG = 180.0 #Parameters MIN_IMP =[500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500] MAX_IMP =[2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500, 2500] MIN_ANG =[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] MAX_ANG =[180, 180, 180, 180, 180, 180, 180, 180, 180, 180, 180, 180, 180, 180, 180, 180]以上會設定控制MGR996的基本參數
K0W = [0,0] K1W = [0,0] K2W = [0,0] HW = [0,0] A0W = [0,0] A1W = [0,0] mode = 0 exit = 0 detail = 0 loop = 0 ii = 1 steptime = 0.1 adjFR = 2.04 autoH = 170 autoHs = 180 walkF = 1 pitch = 0 roll = 0 jikuasi = 0 asiPress_r = 0 asiPress_l = 0 uvcOff = 0 dyi = 0 swf = 12 fwr0 = 0 fwr1 = 0 fwct = 1 fwctEnd = 48 dx = [0,0] dxi = 0 dyi = 0 fhMax = 20 landRate = 0.2 fh = 0 fw = 20 LEG = 180.0以上變數和core.cpp相同
#Objects
pca = ServoKit(channels=16)
# function init
def init():
for i in range(nbPCAServo):
pca.servo[i].set_pulse_width_range(MIN_IMP[i] , MAX_IMP[i])
對MGR996的角度和PWM範圍進行設定
# function main
def main():
pca.servo[0].angle=None #disable channel
pca.servo[1].angle=None #disable channel
pca.servo[2].angle=None #disable channel
pca.servo[3].angle=None #disable channel
pca.servo[4].angle=None #disable channel
pca.servo[5].angle=None #disable channel
pca.servo[8].angle=None #disable channel
pca.servo[9].angle=None #disable channel
pca.servo[10].angle=None #disable channel
pca.servo[11].angle=None #disable channel
pca.servo[12].angle=None #disable channel
pca.servo[13].angle=None #disable channel
pca.servo[14].angle=None #disable channel
pca.servo[15].angle=None #disable channel
time.sleep(0.5)
print("stretch leg")
L1 = 90
L2 = 90
L3 = 90
L4 = 90
L5 = 90
L6 = 90
R1 = 90
R2 = 90
R3 = 90
R4 = 90
R5 = 90
R6 = 90
pca.servo[11].angle = R6
time.sleep(0.2)
pca.servo[10].angle = L6
time.sleep(0.2)
pca.servo[9].angle = R5
time.sleep(0.2)
pca.servo[8].angle = L5
time.sleep(0.2)
pca.servo[7].angle = R4
time.sleep(0.2)
pca.servo[6].angle = L4
time.sleep(0.2)
pca.servo[5].angle = R3
time.sleep(0.2)
pca.servo[4].angle = L3
time.sleep(0.2)
pca.servo[3].angle = R2
time.sleep(0.2)
pca.servo[2].angle = L2
time.sleep(0.2)
pca.servo[1].angle = R1
time.sleep(0.2)
pca.servo[0].angle = L1
time.sleep(0.2)
pca.servo[0].angle=None #disable channel
pca.servo[1].angle=None #disable channel
pca.servo[2].angle=None #disable channel
pca.servo[3].angle=None #disable channel
pca.servo[4].angle=None #disable channel
pca.servo[5].angle=None #disable channel
pca.servo[8].angle=None #disable channel
pca.servo[9].angle=None #disable channel
pca.servo[10].angle=None #disable channel
pca.servo[11].angle=None #disable channel
pca.servo[12].angle=None #disable channel
pca.servo[13].angle=None #disable channel
pca.servo[14].angle=None #disable channel
pca.servo[15].angle=None #disable channel
time.sleep(1)
開始跑步態前，先把腳部伸直，不過缺少速度控制，前次測試停止的角度太大時，動作會很激烈。
global detail
global ii
global steptime
while True:
try:
print(" ")
print("input loop")
loop = int(input("number of loop : "))
break;
except:
print("Invalid input")
while True:
try:
print(" ")
print("input step time")
steptime = float(input("step time : "))
break;
except:
print("Invalid input")
while True:
try:
print(" ")
print("show detail ?")
detail = int(input("input 1 for YES : "))
break;
except:
detail = 0
break;
輸入想測試的loop(總step次數)，每個step間隔時間，以及是否顯示詳細資料(檢查中間的step狀態)。為了測試所以加一些額外的輸出，用來確認python是否正常執行。
print(" ")
localtime = time.asctime( time.localtime(time.time()) )
print ("Start time :", localtime)
print(" ")
用來觀察執行開始與結束的時間。
ii = 1
while ii < loop :
#print("loop ",ii)
ii += 1
walk();
#print("QWERTYUIOP")
重複執行產生步態的walk()函數，直到指定的loop上限。
print("walk done , Release servo")
pca.servo[0].angle=None #disable channel
pca.servo[1].angle=None #disable channel
pca.servo[2].angle=None #disable channel
pca.servo[3].angle=None #disable channel
pca.servo[4].angle=None #disable channel
pca.servo[5].angle=None #disable channel
pca.servo[8].angle=None #disable channel
pca.servo[9].angle=None #disable channel
pca.servo[10].angle=None #disable channel
pca.servo[11].angle=None #disable channel
pca.servo[12].angle=None #disable channel
pca.servo[13].angle=None #disable channel
pca.servo[14].angle=None #disable channel
pca.servo[15].angle=None #disable channel
time.sleep(0.1)
localtime = time.asctime( time.localtime(time.time()) )
print(" ")
print ("End time :", localtime)
print(" ")
以上會以上會在完成loop後，停止發送PWM訊號到MGR996，等於放鬆腳部。
以下開始基本上就是原本core.cpp的python改寫，沒有變動產生部態的計算方式，可以參考另一篇core.cpp的產生步態解析。
# ******************
# * * 脚位置制御 **
# ******************
# function footcont
###footcont to generate local motion###
def footCont(x,y,h,s):
#x:中点を0とする足前後方向距離（前+）
#XD //http://ai2001.ifdef.jp/uvc/qa.jpg
#XD //猜是腳底前後與中心距離，起始是距離，後面計算應該是角度ThetaX
#y:中点を0とする足左右方向距離（右+）
#XD //猜是腳底左右與中心距離
#h:足首ロール軸から股関節ロール軸までの距離
#XD //猜是腳踝與股關節的距離
#s:支持脚(0)遊脚(1)を指定
#XD //支持腳猜是落地或是懸空
#XD //還沒找到起始值如何決定，這是函數，使用時才輸入數值
#XD //但是雙冒號有查到但還是看不懂。
k = float(0)
#XD //計算用的，應該是角度ThetaK
k = math.sqrt(x*x+(y*y+h*h))
#A0からK0までの距離
#XD //A0腳踝?，K0股關節?
if k > LEG:
k = LEG
#計算エラー回避
#XD //避開計算錯誤，大概是要避免腳反摺
x = math.asin(x/k)
#K0脚振り角度 Update in June 1,2021 :Before revision( x=asin(x/LEG) )
#XD //重複x，這看不懂，把x(距離)拿去當輸入，再把arcsin計算結果寫入x(角度)，單位是弧度
k = math.acos(k/LEG)
#K0膝曲げ角度
#XD //重複k，這看不懂，把k(距離)拿去當輸入，再把arccos計算結果寫入k(角度)，單位是弧度
fbAV=0
#UVC評価の為、ジャイロは無効にする
#XD //ジャイロGyro陀螺儀
lrAV=0
#XD //我猜也是gyro相關
K0W[s] = k+x
#XD //單位應該是角度
#XD //http://ai2001.ifdef.jp/uvc/qa.jpg圖上沒有
HW[s] = k*2
#XD //單位應該是角度
#XD //http://ai2001.ifdef.jp/uvc/qa.jpg對圖好理解
A0W[s] = k-x-0.003*fbAV
#XD //單位應該是角度
#XD //http://ai2001.ifdef.jp/uvc/qa.jpg圖上沒有
k = math.atan(y/h)
#K1角度
#XD //單位應該是角度
#XD //看起來重複使用變數用來計算
K1W[s] = k
#XD //看起來重複使用變數用來計算，再寫入目標變數
if(s==0) :
A1W[s] = -k-0.002*lrAV
#XD //腳懸空時的計算
else :
A1W[s] = -k+0.002*lrAV
#print("footcont done")
#XD //腳著地時的計算，計算式看起來就是反向
# **********************
# * * 歩行制御メイン **メインmain
# **********************
def walk():
i = int(0)
j = int(0)
k = float(0)
global autoH
global autoHs
global mode
global fwct
global ii
jikuasi = 0
asiPress_r = 0
asiPress_l = 0
uvcOff = 0
dyi = 0
swf = 12
fwr0 = 0
fwr1 = 0
fwctEnd = 48
#dx = [0,0]
dxi = 0
dyi = 0
fhMax = 20
landRate = 0.2
fh = 0
fw = 20
#LEG = 180.0
#//////////////////////
#// 初期姿勢に移行 ////
#//////////////////////
if mode == 0:
#//louis 姿勢初始化
if autoHs > autoH :
autoHs -= 1
#XD //autoHs初始值180，autoH初始值170
else:
mode=10
#XD //chenge mode to idle
#XD //mode initial value 0
if detail == 1 :
print("姿勢初始化 ",mode)
#print("mode ",mode," J0")
footCont( -adjFR, 0, autoHs, 0 );
#XD //(軸足) adjFR initial 2.04
#print("mode ",mode," J1")
footCont( -adjFR, 0, autoHs, 1 );
#XD //(遊脚) adjFR initial 2.04
#////////////////////
#// アイドル状態 ////
#////////////////////
#XD // idle status
elif mode == 10 :
K0W[0] = 0
#XD //股関節0(軸足) value 0
K0W[1] = 0
#XD //股関節1(遊脚) value 0
#// パラメータ初期化 ////parameter initial
dx[0] =0
#前後方向脚位置
dx[1] =0
#前後方向脚位置
fwr0 =0
#中心からの戻り股幅(軸足)
fwr1 =0
#中心からの戻り股幅(遊脚)
fwct =0
#一周期カウンタ
# louis //計算到步態週期的第幾個count
dxi =0
#縦距離積分値
dyi =0
#横距離積分値
dy =0
#横振り距離
jikuasi =0
#XD //jikuasi軸足
fwctEnd =48
#一周期最大カウント数
swf =12
#横振り最大距離
fhMax =20
#足上げ最大高さ
landRate=0.2
#0.1一歩の内、両足接地期間の割合
fh =0
#足上げ高さ指示
#XD //推測是抬腳高度
if detail == 1 :
print("アイドル状態 ",mode)
#print("mode ",mode," J0")
footCont( -adjFR, 0, autoH, 0 );
#XD //(軸足)
#print("mode ",mode," J1")
footCont( -adjFR, 0, autoH, 1 );
#XD //(遊脚)
if walkF & 0x01 :
#歩行フラグ （b0:歩行 b1:未 b2:未）
#XD // #&# 是位元運算and
fw=20
#中心からの振り出し股幅
mode=20
#XD // switch to mode 20
#///////////////////////////////////////////////////////////////
#////////////////////// 歩行制御 ///////////////////////////
#///////////////////////////////////////////////////////////////
elif mode == 20 or mode == 30:
#XD //後面有差，就是20和30會共用大部分的計算，但是根據20或30調整部分計算
#elif mode == 30 :
############################################################
#################### UVC(上体垂直制御) ####################
############################################################
if ( ( jikuasi == 0 and asiPress_r < -0.1 and asiPress_l > -0.1 ) or ( jikuasi == 1 and asiPress_r > -0.1 and asiPress_l < -0.1 ) ) :
#XD //看起來是分左右腳檢查，只需要左腳或右腳接地壓力數值有一邊成立
k = 1.5 * 193 * sin(lrRad)
#// 左右方向変位 ////
#XD //lrRad頭左右角度 右が-，放在biped.h
#XD //看來是用頭判斷身體偏左或偏右
#XD //k計算用，不用在意起始值
if jikuasi == 0 :
dyi += k
#XD //軸腳時，dyi增加k
else :
dyi -= k
#XD //游腳時，dyi減少k
if dyi > 0 :
dyi = 0
#横距離積分値
if dyi < -30 :
dyi = -30
#XD //把dyi數值限制在-30與0之間
k = 1.5 * 130 * math.sin(fbRad)
#// 前後方向変位 ////
#XD //fbRad頭前後角度 前が-
#XD //也是用頭判斷身體偏前或偏後
#XD //k計算用，不用在意起始值
dxi += k
#XD //dxi縦距離積分値，但是看不到什麼時候積分的
dyi *= 0.90
#減衰
if uvcOff == 1 :
#XD //關閉UVC，把dxi和dyi歸零
dxi = 0
dyi = 0
############################################################
######################### 基本歩容 #######################
############################################################
#XD //步態只看腳軸尖
#// 横振り ////
k = swf * math.sin(math.pi*(fwct)/fwctEnd)
#sinカーブ //louis カーブcurve
#XD //swf，不起始值12
#XD //math.pi圓周率，sinf就是sin，只是傳回f浮點數，輸入是弧度
#XD //fwct會由零逐步增加到最大值fwctEnd
#XD //算式看懂了，但是意義不明
if jikuasi == 0 :
dy = k
#右振り
#XD //軸腳時，dy等於k
else :
dy = -k
#左振り
#XD //游腳時，dy等於負k
#// 軸足側前振り制御 ////
if fwct < fwctEnd/2 :
dx[jikuasi] = fwr0*( 1 - 2.0*fwct/fwctEnd )
#立脚中期まで
#XD //半週期內，2.0*fwct/fwctEnd一定小於1，所以fwr0會隨跨出逐步減少
else :
dx[jikuasi] = -(fw-dxi)*( 2.0*fwct/fwctEnd-1 )
#立脚中期移行UVC適用
#XD //超過半週期時，2.0*fwct/fwctEnd一定大於1
#// 遊脚側前振り制御 ////
if mode == 20 :
if detail == 1 :
print("両脚シフト期間",mode)
#両脚シフト期間
#XD //シフトshift應該是兩隻腳交錯的時候。
if fwct < (landRate*fwctEnd) :
#XD //檢查fwct是否超過範圍，在範圍內就
dx[jikuasi^1] = fwr1-(fwr0-dx[jikuasi])
#XD //由jikuasi的dx資料，對產生另一jikuasi的dx數據
else :
#XD //fwct超過範圍就
fwr1 = dx[jikuasi^1]
mode=30;
#XD //這裡切換成case 30
if mode == 30 :
if detail == 1 :
print("前振出",mode)
#前振出
#XD //k計算用，不用在意起始值，只有量值沒有方向性
k=(
-math.cos(
math.pi*( fwct-landRate*fwctEnd )/
#XD //超出landrate比率的步態count
( (1-landRate)*fwctEnd )
#前振り頂点までの残りクロック数
#XD //跨步到頂點前，剩餘的步態count
)+1
)/2
#0-1の∫的カーブ
#XD //∫微分，カーブcurve，似乎直接用/2簡單達成
dx[jikuasi^1] = fwr1+k*( fw-dxi-fwr1 )
#XD //算式看得懂，但是原理就不知道了
if dx[jikuasi] > 100 :
#振り出し幅リミット
#XD //跨步距離有檢查正方向上限
dxi -= dx[jikuasi] - 100
#XD //超過100的部分，就減少dxi積分
dx[jikuasi] = 100
#XD // 然後回歸100
if dx[jikuasi] < -100 :
#XD //跨步距離也檢查負方向下限
dxi -= dx[jikuasi] + 100
#XD //低過-100的部分，就減少dxi積分
dx[jikuasi] = -100
#XD // 然後回歸-100
if dx[jikuasi^1] > 100 :
dx[jikuasi^1] = 100
#振り出し幅リミット抑制
#XD //換腳，跨步距離有檢查正方向上限，然後回歸100
if dx[jikuasi^1] < -100 :
dx[jikuasi^1] = -100
#XD //換腳，跨步距離也檢查負方向下限，然後回歸-100
#// 足上制御 ////
#XD //控制抬腳高度
i = landRate * fwctEnd
#XD //算出兩腳都落地的步態count
if fwct > i :
fh = fhMax * math.sin( math.pi*(fwct-i)/(fwctEnd-i) )
#XD //超出落地的步態count後，才計算抬腳高度，一樣用步態比率計算
else :
fh = 0
#// 脚制御関数呼び出し ////
#XD //関数function
if jikuasi == 0 :
#是算完腳尖位置，才呼叫footCont來計算，轉軸角度
#print("mode ",mode," J0")
footCont( dx[0]-adjFR , -dy-dyi+1, autoH, 0 )
#print("mode ",mode," J1")
footCont( dx[1]-adjFR , dy-dyi+1, autoH-fh, 1 )
else :
#print("mode ",mode," J0")
footCont( dx[0]-adjFR , -dy-dyi+1, autoH-fh, 0 )
#print("left ",K2W[0]," ",K1W[0]," ",K0W[0]," ",HW[0]," ",A0W[0]," ",A1W[0]," ","right ",K2W[1]," ",K1W[1]," ",K0W[1]," ",HW[1]," ",A0W[1]," ",A1W[1])
#print("mode ",mode," J1")
footCont( dx[1]-adjFR , dy-dyi+1, autoH, 1 )
#print("left ",K2W[0]," ",K1W[0]," ",K0W[0]," ",HW[0]," ",A0W[0]," ",A1W[0]," ","right ",K2W[1]," ",K1W[1]," ",K0W[1]," ",HW[1]," ",A0W[1]," ",A1W[1])
############################################################
############ CPG（歩周期生成、今回は簡易仕様） ############
############################################################
if fwct == fwctEnd :
#簡易仕様（固定周期）
#XD //步態做滿週期時，
jikuasi ^= 1
#是換腳動作，^=1是XOR 1。0101切換。
fwct = 1
#XD //回歸起始值1
dxi = 0
#XD //歸零
fwr0 = dx[jikuasi]
#XD //軸足
fwr1 = dx[jikuasi^1]
#XD //軸足，^=1是指數1。結果不會變阿。
fh=0
#XD //歸零
mode = 20
#XD //case切到20
if fw == 20 :
#歩幅制御
landRate = 0.1
#XD //回到初始值
fw = 40
#XD //把跨步距離加到40
else :
fwct += 1
#XD //週期加1
else :
print("else")
exit = 1
print("exit",exit)
這個else，我已經忘記用途了 -_-||
前段產生步態後，就要煮換成送到MGR996的角度，這邊要順便根據實際機構，處理旋轉方向和0度位置。
####servo move###
if detail == 1 :
print("left ",K2W[0]," ",K1W[0]," ",K0W[0]," ",HW[0]," ",A0W[0]," ",A1W[0])
print("right ",K2W[1]," ",K1W[1]," ",K0W[1]," ",HW[1]," ",A0W[1]," ",A1W[1])
L1 = int(K2W[0]*180/math.pi+90)
L2 = int(-K1W[0]*180/math.pi+90)
L3 = int(K0W[0]*180/math.pi+90)
L4 = int(HW[0]*180/math.pi+90)
L5 = int(-A0W[0]*180/math.pi+90)
L6 = int(A1W[0]*180/math.pi+90)
R1 = int(K2W[1]*180/math.pi+90)
R2 = int(-K1W[1]*180/math.pi+90)
R3 = int(-K0W[1]*180/math.pi+90)
R4 = int(-HW[1]*180/math.pi+90)
R5 = int(A0W[1]*180/math.pi+90)
R6 = int(-A1W[1]*180/math.pi+90)
#print("send to servo---------------------------------------------------------------------------------")
#print("left ",L1," ",L2," ",L3," ",L4," ",L5," ",L6)
#print("right ",R1," ",R2," ",R3," ",R4," ",R5," ",R6)
#print("fwct ",fwct)
print("loop,",ii,",mode,",mode,",fwct,",fwct,",left,",L1,",",L2,",",L3,",",L4,",",L5,",",L6,",right,",R1,",",R2,",",R3,",",R4,",",R5,",",R6)
#print("detail ",detail)
以下就把轉換後的角度，輸出給MGR996完成動作。
#left leg
pca.servo[0].angle = L1
pca.servo[2].angle = L2
pca.servo[4].angle = L3
pca.servo[6].angle = L4
pca.servo[8].angle = L5
pca.servo[10].angle = L6
#right leg
pca.servo[1].angle = R1
pca.servo[3].angle = R2
pca.servo[5].angle = R3
pca.servo[7].angle = R4
pca.servo[9].angle = R5
pca.servo[11].angle = R6
time.sleep(steptime)
if __name__ == '__main__':
init()
main()
為了測試所以加一些額外的輸出，用來確認python是否正常執行，MG996R不供電，空跑測試
同樣MG996R不供電，空跑測試，把輸出的結果，用excel畫出曲線。
右腳看起來大致正確，但是左腳還有問題需要處理。
送給單邊腳步跑跑看，很順利6+6軸都會動，但是各軸的基準方向和原點都還不正確。
