BNO055を使って4足歩行ロボットquadの不整地適応(バランス)実験をしてみました。BNO055から出力されるオイラー角(pitch/rollの姿勢角度)をそのまま足の曲げ伸ばしに対応させています。また振動対策としてちょっとズルをして、Pitch方向は前足のみ、Roll方向は左足のみを動かしています。Yaw軸回転は、地面が平らなときのみ稼働です。
BNO055からのデータ読み出し(Arduino/Python)はこちら http://meuse-robotics.com/bno055
import machine, utime
# ******************
# ***** Action *****
# ******************
STOP = 0
LTRN = 1
RTRN = 2
SV_FREQ = 50.0 # サーボ信号周波数
MAX_DUTY = 65025.0 # 周期内の分割数
MIN_SV_PULSE = 0.6 # 最小パルス幅 0°
MAX_SV_PULSE = 2.4 # 最大パルス幅 180°
correction = [14,-14,4, 0,0,6, -25,-12,4, 20,-4,-8]
servo = []
temp_angle = [90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90]
ltrn = [\
[90-10, 90, 90, 90+ 5, 90, 90, 90+ 5, 90, 90, 90-10, 90, 90, 3],\
[90+10,100,110, 90+ 0, 90, 90, 90+ 0, 90, 90, 90+10, 80, 70, 3],\
[90+10, 90, 90, 90- 5, 90, 90, 90- 5, 90, 90, 90+10, 90, 90, 8],\
[90+ 5, 90, 90, 90-10, 90, 90, 90-10, 90, 90, 90+ 5, 90, 90, 3],\
[90+ 0, 90, 90, 90+10, 80, 70, 90+10,100,110, 90+ 0, 90, 90, 3],\
[90- 5, 90, 90, 90+10, 90, 90, 90+10, 90, 90, 90- 5, 90, 90, 8]\
]
rtrn = [\
[90+10, 90, 90, 90- 5, 90, 90, 90- 5, 90, 90, 90+10, 90, 90, 3],\
[90-10,100,110, 90- 0, 90, 90, 90- 0, 90, 90, 90-10, 80, 70, 3],\
[90-10, 90, 90, 90+ 5, 90, 90, 90+ 5, 90, 90, 90-10, 90, 90, 8],\
[90- 5, 90, 90, 90+10, 90, 90, 90+10, 90, 90, 90- 5, 90, 90, 3],\
[90- 0, 90, 90, 90-10, 80, 70, 90-10,100,110, 90- 0, 90, 90, 3],\
[90+ 5, 90, 90, 90-10, 90, 90, 90-10, 90, 90, 90+ 5, 90, 90, 8]\
]
div_counter = 0
key_frame = 0
next_key_frame = 1
rows = 0
action = []
action_mode = STOP
isTurning = False
servo_flag = False
tim = machine.Timer()
# 30Hzのタイマー割り込み
def tick(timer):
global servo_flag
servo_flag = True
tim.init(freq=30, mode=machine.Timer.PERIODIC, callback=tick)
# パルス幅を計算する関数
def get_pulse_width(angle):
pulse_ms = MIN_SV_PULSE + (MAX_SV_PULSE - MIN_SV_PULSE) * angle / 180.0
x = (int)(MAX_DUTY * (pulse_ms * SV_FREQ /1000.0))
return x
# 全てのサーボを順番に駆動
for i in range(12):
servo.append(machine.PWM(machine.Pin(11 - i)))
servo[i].freq(50)
servo[i].duty_u16(get_pulse_width(90 + correction[i]))
# ******************
# ***** IMU *****
# ******************
ADDRESS = 0x28
EULER_REGISTER = 0x1A
i2c = machine.I2C(0, sda=machine.Pin(16), scl=machine.Pin(17), freq=100_000)
buffer = bytearray(6)
euler = [0, 0, 0]
def merge(low,high):
result = low | (high << 8)
if result > 32767:
result -= 65536
return result
def init():
if i2c.readfrom_mem(ADDRESS, 0x00, 1) == b'\xa0':
config=[(0x3d, b'\x00', 80),\
(0x3f, b'\x20', 1000),\
(0x3e, b'\x00', 80),\
(0x07, b'\x00', 80),\
(0x3f, b'\x80', 1000),\
(0x3d, b'\x0c', 80)]
for register, value, delay in config:
i2c.writeto_mem(ADDRESS, register, value)
utime.sleep_ms(delay)
init()
while True:
# *** Servo ***
if servo_flag == True:
servo_flag = False
# *** get sensor ***
try:
i2c.readfrom_mem_into(ADDRESS, EULER_REGISTER, buffer)
except Exception as error:
pass
euler[0] = merge(buffer[0], buffer[1])
euler[1] = merge(buffer[2], buffer[3])
euler[2] = merge(buffer[4], buffer[5])
heading = int(euler[0]/16.0)
pitch = int(euler[1]/16.0)
roll = int(euler[2]/16.0)
if (pitch > -1 and pitch < 1) and (roll > -1 and roll <1):
if heading < 350 and heading > 270:
action_mode = RTRN
action.clear()
action = rtrn.copy()
rows = len(rtrn)
isTurning = True
elif heading > 10 and heading <90:
action_mode = LTRN
action.clear()
action = ltrn.copy()
rows = len(ltrn)
isTurning = True
else:
if isTurning == False:
action_mode = STOP
else:
if isTurning == False:
action_mode = STOP
# *** drive servo ***
if action_mode != STOP:
# キーフレームを更新
div_counter += 1
if div_counter >= action[key_frame][12]:
div_counter = 0
key_frame = next_key_frame
next_key_frame += 1
if next_key_frame > rows-1:
next_key_frame = 0
isTurning = False
action_mode = STOP
# 角度計算
for i in range(12):
temp_angle[i] = action[key_frame][i] +\
(action[next_key_frame][i] - action[key_frame][i])\
* div_counter / action[key_frame][12]
else:
temp_angle[0] = 90
temp_angle[1] += (-roll/4 + pitch/4)
temp_angle[2] += (-roll/2 + pitch/2)
temp_angle[3] = 90
temp_angle[4] -= pitch/4
temp_angle[5] -= pitch/2
temp_angle[6] = 90
temp_angle[7] -= roll/4
temp_angle[8] -= roll/2
temp_angle[9] = 90
temp_angle[10] = 90
temp_angle[11] = 90
# サーボ駆動
for i in range(12):
servo[i].duty_u16(get_pulse_width(int(temp_angle[i]) + correction[i]))
コメント