BNO055利用で不整地適応

BNO055を使って４足歩行ロボット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]))