UMEHOSHI ITA TOP PAGE

[Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車を使った実験

このページで示したロボットの倒立振子の検討の記録の続きのページです。

ロボットの倒立振子の検討の記録 2

ここで使用するロボットは二輪車と、別途の一つの支柱が接地する構造で、電源を切っても倒れない構造になっている。
この状態から、支柱浮かせて二輪だけで動かす目標の検討です。

実験は、以前に作成した「TCPサーバー」のロボットに、 実験用のPythonファイルをロボットに送り込んで実行する繰り返しで行う。
送信したファイルは、「/usr/local/apps/」に送られる。
実験は、このフォルダに後述の「raspiAPumeTest.py」ファイルを送ることで行いました。
倒立振子を行う場合に必要な情報は、モータの前進や後進の方向に回転するPitch情報です。
現状で支柱が接地している場合はPitchが0.6です。そいて、支柱の接地が浮いた時はPitchが45まで回転できます。
モータを前進させ、その後でモータ逆回転させるとつんのめって支柱の接地が浮いてPitchが大きくなります。
この状態を保持させる制御を行います。
接地安定状態目標となる二輪車だけの倒立制御状態

最初の目標として、支柱の接地が浮いた時の目標角度の設定を「target_angle=23.5」として実験しました。
この角度は、上記の右写真の「二輪車だけの倒立制御状態」の角度で、前後で均衡がとりやすいと判断した時のPitch測定値です。
(Pitchの測定値は、ここで紹介した9軸センサーBNO055から得られる値で、キャリブレーションや オフセット調整済みの値ですが、
　　再びキャリブレーションを行うと変化してしまう可能性がある値です)
そして、単純に「Pitch < target_angle」であれば逆転させて、
「Pitch > target_angle」であれば正転させる制御をしてみました。

この制御に移行するため、最初にモータを前進させて助走した後、モータ逆回転で支柱浮かせています。
この判定も「Pitch > target_angle」で、最初はそうなるまで逆転させます。
最初の作った時の動作イメージを以下に示します。（下記クリックでXのムービに移動します。）

以下にこのコードを示します。
上記の動作は、以下のコードのmeasure_loop()関数において、 deadband = 1.0 # デッドバンドの設定を0にした挙動です。 
#!/usr/bin/python3
# -*- coding: utf-8 -*-
#   TCPサーバープログラム（/usr/local/apps/raspiAPumeTest.py）
#  [Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車のサービスから呼び出される
# 倒立振子を試すコード

import subprocess
import os # ファイル有無を調べるため追加 ★
def exists(path):
    try:
        os.stat(path)
        return True
    except (OSError, AttributeError):
        # ファイルが存在しない、またはos.statが使えない場合にFalseを返す
        return False

# ---- LED用の出力とタクトスイッチ用入力 のためのGPIO初期化-----
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(21, GPIO.OUT)  # GPIO21を出力に設定
GPIO.output(21, GPIO.HIGH)  # ON（3.3V）
for no in [6, 16, 17]:
    GPIO.setup(no, GPIO.IN, pull_up_down=GPIO.PUD_UP)  # プルアップ付き入力

#------ GPIO初期化 終了 -------------------------------------

import signal # システムが閉じることを判断するため追加 ★
import board
import busio
from adafruit_ssd1306 import SSD1306_I2C # SSD1306ディスプレイ用
import adafruit_vl53l1x # VL53L1X使用 レーザー測距センサーモジュール用
# オープンソースハードウェアの設計・製造・販売を行うアメリカの企業のAdafruit（エイダフルート）モジュール利用
from PIL import Image, ImageDraw, ImageFont
import time

i2c = busio.I2C(board.SCL, board.SDA)# --- I2C初期化 ---

# --- SSD1306ディスプレイ初期化 (128x64の場合) -----------
oled = SSD1306_I2C(128, 64, i2c)
oled.contrast(128) # 0?255

oled.fill(0)   # --- クリア
oled.show() # ---表示

# --- Pillowで描画領域を作成 ---
image = Image.new("1", (oled.width, oled.height))
draw = ImageDraw.Draw(image)
font = ImageFont.load_default()# --- フォント設定 ---

def draw_text(txt: str, row=0, showFlag = True, newImageFlag = False, font=font, fill=255):
   global image,draw
   if newImageFlag: 
      image = Image.new("1", (oled.width, oled.height)) # イメージ作り直し（全体クリア）
      draw = ImageDraw.Draw(image)
   # --- テキスト描画 (0＝黒、255＝白)上記設定で、横21文字---
   draw.text((0, row*15), txt , font=font, fill=255)
   # --- 画面に表示 ---
   oled.image(image)
   if showFlag: oled.show()

draw_text(f"UMEHOSHI ITA",0)

# 9軸センサー BNO055 制御 ---------------------------------------------------
import smbus # I2C通信をPythonから簡単に扱うためのモジュール
import os

# BNO055 の初期化
BNO055_ADDRESS = 0x28  # BNO055のI2Cアドレス(ADRピンがGNDなら0x28、VDDなら0x29になります)
BNO055_OPR_MODE = 0x3D # 動作モードを設定するためのレジスタ
BNO055_EULER_H_LSB = 0x1A # オイラー角（方位・ロール・ピッチ）のデータが始まるアドレス
bus = smbus.SMBus(1)# 引数の1でRaspberry PiのボードGPIO2: SDA、GPIO3: SCLを指定

bno055_calib_bin_path="/usr/local/apps/bno055_calib.bin" # キャリブレーションデータファイルパス
# このキャリブレーションデータファイルが存在しなければ、作成する。
try:
   os.stat(bno055_calib_bin_path) # ファイルが存在しない場合は、エラー
except OSError:
   bus.write_byte_data(BNO055_ADDRESS, BNO055_OPR_MODE, 0x00) # 設定変更(OPR_MODE)でCONFIGモードに切り替える
   time.sleep(0.05)
   # センサーをリセット（0x3FのSYS_TRIGGERレジスタのビット7をセット）
   bus.write_byte_data(BNO055_ADDRESS,0x3F, 0x20)
   time.sleep(0.7)  # リセット後は再起動まで時間がかかる
   # 出力単位(UNIT_SEL)を設定（0x00で「角度＝度（°）」単位）
   bus.write_byte_data(BNO055_ADDRESS,0x3B, 0x00)
   # センサーフュージョンを有効にするNDOFモードに変更
   bus.write_byte_data(BNO055_ADDRESS,BNO055_OPR_MODE, 0x0C) # NDOFモードへ
   time.sleep(0.05)
   while True: # NDOFモードで全てのセンサー（SYS, GYR, ACC, MAG）が 3 になるまで動かす。
      cal = bus.read_byte_data(BNO055_ADDRESS, 0x35)
      sys = (cal >> 6) & 0x03
      gyr = (cal >> 4) & 0x03
      acc = (cal >> 2) & 0x03
      mag = (cal >> 0) & 0x03
      draw_text(f"bno055 NDOF Calibrating...",0, showFlag = False,newImageFlag = True) # キャリブレーションの開始
      draw_text(f"SYS:{sys}, GYR:{gyr}, ACC:{acc}, MAG:{mag}", 1)
      time.sleep(0.5)
      if sys == 3 and gyr == 3 and acc == 3 and mag == 3 : break # キャリブレーション完了？
      # 各値が 3 になれば完全キャリブレーション完了です
   #
   calib_data = bus.read_i2c_block_data(BNO055_ADDRESS, 0x55, 22)
   with open(bno055_calib_bin_path, "wb") as f:
      f.write(bytearray(calib_data)) # オフセット値を読み出して保存
   #
   draw_text(f"End Calibration",2) # キャリブレーションデータファイル作成終了

# オフセット調整（センサーの取り付け位置や方向を自動補正）
bno055_offset_path="/usr/local/apps/bno055_offset.txt"
# 上記ファイル内に X軸が北を向く時にHeading、水平に置いた時にRollと Pitchが記憶される
make_offset_mode=False # "bno055_offset.txt"オフセット調整ファイル作成モード
try:
   with open(bno055_offset_path, "r") as fr:
      s = fr.readline() # 一行読み取り(改行を含めて)
   draw_text(f"bno055 offset setting",0,newImageFlag = True)
   a = s.split(",")
   heading_offset = float(a[0]) # オフセット調整値取得
   roll_offset = float(a[1])
   pitch_offset = float(a[2])
   print(f"offset value heading:{heading_offset}, roll:{roll_offset}, pitch:{pitch_offset}")
except:
   make_offset_mode = True # bno055_offset.txtのオフセット調整作成モード

bus.write_byte_data(BNO055_ADDRESS, BNO055_OPR_MODE, 0x00) # 設定変更(OPR_MODE)でCONFIGモードに切り替える
time.sleep(0.05)

with open(bno055_calib_bin_path, "rb") as f:
   calib_data = list(f.read(22)) # 別途bno055_calib_write.pyで行ったキャリブレーションの記憶ファイルを読む

# 設定モードへ
bus.write_byte_data(BNO055_ADDRESS, 0x3D, 0x00)
time.sleep(0.025)

# キャリブレーションデータを書き込んで、調整情報を復元
bus.write_i2c_block_data(BNO055_ADDRESS, 0x55, calib_data)

# NDOFモードに戻す
bus.write_byte_data(BNO055_ADDRESS, 0x3D, 0x0C)
time.sleep(0.05)

# 出力単位(UNIT_SEL)を設定（0x00で「角度＝度（°）」単位）
bus.write_byte_data(BNO055_ADDRESS,0x3B, 0x00)

def to_signed(val):
   """16ビット値を符号付き整数に変換"""
   if val >= 0x8000:
      val -= 0x10000
   return val


prev1_Pitch = 0 # 前のPitchの測定値
prev2_Pitch = 0 # 前のPitchの測定値
prev3_Pitch = 0 # 前のPitchの測定値
def read_euler():
   ''' make_offset_modeがTrueの場合は、現在の
   heading：左右への向き, roll：左右の傾き, pitch：上下の傾きを返す。
   make_offset_modeがFalseの場合は、調整過程のheading, roll, pitchを返す'''
   #
   global prev1_Pitch,prev2_Pitch,prev3_Pitch # 前のPitchの測定値
   data = bus.read_i2c_block_data(BNO055_ADDRESS, BNO055_EULER_H_LSB, 6)
   # 各要素が 1 バイト（0〜255）の整数を6個のリストで得られる。（1 LSB = 1/16 度）

   # データはリトルエンディアン形式（下位→上位の順）
   heading = (data[1] << 8) | data[0]  # 方位角（北基準のYAW）
   roll    = (data[3] << 8) | data[2]  # ロール角（左右の傾き）
   pitch   = (data[5] << 8) | data[4]  # ピッチ角（前後の傾き）

   # ロールとピッチは符号付き
   roll = to_signed(roll)
   pitch = to_signed(pitch)

   # スケーリング（1 LSB = 1/16 度）
   heading = heading / 16.0 # 
   roll    = roll / 16.0
   pitch   = pitch / 16.0

   if make_offset_mode == False: #オフセット調整処理
      # X軸が北を向く時にHeadingが0、水平に置いた時にRollと Pitchが0になるオフセット調整
      heading = heading - heading_offset # headingズレ補正
      heading = heading if heading >= 0 else heading + 360
      roll = roll - roll_offset # rollズレ補正
      if roll > 90:
         roll = -(90 - roll)
      elif roll < -90:
         roll = -(-90 - roll)
      pitch = pitch - pitch_offset # pitchズレ補正
      if pitch > 180:
         pitch = -(360 - pitch)
      elif pitch < -180:
         pitch = -(-360 - pitch)
   #
   prev3_Pitch = prev2_Pitch # 前の測定値記憶
   prev2_Pitch = prev1_Pitch # 前の測定値記憶
   prev1_Pitch = pitch       # 前の測定値記憶
   return heading, roll, pitch # 左右への向き, 左右の傾き, 上下の傾きを返す


if make_offset_mode: #オフセット調整処理
      count = 100 # ロボットを水平にしてはX軸が北を向いてから安定に必要な予想回数
      while True:
         h, r, p = read_euler()
         print(f"Heading: {h:7.2f}, Roll: {r:7.2f}, Pitch: {p:7.2f}")
         count -= 1
         print(f"  {count}が0になるまでに、ロボットを水平にしてはX軸が北を向くように置いてください。")
         draw_text(f"Before CountReaches 0",0,showFlag = False, newImageFlag = True)
         draw_text(f"Place it horizontally",1,showFlag = False)
         draw_text(f"   and point it north",2,showFlag = False)
         draw_text(f" count:{count}",3)
         time.sleep(0.2)
         if count <= 0: 
            with open(bno055_offset_path, "w") as fw:
               fw.write(f"{h},{r},{p}\n") # オフセット調整データ書き込み
            heading_offset, roll_offset,pitch_offset=h,r,p
            break # 上記でX軸が北を向く時にHeading、水平に置いた時にRollと Pitchを記憶

# --------------------------------------------------------------------------------
vl53 = adafruit_vl53l1x.VL53L1X(i2c)# VL53L1X使用 レーザー測距センサーモジュール初期化
print("VL53L1X Start measuring...")
vl53.start_ranging()

distance = vl53.distance
time.sleep(0.5)
print(f"Distance: {distance} mm")

# umetcp umeusb 通信関連----------------------------------------------------
import os
import umetcp
from umetcp import send_message, receiveData
import socket
import umeusb
import traceback

sock = None # クライアントと通信するソケット

def my_tcp_receive_file_func(filepath):
   ''' 受信したumehoshiアプリ用「.umh」のファイルより、
   「UME専用Hexコマンド」の文字列をusbへ出力する'''
   name,ext = os.path.splitext(filepath)
   if not ext == ".umh" : return
   with open( filepath ) as f: ss=f.readlines()
   ss="".join(ss[1:])
   print(ss)
   umeusb.send_cmd(ss, quantity=0) # TCPで受信した 「.umh」データを[UMEHOSHI ITA]へ送る

umetcp.tcp_receive_file_func = my_tcp_receive_file_func # tcpファイル受信データの処理を置き換え

def my_tcp_recieve_message(msg):
   if msg.startswith("G:"):
      file_path = msg[2:]
      if exists(file_path):
         umetcp.send_file(sock, file_path)
      else:send_message(sock, f"G:{file_path}Not Operation\n")
   else: umeusb.send_cmd(msg)

umetcp.tcp_receive_message_func=my_tcp_recieve_message  # TCO受信文字列（UME専用Hexコマンド）処理を置き換え

def my_usb_receive_func(bin):
   'usb受信のイベントで実行するデフォルト処理'
   global sock
   #print("-----------",bin)
   ss = bin.decode('utf-8')
   a=ss.split('\n')
   for s in a:
      s = s.strip()
      if s == "": continue 
      if sock != None:
         print(f"USB receive:{s}")
         send_message(sock, s) # [UMEHOSHI ITA]からの応答メッセージをTCPで返す
      else: print( f"USB receive:{s}" )

umeusb.usb_receive_func = my_usb_receive_func # USB受信データの処理を置き換える。

umeusb.init_sub()
import threading
t_id = threading.Thread(target=umeusb.read_loop)
t_id.start()

#ip="192.168.4.1"
ip=umetcp.get_wlan0_ip() # IPアドレス取得
while ip == None:
   ip=umetcp.get_wlan0_ip()
   if ip : break
   time.sleep(0.1)

umeusb.send_cmdfile("/usr/local/apps/uStartInit.umh") # ロボット初期化("R009D020010004E")
server_addr =(umetcp.get_wlan0_ip(), 59154)
hostname=socket.gethostname()
print("Serverの情報：",hostname, server_addr)

def info_show2( pitch , target_angle):
   ''' IPアドレスpとポート番号の情報と、ピッチ角度、目標角度をSSD1306ディスプレイに表示する'''
   draw_text(f"{server_addr[0]},{server_addr[1]}",0,showFlag = False, newImageFlag = True)
   draw_text(f"Pitch:{pitch:5.1f}  {target_angle}",2,showFlag = True)

# # ピッチなどの計測と、その制御ループ----------------------------------------------------------

measure_loop_flag=True # ピッチなどの計測と、その制御ループを続けるためのフラグ
def measure_loop():
   global measure_loop_flag # 測定監視ループフラグ
   next_measure_time=0 # 測定間隔制御用（次の測定の時間を記憶する）
   flag_push = False # 倒立制御開始ボタンが押されてから倒立制御を終えるまでTrue
   flag_control = False # 倒立制御中である間だけTrue
   flag_fail = False # 制御失敗か？
   start_control_time=0 # 制御開始の時間(秒)
   start_forward_time = 1.0 # 制御開始前のモータ前進を行う期間(秒)
   target_angle=23.5 # 目標角度　15→ 20→ 25← 23→
   deadband = 1.0 # デッドバンド
   time_now=0 # 現在の測定時間
   time_bak=0 # 一つ前の測定時間
   fw = None
   #
   while measure_loop_flag:
      if GPIO.input(6) == GPIO.LOW: # SW2スイッチ(緑)が押された？　リブート処理------------
         draw_text(f"Rebooting.",0,showFlag = True, newImageFlag = True)
         time.sleep(0.01)
         measure_loop_flag=False
         os.system("sudo reboot") # OSにリブートコマンドを送信
         sock.close()
         break
      #
      if GPIO.input(17) == GPIO.LOW: # SW4スイッチ(黒色)が押された？ 制御終了-----------
         flag_push = False
         flag_control = False
         if fw and fw.closed == False:
            fw.write('End\n')
            fw.close() # 保存終了
            fw = None
      #
      if next_measure_time > time.time():
         continue # 次の周期まで待つ
      time_bak=time_now
      time_now=time.time()      
      next_measure_time = time_now + 0.0002 # 次の測定時間を更新

      #
      if flag_push==False and GPIO.input(16) == GPIO.LOW: # 制御開始用SW3スイッチ(黄色)が押された？
         time.sleep(2)
         fw = open('/usr/local/apps/log.txt', 'w')
         start_control_time = time.time() # 制御スタート時間
         flag_push = True
      #
      heading, roll, pitch = read_euler() # ジャイロ測定測定
      if flag_push == False : info_show2(pitch,target_angle) # 測定値の表示
      #
      if flag_push and flag_control == False: # 倒立制御に入る前の処理
         if time.time() < start_control_time + start_forward_time: 
            umeusb.usb_send("R009D020200004D", quantity=0) # Forward 最初の助走
            #umeusb.usb_send("R009D020A00003E", quantity=0) # STOP
         else:
            umeusb.usb_send("R009D020300004C", quantity=0) # Back 最初の倒立のための逆転
            fw.write(f'B:{time_now-time_bak} pitch:{pitch}\n')    
            if pitch > target_angle: # 倒立角度に達した？
               flag_control = True
               fw.write(f'S:{time_now-time_bak} pitch:{pitch}\n')
      #
      if flag_control: # 倒立振子制御中
         if pitch > target_angle + deadband:
            umeusb.usb_send("R009D020200004D", quantity=0) # Forward
            fw.write(f'F:{time_now-time_bak} pitch:{pitch}\n')
         elif pitch < target_angle - deadband:
            umeusb.usb_send("R009D020300004C", quantity=0) # Back
            fw.write(f'B:{time_now-time_bak} pitch:{pitch}\n')
         else:       
            umeusb.usb_send("R009D020A00003E", quantity=0) # STOP
            fw.write(f'S:{time_now-time_bak} pitch:{pitch}\n')
         #
         if time.time() > start_control_time + 3: #この時間で制御を終了
            umeusb.usb_send("R009D020A00003E", quantity=0) # STOP
            flag_push = False
            flag_control = False
            if fw and fw.closed == False:
               fw.write('End\n')
               fw.close() # 保存終了
               fw = None
         #
   #


#info_show() # ディスプレイへ表示
t_id2 = threading.Thread(target=measure_loop)
t_id2.start()

# 終了時に実行したい処理 ★
def handle_shutdown(signum, frame):
   global measure_loop_flag
   if measure_loop_flag == False: return
   print(f"シャットダウンを検知しました (Signal: {signum})")
   measure_loop_flag = False
   draw_text(f"Wait....",0,showFlag = True, newImageFlag = True)
   time.sleep(10)
   draw_text(f"Shutdown.",0,showFlag = True, newImageFlag = True)
   time.sleep(0.01)

# SIGTERM（システム終了時の標準信号）を登録 ★
signal.signal(signal.SIGTERM, handle_shutdown) # ★

# TCPサーバー起動処理
try:
   serversock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
   serversock.bind(server_addr)  # IPとポート番号を指定します
   print(f"{server_addr}:接続要求を待つ")
   serversock.listen()
except : serversock = None
while serversock != None:
   print("接続を待って、接続してきたら許可")
   sock, address = serversock.accept()#サーバの接続受け入れ
   print("接続相手：",address)
   try:
      receiveData( sock ) # 受信ループ
   except Exception as e:
      print(f"例外の種類: {type(e)}")
      print(f"スタックトレース: {traceback.format_exc()}")
      draw_text(f"wait next",0,showFlag = True, newImageFlag = True)
   sock.close()
単純に「Pitch < target_angle」であれば逆転させて、「Pitch > target_angle」であれば正転させるだけの制御では、 振動が収まり難くオーバーシュートを繰り返すことが多い。
そこで、上記のコードでは deadband = 1.0 # デッドバンドの値を使って 目標角度の±deadbandの範囲ではモータを止めるようにして振動を抑える目論見のコードです。 
if pitch ＞ target_angle + deadband: 
	前進の命令をUSBでUMEHOSHI ITAに送る。
elif pitch ＜ target_angle - deadband: 
	後進の命令をUSBでUMEHOSHI ITAに送る。
else: 
	停止の命令をUSBでUMEHOSHI ITAに送る。
このような手法は、「Bang-Bang制御」と呼ばれます。
以上のコードの動作イメージを以下に示します。（下記クリックでXのムービに移動します。）

このようなバンバン制御は、制御対象の状態を目標値に保つために、 制御入力が最大値または最小値のいずれかに切り替わる制御方式で、 制御入力が急激に変化するため、制御対象の挙動が滑らかになりにくいという欠点があります。
そのせいか、目標値付近で振動的な挙動が発生して、安定しませんでした。

対策として、バンバン制御に似ている方法でヒステリシスを加える方法があるそうである。
（状態遷移のしきい値の差を設けることで、頻繁な切り替えを防ぐ考え）
そのしきい値を検討するため、どように制御しているかモータ制御のタイミングをファイル化（/usr/local/apps/log.txt）してみました。
そしてこれを次のコードでグラブ化させた。 
import numpy as np
import matplotlib.pyplot as plt
ts=[] # 測定時間
pitchs=[] # 上記規則時のロボット角度
pwm=[] # 上記規則時のモータモータ制御に使ったPWMの大きさ（正転、逆転、停止）
fr = open('log.txt', 'r')
line = fr.readline()
timing=0
while line != '':
    i=line.find(" ")
    if line.startswith('B:') or line.startswith('F:') or line.startswith('S:'):
        flag=line[0]
        if timing == 0:
           timing = float(line[2:i])
        else:
           timing += float(line[2:i])
        ts.append(timing)
        i+=len('pitch:')+1
        pitch=float(line[i:])
        pitchs.append(pitch)
        print(f"{timing} {pitch} {flag} ")
    #
    if line.startswith('B:'):
        pwm.append(-0x04*5) # 逆転設定のPWNの表示用相対値
    elif line.startswith('F:'):
        pwm.append(0x04*5) # 正転設定のPWNの表示用相対値
    elif line.startswith('S:'):
        pwm.append(0)	# モータ停止の値
    line = fr.readline()
fr.close()

fig = plt.figure() # ◎ファイル保存の準備

plt.plot(ts,pitchs)
plt.plot(ts,pwm, linestyle="--")
plt.xlabel("time")
plt.ylabel("pitch")
plt.title("")
plt.legend() # グラフ内にも上記のlabel表示をつける
# plt.grid() # グリッドの表示
fig.savefig("img.png")# ◎表示イメージのファイル保存
plt.show() #画面に表示して閉じるまで待つ
下記は、上記の実行で得られたイメージです。

バンバン制御のロボット傾きの測定値（青）と、モータ制御の挙動（ダイダイ色破線）です。
モータの切り替えは、23.5＋１ と23.5ー１の目標角度で行われています。
23.5＋１に増えるまで逆転せて、この判定で正転に切り替えていますが、これまでエネルギーで角度が増え続けているため大きな振動が出来ています。

以上より改善するためには、モータの切り替え目標を増やして、その目標に応じたPWM制御が必要と感じます。

モータの切り替え点を増やして、正転と逆転の大きさを変更して制御してみる。

理想的には、目標角度と現在の角度の差に応じてモータ出力をPWM制御で変えると、よりスムーズな制御ができる。 次の処理を繰り返せば良い。
  1. error = pitch - target_angle　目標と現在角度の差を求める。
  2. pwm = Kp * error 差からモータを制御のPWMの値を求める
  3. motor.set_pwm(pwm) モータをPWMで制御する。
角度測定とモータの正転・逆転の制御をしているのがRaspbarryPi側のPythonで、 モータを直接にPWM制御しているのは、USB接続の「UMEHOSHI ITA」基板です
使用している「UMEHOSHI ITA」基板では、こちらのEEPROM化のためのシンプルなモータ制御コードを使っており、 このモータの正転、逆転、停止の動作は、Pythonからエントリーポイントの文字列（「UME専用Hexコマンド」）の受信で動いています。

ここで使っている「UMEHOSHI ITA」基板のシンプルなモータ制御コードには、速度を上げるエントリーポイントや下げるエントリーポイントはあるのですが、 直接に指定のPWMの値に変更するエントリーポイントが無いのです。
（このEEPROMのエントリポイントで、PWMのデューティ比（0〜0x0FFFF)を、左右それぞれを0x1000毎の変更ができますが、大きな変動や左右同時変更もできません。）

起動時にRAM内に６段階のPWM設定用の新たな関数を定義し、そのエントリーポイントをPythonから呼びす安易な案を模索しました。
（「UMEHOSHI ITA」基板のPWMの変数記憶域を絶対アドレス指定で変更する技法でも可能ですが、この裏技は取り敢えず却下！）
RAM内に配置する６段階のPWM設定用の新たな関数は次の通りです。
なお、これまで（上記）のPWD設定値は0x4FFFでした。これを0x0BFFFにすると助走無しの逆回転で倒立できることが分かりました。
よって、PWDの最大値を0x0BFFFとします。 
#include <xc.h> // pwm_pi3ma_init.c
#include "common.h"

#define BASE_FUNC   0x9D020000 // EEPROM領域配置用(パワーONでないリセット操作の実行アドレス)

#define AdrStart     (BASE_FUNC+0x0010) 	// アプリの起動時の初期ルーチンのアドレス
#define AdrForward   (BASE_FUNC+0x0200)	// モータ前進ルーチンのアドレス
#define AdrGoBack    (BASE_FUNC+0x0300)	// モータ後進ルーチンのアドレス
#define AdrRight     (BASE_FUNC+0x0400)	// モータ右回転ルーチンのアドレス
#define AdrLeft      (BASE_FUNC+0x0500)	// モータ左回転ルーチンのアドレス
#define AdrTimer2    (BASE_FUNC+0x0B00)	// 指定時間後にモータ停止のTimer2用アドレス
#define AdrBeep      (BASE_FUNC+0x0D00)	// ビープ音

typedef struct {
	int cn6PWM;	// 現在の左デューティ幅（コネクタから見て右のCN6コネクタ）
	int cn7PWM;	// 現在の右デューティ幅（コネクタから見て右のCN7コネクタ）
	int time2Count;//Timer2割り込みでカウントアップ(下記stopCountまでカウント)
	int stopCount; // time2CountがstopCount以上で、time2CountがをゼロにしてモータをOFF
} MortorCtrl;

#define MT ((MortorCtrl *)0xA0009000 ) // ram_area3領域の絶対アドレスで、グローバル変数を定義

__attribute__((address( 0x80005100 ))) void setPWM1 (void);
void setPWM1(void) // "R00800051000060"
{
	MT->cn6PWM = MT->cn7PWM = 0xFFF; // 左右デューティ幅を設定
}

__attribute__((address( 0x80005200 ))) void setPWM2 (void);
void setPWM2(void) // "R0080005200005F"
{
	MT->cn6PWM = MT->cn7PWM = 0x1FFF; // 左右デューティ幅を設定
}

__attribute__((address( 0x80005300 ))) void setPWM3 (void);
void setPWM3(void) // "R0080005300005E"
{
	MT->cn6PWM = MT->cn7PWM = 0x38FF; // 左右デューティ幅を設定
}

__attribute__((address( 0x80005400 ))) void setPWM4 (void);
void setPWM4(void) // "R0080005400005D"
{
	MT->cn6PWM = MT->cn7PWM = 0x4FFF; // 左右デューティ幅を設定
}

__attribute__((address( 0x80005500 ))) void setPWM5 (void);
void setPWM5(void) // "R0080005500005C"
{
	MT->cn6PWM = MT->cn7PWM = 0x07FFF; // 左右デューティ幅を設定

}

__attribute__((address( 0x80005600 ))) void setPWM6 (void);
void setPWM6(void) // "R0080005600005B"
{
	MT->cn6PWM = MT->cn7PWM = 0x0BFFF; // 左右デューティ幅を設定
}

__attribute__((address( 0x80005000 ))) void start (void);
void start()
{
	( (void (*)(void) )AdrStart )();
	setPWM6();//最大左右デューティ幅を設定
}
これをビルドして出来上がった.hexファイルより、次の「pwm_pi3ma_init.c.umh」を作ります。 
Init
S108000510000F8FFBD270400BEAF21F0A00300A0023C3D
S1080005110000090433400A0023C00904234FF0F0424BB
S108000512000040044AC0400428C000062AC21E8C003A5
S1080005130000400BE8F0800BD270800E00300000000BB
S108000520000F8FFBD270400BEAF21F0A00300A0023C3C
S1080005210000090433400A0023C00904234FF1F0424B9
S108000522000040044AC0400428C000062AC21E8C003A4
S1080005230000400BE8F0800BD270800E00300000000BA
S108000530000F8FFBD270400BEAF21F0A00300A0023C3B
S1080005310000090433400A0023C00904234FF380424C4
S108000532000040044AC0400428C000062AC21E8C003A3
S1080005330000400BE8F0800BD270800E00300000000B9
S108000540000F8FFBD270400BEAF21F0A00300A0023C3A
S1080005410000090433400A0023C00904234FF4F0424B4
S108000542000040044AC0400428C000062AC21E8C003A2
S1080005430000400BE8F0800BD270800E00300000000B8
S108000550000F8FFBD270400BEAF21F0A00300A0023C39
S1080005510000090433400A0023C00904234FF7F0424B0
S108000552000040044AC0400428C000062AC21E8C003A1
S1080005530000400BE8F0800BD270800E00300000000B7
S108000560000F8FFBD270400BEAF21F0A00300A0023C38
S1080005610000090433400A0023C00904234FFBF0434A3
S108000562000040044AC0400428C000062AC21E8C003A0
S1080005630000400BE8F0800BD270800E00300000000B6
S108000500000E8FFBD271400BFAF1000BEAF21F0A00317
S108000501000029D023C1000423409F8400000000000EE
S1080005020008015000C0000000021E8C0031400BF8FBB
S1080005030001000BE8F1800BD270800E00300000000BE
R00800050000061
以上を「UMEHOSHI ITA」基板の初期起動として使います。 それには、raspiAPumeTest.pyの中の
umeusb.send_cmdfile("/usr/local/apps/uStartInit.umh") # ロボット初期化("R009D020010004E") を、次のように変更します。
umeusb.send_cmdfile("/usr/local/apps/pwm_pi3ma_init.c.umh") # ロボット初期化
以上で、起動時に「UMEHOSHI ITA」のRAM領域にこれが展開され、下記のエントリーポイントが追加で使えるようになります。
なお、 この「あるモータの力でどれだけ頑張っても、振子が起き上がれない角度」のことを、一般に「倒立不能角（や制御不能角や 回復可能限界角（Recoverable Limit Angle））」と呼びます。
目標倒立角度に対して、この倒立不能角度より少し近い角度を判定して、対応のPWDの値を設定したいと考えます。
ここで使う判定角度は一般に、スイッチング角度（Switching Angle）や 閾値角度（Threshold Angle：制御戦略を切り替えるためのしきい値として使われる角度）と呼ばれます。

各エントリーポイントのPWD設定値で、正転と逆転方向の閾値角度（目標倒立角度(target_angle)の差で記述）を決めて、次の表にしました。
PWMの力の大きさのデューティ比は、0〜0x0FFFFであるが0x0FFFから0x0BFFFの範囲で使っている。
PWMの力の大きさエントリーポイント
実行コマンド		 逆転の
閾値角		正転回転の
閾値角
最大値setPWM6の(0x0BFFF) R0080005600005B target_angle-6.5度未満 target_angle+6.5度より大きい
setPWM5の(0x07FFF) R0080005500005C target_angle-3.0度未満 target_angle+3.0度より大きい
setPWM4の(0x4FFF) R0080005400005D target_angle-2.0度未満 target_angle+2.0度より大きい
setPWM3の(0x38FF) R0080005300005E target_angle-1.5度未満 target_angle+1.5度より大きい
setPWM2の(0x1FFF) R0080005200005F target_angle-1.0度未満 target_angle+1.0度より大きい
最小値setPWM1の(0x0FFF) R00800051000060 target_angle-0.5度未満 target_angle+0.5度より大きい
モータ停止 R009D020A00003E 上記以外 上記以外
目標倒立角度(target_angle)を22.4度に設定し、6段階のPWMのデューティ比を上記ように比較して切り替える手法で動作させた例を
Xのリンク（22.4_IMG_1319.mp4） で示します。
この時のPWM制御の状況を次のように可視化しました。

ロボット傾きの測定値が青で、モータ制御の挙動がダイダイ色破線で、デューティ比の大きさを表しており、PWMの６段階切り替えは出来ているようです。
倒立の回復可能限界角を5度未満と70度より大にしており、5度未満の判定で制御を止めています。

前述は、目標倒立角度(target_angle)を22.4度に設定した挙動で、この設定値をチョット変更すると大きく変わります。
下記の動作は、 目標倒立角度(target_angle)を22.5度に設定し、6段階のPWMのデューティ比を前述と同じ比較で切り替える手法で動作させた例を
Xのリンク（22.5B_IMG_1328.mp44） で示します。
この時のPWM制御の状況を下記で可視化しました。

倒立の回復可能限界角を5度未満と70度より大にしており、70度を超えた判定で制御を止めています。

なお、上記制御は、raspiAPumeTest.pyにおいて, measure_loopに関する黄色の範囲を次の様に置き換えたコードを使っています。
def save_results(results, last_str): # 結果保存用
   with open('/usr/local/apps/log2.txt', 'w') as fw:
      for t in results:
         fw.write(f'{t[0]}:{t[1]} pitch:{t[2]} periodo:{t[3]}\n')
      fw.write(last_str)

# # ピッチなどの計測と、その制御ループ----------------------------------------------------------
measure_loop_flag=True # ピッチなどの計測と、その制御ループを続けるためのフラグ
def measure_loop():
   global measure_loop_flag # 測定監視ループフラグ
   next_measure_time=0 # 測定間隔制御用（次の測定の時間を記憶する）
   flag_push = False # 倒立制御開始ボタンが押されてから倒立制御を終えるまでTrue
   flag_control = False # 倒立制御中である間だけTrue
   start_control_time=0 # 制御開始の時間(秒)
   start_forward_time = 0.05 # 制御開始前のモータ前進を行う期間(秒)
   target_angle=22.5 # 目標角度　15→ 20→ 25← 23→ 23.5  22.5←
   deadband = 0.5 # デッドバンド
   time_now=0 # 現在の測定時間
   motor_ctrl_time=0 # motor制御用コマンドをSUB送信した時の時間を記憶
   results = [] # 制御履歴を残すリスト（制御終了時にファイル化）
   pwmS=""
   pwmS_bak=""
   pwdN=0   # PWM デューティ比の大きさ（0〜6）
   pwdN_bak=6 # 上記の設定前の状態
   umeusb.usb_send("R0080005600005B", quantity=0) # 最大値setPWM6の(0x0BFFF)
   #
   while measure_loop_flag:
      if GPIO.input(6) == GPIO.LOW: # SW2スイッチ(緑)が押された？　リブート処理------------
         measure_loop_flag=False
         draw_text(f"Rebooting.",0,showFlag = True, newImageFlag = True)
         time.sleep(0.01)
         try: # shell=False（デフォルト）で呼び出すのが安全
            sock.close()
            subprocess.run(["sudo", "reboot"], check=True)
         except subprocess.CalledProcessError as e:
             draw_text(f"Reboot fail.",0,showFlag = True, newImageFlag = True)
         break
      #
      if GPIO.input(17) == GPIO.LOW: # SW4スイッチ(黒色)が押された？ 制御終了-----------
         flag_push = False
         flag_control = False
         save_results(results, "End SW\n") # 結果を保存
      #
      if flag_push==False and GPIO.input(16) == GPIO.LOW: # 制御開始用SW3スイッチ(黄色)が押された？
         time.sleep(2)
         results = []
         start_control_time = time.time() # 制御スタート時間
         flag_push = True
      #
      time_now=time.time()
      if next_measure_time > time_now:
         time.sleep(next_measure_time-time_now)
         continue # 次の周期まで待つ
      next_measure_time = time_now + 0.002 # 次の測定時間を更新
      #
      startM=time.time()
      for ck in range(3):
          heading, roll, pitch = read_euler() # ジャイロ測定測定
          if pitch < 150: break
      #
      endM=time.time()
      if flag_push == False : info_show2(pitch,target_angle) # 測定値の表示
      #
      if flag_push and flag_control == False: # 倒立制御に入る前の処理
         if time.time() < start_control_time + start_forward_time: 
            umeusb.usb_send("R009D020200004D", quantity=0) # Forward 最初の助走
         else:
            umeusb.usb_send("R009D020300004C", quantity=0) # Back 最初の倒立のための逆転
            if pitch > 5: # 倒立の回復可能限界角に達した？
               flag_control = True
      #
      if flag_control :# 倒立振子制御中
         pwdN=0
         if pitch < target_angle-6.5 or pitch > target_angle+6.5:
            pwdN=6
            if pwdN!=pwdN_bak: 
               umeusb.usb_send("R0080005600005B", quantity=0) # 最大値:setPWM6の(0x0BFFF)
         elif pitch < target_angle-3.0 or pitch > target_angle+3.0:
            pwdN=5
            if pwdN!=pwdN_bak: # USB送信量を抑える工夫
               umeusb.usb_send("R0080005500005C", quantity=0) # setPWM5の(0x07FFF)
         elif pitch < target_angle-2.0 or pitch > target_angle+2.0:
            pwdN=4
            if pwdN!=pwdN_bak: # USB送信量を抑える工夫
               umeusb.usb_send("R0080005400005D", quantity=0) # setPWM4の(0x4FFF)
         elif pitch < target_angle-1.5 or pitch > target_angle+1.5:
            pwdN=3
            if pwdN!=pwdN_bak: # USB送信量を抑える工夫
               umeusb.usb_send("R0080005300005E", quantity=0) # setPWM3の(0x38FF)
         elif pitch < target_angle-1.0 or pitch > pitch < target_angle+1.0:
            pwdN=2
            if pwdN!=pwdN_bak: # USB送信量を抑える工夫
               umeusb.usb_send("R0080005200005F", quantity=0) # setPWM2の(0x1FFF)
         elif pitch < target_angle-0.5 or pitch > target_angle+0.5:
            pwdN=1
            if pwdN!=pwdN_bak: # USB送信量を抑える工夫
               umeusb.usb_send("R00800051000060", quantity=0) # 最小:setPWM1の(0x0FFF)
         
         pwdN_bak = pwdN
         # モータの停止、逆転、正転
         if pwdN == 0: # motor停止
            pwmS=f'S{pwdN}'
            if  startM-motor_ctrl_time>0.01  or pwmS != pwmS_bak: # USB送信量を抑える工夫
               umeusb.usb_send("R009D020A00003E", quantity=0) # STOP      
               pwmS_bak = pwmS
               motor_ctrl_time = startM
            #  
         elif pitch <= target_angle: # 逆転
            pwmS=f'B{pwdN}'
            if startM-motor_ctrl_time>0.01 or pwmS != pwmS_bak: # USB送信量を抑える工夫
               umeusb.usb_send("R009D020300004C", quantity=0) # Back
               pwmS_bak = pwmS
               motor_ctrl_time = startM # モータ制御コマンドを送った時間をメモ
            #          
         else: # (pitch > target_angle+deadband): #正転
            pwmS=f'F{pwdN}'
            if  startM-motor_ctrl_time>0.01  or pwmS != pwmS_bak: # USB送信量を抑える工夫
               umeusb.usb_send("R009D020200004D", quantity=0) # Forward
               pwmS_bak = pwmS
               motor_ctrl_time = startM
            #               
         #
         results.append( (f'{pwmS}', startM, pitch, endM-startM) )          
         umeusb.usb.flush() 
         if pitch < 5 or pitch > 70 or startM > start_control_time + 3: #この時間で制御を終了
            umeusb.usb_send("R009D020A00003E", quantity=0) # STOP
            flag_push = False
            flag_control = False
            save_results(results, f"End pitch:{pitch} time:{startM-start_control_time}.\n") # 結果を保存
         #
      #
   #