UMEHOSHI ITA TOP PAGE

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

このページで示したロボットの倒立振子の検討の記録の続きのページです。
なお、EEPROMの内容は、このページで作成した内容と同じですが、その処理は使っていません。
（別途のRAMにSPI受信処理を埋め込む方式で検討しています）

ロボットの倒立振子の検討の記録 その4　（その3）

動作動画（Xのリンク）

Raspberry Pi 3 Model A+と「UMEHOSHI ITA」をSPIで通信するためのハード追加

[Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車で、 拡張ボードでSPI用のコネクタを追加した回路結線図＆配置図

Raspberry Pi 3 Model A+側のSPIマスターで通信するための動作確認コード

suzuki@raspberrypi:~ $ ls /dev/spidev*
/dev/spidev0.0  /dev/spidev0.1
suzuki@raspberrypi:~ $
suzuki@raspberrypi:~ $ lsmod | grep spi
spidev                 20480  0
spi_bcm2835            20480  0
suzuki@raspberrypi:~ $

import spidev
import time

spi = spidev.SpiDev() # SPI操作オブジェクト生成
spi.open(0, 0)        # bus=0 device=0 (CE0) でSPIオープン

spi.max_speed_hz = 7812500 # 7.8 MHzSPIクロック
spi.mode = 0 # クロック信号（SCLK）はLow待機、LowからHighに立ち上がる瞬間でデータを読み取り

v=0　# 送信データ（0,1,2・・・・と増やした値を　Enterキー操作で少しずつ送る）
while True:
   send_data = [v] #  例えばビッグエンディアンで0x15AFを送るなら[0x15, 0xAF]のリストで送る。決まってないが一般の受信側のオーダーで送る。
   input("Enter>") # Enterで送信へ進む
   print("Send :", send_data)
   recv_data = spi.xfer2(send_data)# 送受信
   # print("Send :", send_data) # （send_dataの内容は、受信データ置き換わる挙動もある）
   print("Recv :", recv_data) 
   time.sleep(0.1)
   v += 1 # 送信データの後進

Raspberry Pi 3 Model A+と「UMEHOSHI ITA」をSPIで通信する

PIC32MX270側のSPIスレーブ確認コード（ポーリング版）

#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include "common.h"
#include <proc/p32mx270f256b.h>
#include <sys/attribs.h>//割込み関係の定義

int spi_out_v = 0x0ff; // SPI出力用データ

/*
 PIC32MX270F256B-50I/SP　（SP は 28pin SPDIP パッケージ）のUART1を、
 18番ピンをRTS(出力)、12ピンをRX(入力)、11ピンをTX(出力)、17ピンRB8をCTS(入力)
 として使っているが、 これを、次のSPI2のスレーブモードに設定し直す
 18番ピンをCS GPIO入力、12ピンをSDI2(入力)、11ピンRB5をSDO2(出力)、17ピンをデジタル入力
 そのためのPPSは「ピン配線設定」関数。つまり **「配線設定」**です。
 */
void PPS_Init_SPI2_Slave(void)
{   
    /* ---------- UART1のフロー制御(CTS/RTS)を解除 ---------- */
    U1MODEbits.UEN = 0b00;  // UART1はTX/RXのみ使用（CTS/RTSピンを開放）
    U1MODEbits.ON = 0;      // UART1自体を使わない場合は停止

    ODCBbits.ODCB9 = 0; // RB9デフォルト設定のオープンドレイン無効
    CNPUBbits.CNPUB9 = 0;//CNPUB (Change Notice Pull-up B) レジスタの第9ビットを0
    //(これにより、RB9ピンの内部プルアップ抵抗を無効)
    CNPDBbits.CNPDB9 = 0;//CNPDB (Change Notice Pull-down B)レジスタの第9ビットを 0
    //(これにより、RB9ピンの内部プルダウン抵抗を無効) 
    LATBbits.LATB9 = 1; // RB9のRTS(出力)をHiにする
    TRISBbits.TRISB9 = 1;// RB9を入力ピンにする (基板のパターンが繋がっているため)
    
    LATBbits.LATB4 = 1;  // RB4を1にする。（必要ないかも？）
    TRISBbits.TRISB4 = 1; // RB4を入力用に指定
    ODCBbits.ODCB4 = 0; // デフォルト設定のオープンドレイン無効
    CNPUBbits.CNPUB4 = 0;//内部プルアップ抵抗を無効
    CNPDBbits.CNPDB4 = 0;//内部プルダウン抵抗を無効

    TRISAbits.TRISA4 = 1;   // RA4  (pin12) SDI2入力へ
    ANSELBbits.ANSB15 = 0;  // 26番ピンのRB15をデジタルモードに設定
    TRISBbits.TRISB15 = 1;  // スレーブの場合は入力、マスターなら0(出力)に設定

    TRISBbits.TRISB8 = 0; // RB8を出力用に指定
    CNPUBbits.CNPUB8 = 0;//内部プルアップ抵抗を無効

    /* ---------- PPS設定アンロック ---------- */
    SYSKEY = 0xAA996655;        // 書き込み保護解除キー1
    SYSKEY = 0x556699AA;        // 書き込み保護解除キー2
    CFGCONbits.IOLOCK = 0;      // PPSレジスタのロック解除

    /* ---------- 入力PPS設定 ---------- */
    SS2R  = 0b0100;             // SS2入力をRB9へ
    SDI2R = 0b0010;             // SDI2入力をRA4へ

    /* ---------- 出力PPS設定 ---------- */
    RPB8R = 0b0100; // SDO2へ
    asm("NOP");

    /* ---------- PPS再ロック ---------- */
    CFGCONbits.IOLOCK = 1;
    SYSKEY = 0;                 // 保護再有効
}

/*
 SPI2に関する周辺機能設定の初期設定
 * クロック極性、スレーブ設定、割り込みなど、ここだけ変更する可能性を考慮して
 * 「ピン配線設定」のPPS_Init_SPI2_Slaveを別関数にしている。 
 */
void Init_SPI2_Slave(void)
{
    _RB5 = 1; // D1 LED の初期点灯設定

    PPS_Init_SPI2_Slave(); // PIC32MX270F256B-50I/SP のSPI2スレーブ用端子にピン割り当てを変更

    SPI2CON = 0;                // SPI2制御レジスタ初期化
    SPI2STAT = 0;               // ステータス初期化

    /* ---------- SPIモード設定 ---------- */
    SPI2CONbits.MSTEN = 0;      // 0 = スレーブモード
    SPI2CONbits.SSEN  = 1;      // SS有効 Slave Select Enable (Slave mode) bit
    SPI2CONbits.MODE16 = 0;     // 8bit通信
    SPI2CONbits.MODE32 = 0;     // 32bit通信無効

    /* SPIモード0設定 (多くのマスターが使用) */
    SPI2CONbits.CKP = 0;        // クロックアイドルLow
    SPI2CONbits.CKE = 1;        // クロック立上りでデータ更新

    SPI2CONbits.SMP = 0;        // スレーブでは通常0

    SPI2BUF;                    // バッファをダミー読み出し
    SPI2STATbits.SPIROV = 0;    // 受信オーバーフロークリア

    // SPI用Enhanced Buffer(ENHBUF)を0(デフォルト)に指定（重要）
    SPI2CONbits.ENHBUF = 0;
    SPI2CONbits.SRXISEL = 1; //01受信バッファが1byteに指定
   
    SPI2CONbits.ON = 1;         // SPI2モジュール有効
    SPI2BUF = spi_out_v;// 最初の送信データのセット（重要）
}


void timer4() // SPIスレーブ のポーリング処理の確認用タイマー処理
{
	static int count  = 0;
	if(++count  % 1000 != 0) return;
	// count  が 1000の倍数時だけ（0.5秒ごと）に以下を実行
	
	_RB5 = ! _RB5;// このタイマーの動作確認用の D1 LED 点滅の出力を反転

	if (SPI2STATbits.SPIRBF == 1) { // SPI受信バッファが一杯になったら
		// ここを通るなら、SPI受信ができている（配線とSPI設定はOK）
		uint8_t rxData = SPI2BUF; // 受信データを読み出す（これでSPIRBFが0に戻る）
		_clear_beep_code();
		_debug_hex8(0, rxData , 1); // 受信データ確認
		spi_out_v-=1;
		while(SPI2STATbits.SPITBE == 0); // 送信バッファ空待ち
		SPI2BUF = spi_out_v;// 次の送信データのセット
	}
	if (SPI2STATbits.SPIROV == 1) { // エラーフラグが立っていないか
		SPI2STATbits.SPIROV = 0; // 一旦クリア
		_debug_hex16(0, SPI2BUF, 1);
	}
}

// UMEHOSHI ITA のRAMプログラムの設定希望プログラム(0x80005000番地より起動)
__attribute__((address( 0x80005000 ))) void start (void);
void start()
{
	_clear_beep_code(); // _debug_hex? 関数利用の初期化
	_UM_PTR_GOTO_BEEP = NULL; // debug_hex?の出力をループしない設定

	Init_SPI2_Slave();// SPI2の初期化

	// デフォルトで0.00005秒ごとに呼び出される関数に、SPIスレーブ のポーリング処理のtimer4を設定
	_HANDLES[_IDX_TIMER_4_FUNC] = timer4;
     	T4CONbits.ON = 1;// timer4割込みオン（SPIスレーブ のポーリングの起動）
	
 	_RB5 = 0; // D1 LED の消灯
	_debug_hex4(0,0x0f,1);//  _debug_hex? の動作確認用
	_send_string("start END\r\n");//起動関数終了の表示
}

Raspberry Pi 3 Model A+と「UMEHOSHI ITA」をSPI割り込みで通信するためのソフト変更

PIC32MX270側のSPIスレーブ確認コード（割り込み版）

void __ISR(_SPI2_VECTOR, IPL3SOFT) SPI2_Handler(void)
{
    ((void (**)(void) )_PTR_HANDLERS)[_IDX_SPI2_FUNC]();
}

#define _IDX_PPS_INIT_SPI2_SLAVE 70  // ★ SPI2のPPS 設定 PPS_Init_SPI2_Slave
#define _PPS_Init_SPI2_Slave()  (((void (**)(void) )_PTR_HANDLERS)[_IDX_PPS_INIT_SPI2_SLAVE]())
#define _IDX_INIT_SPI2_SLAVE 71 // ★ SPI2の初期設定　割り込みを含む初期化　Init_SPI2_Slave
#define _Init_SPI2_Slave()  (((void (**)(void) )_PTR_HANDLERS)[_IDX_INIT_SPI2_SLAVE]())
#define _IDX_SPI2_FUNC 72       // ★ SPI2割り込みで呼ぶ関数記憶用の添え字

/*
 PIC32MX270F256B-50I/SP　（SP は 28pin SPDIP パッケージ）のUART1を、
 18番ピンをRTS(出力)、12ピンをRX(入力)、11ピンをTX(出力)、17ピンRB8をCTS(入力)
 として使っているが、 これを、次のSPI2のスレーブモードに設定し直す
 18番ピンをCS GPIO入力、12ピンをSDI2(入力)、11ピンRB5をSDO2(出力)、17ピンをデジタル入力
 そのためのPPSは「ピン配線設定」関数。つまり **「配線設定」**です。
 */
void PPS_Init_SPI2_Slave(void)
{   
    /* ---------- UART1のフロー制御(CTS/RTS)を解除 ---------- */
    U1MODEbits.UEN = 0b00;  // UART1はTX/RXのみ使用（CTS/RTSピンを開放）
    U1MODEbits.ON = 0;      // UART1自体を使わない場合は停止

    ODCBbits.ODCB9 = 0; // RB9デフォルト設定のオープンドレイン無効
    CNPUBbits.CNPUB9 = 0;//CNPUB (Change Notice Pull-up B) レジスタの第9ビットを0
    //(これにより、RB9ピンの内部プルアップ抵抗を無効)
    CNPDBbits.CNPDB9 = 0;//CNPDB (Change Notice Pull-down B)レジスタの第9ビットを 0
    //(これにより、RB9ピンの内部プルダウン抵抗を無効) 
    LATBbits.LATB9 = 1; // RB9のRTS(出力)をHiにする
    TRISBbits.TRISB9 = 1;// RB9を入力ピンにする (基板のパターンが繋がっているため)
    
    LATBbits.LATB4 = 1;  // RB4を1にする。（必要ないかも？）
    TRISBbits.TRISB4 = 1; // RB4を入力用に指定
    ODCBbits.ODCB4 = 0; // デフォルト設定のオープンドレイン無効
    CNPUBbits.CNPUB4 = 0;//内部プルアップ抵抗を無効
    CNPDBbits.CNPDB4 = 0;//内部プルダウン抵抗を無効

    TRISAbits.TRISA4 = 1;   // RA4  (pin12) SDI2入力へ
    ANSELBbits.ANSB15 = 0;  // 26番ピンのRB15をデジタルモードに設定
    TRISBbits.TRISB15 = 1;  // スレーブの場合は入力、マスターなら0(出力)に設定

    TRISBbits.TRISB8 = 0; // RB8を出力用に指定
    CNPUBbits.CNPUB8 = 0;//内部プルアップ抵抗を無効

    /* ---------- PPS設定アンロック ---------- */
    SYSKEY = 0xAA996655;        // 書き込み保護解除キー1
    SYSKEY = 0x556699AA;        // 書き込み保護解除キー2
    CFGCONbits.IOLOCK = 0;      // PPSレジスタのロック解除

    /* ---------- 入力PPS設定 ---------- */
    SS2R  = 0b0100;             // SS2入力をRB9へ
    SDI2R = 0b0010;             // SDI2入力をRA4へ

    /* ---------- 出力PPS設定 ---------- */
    RPB8R = 0b0100; // SDO2へ
    asm("NOP");

    /* ---------- PPS再ロック ---------- */
    CFGCONbits.IOLOCK = 1;
    SYSKEY = 0;                 // 保護再有効
}

/*
 SPI2に関する周辺機能設定の初期設定
 * クロック極性、スレーブ設定、割り込みなど、ここだけ変更する可能性を考慮して
 * 「ピン配線設定」のPPS_Init_SPI2_Slaveを別関数にしている。 
 */
void Init_SPI2_Slave(void)
{
    _RB5 = 1; // D1 LED の初期点灯設定

    PPS_Init_SPI2_Slave(); // PIC32MX270F256B-50I/SP のSPI2スレーブ用端子にピン割り当てを変更

    SPI2CON = 0;                // SPI2制御レジスタ初期化
    SPI2STAT = 0;               // ステータス初期化

    /* ---------- SPIモード設定 ---------- */
    SPI2CONbits.MSTEN = 0;      // 0 = スレーブモード
    SPI2CONbits.SSEN  = 1;      // SS有効 Slave Select Enable (Slave mode) bit
    SPI2CONbits.MODE16 = 0;     // 8bit通信
    SPI2CONbits.MODE32 = 0;     // 32bit通信無効

    /* SPIモード0設定 (多くのマスターが使用) */
    SPI2CONbits.CKP = 0;        // クロックアイドルLow
    SPI2CONbits.CKE = 1;        // クロック立上りでデータ更新

    SPI2CONbits.SMP = 0;        // スレーブでは通常0

    SPI2BUF;                    // バッファをダミー読み出し
    SPI2STATbits.SPIROV = 0;    // 受信オーバーフロークリア

    // SPI用Enhanced Buffer(ENHBUF)を0(デフォルト)に指定（重要）
    SPI2CONbits.ENHBUF = 0;
    SPI2CONbits.SRXISEL = 1; //01受信バッファが1byteに指定

    /* ---------- 割り込み設定 ---------- */
    IFS1bits.SPI2RXIF = 0;       // 受信割り込みフラグクリア
    IEC1bits.SPI2RXIE = 1;       // SPI2受信割り込み許可
    //IEC1bits.SPI2TXIE = 1;      // SPI2送信割り込み許可
    IEC1bits.SPI2EIE  = 1;      // SPI2エラー割り込み許可

    IPC9bits.SPI2IP = 3;  // 優先度(Priority)を 3 に設定 (1?7)
    IPC9bits.SPI2IS = 0;  // 副優先度(Sub-priority)を 0 に設定 (0?3)
   
    SPI2CONbits.ON = 1;         // SPI2モジュール有効
}

    extern void PPS_Init_SPI2_Slave(void);
    extern void Init_SPI2_Slave(void);
    extern void SPI2_Slave_func(void);
    handlers[_IDX_PPS_INIT_SPI2_SLAVE]=(void *)PPS_Init_SPI2_Slave; // SPI2利用のためのPPS設定
    handlers[_IDX_INIT_SPI2_SLAVE]=(void *)Init_SPI2_Slave; // SPI2の初期化デファルト関数登録
    handlers[_IDX_SPI2_FUNC]=(void *)SPI2_Slave_func;       // SPI2受信割り込みデファルト

#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include "common.h"
#include <proc/p32mx270f256b.h>
#include <sys/attribs.h>//割込み関係の定義

int spi_out_v = 0x0ff; // SPI出力用データ

void SPI2_Slave_func(void)// スレーブ用割り込み処理から呼ばれる
{
    /* ---- 受信割り込み ---- */ 
    if (IFS1bits.SPI2RXIF)// SPI2RXIF = 受信バッファにデータが入った 
    {
        uint8_t data;
        data = SPI2BUF;        // SPI受信データを読む
                               // 読まないとSPIROV(オーバーフロー)が発生する
        /* ここで data を処理する */
        _clear_beep_code();
        _debug_hex8(0, data, 1); // 受信データ確認
        spi_out_v-=1;
        while(SPI2STATbits.SPITBE == 0); // 送信バッファ空待ち
        SPI2BUF = spi_out_v;// 次の送信データのセット
    }

    /* ---- 送信割り込み ---- */
    /* PIC32（および多くのマイコン）において、「割り込みフラグ（IFS）」と「割り込み許可ビット（IEC）」は独立して動いているため
       受信で割り込みがあると、送信割り込みを不許可にしても、「割り込みフラグ（IFS）」がセットされる。
　　　(書かない方はよい。)
    if (IFS1bits.SPI2TXIF)// SPI2TXIF = 送信バッファが空 
    {
        SPI2BUF = 0x55;        // 次の送信データを書く
                               // 書くと送信開始される

        IFS1bits.SPI2TXIF = 0; // TX割り込みフラグクリア
    }
    */

    /* ---- エラー割り込み ---- */
    if (IFS1bits.SPI2EIF)
    {
        SPI2STATbits.SPIROV = 0;   // 受信オーバーフロー解除
        IFS1bits.SPI2EIF = 0;      // エラーフラグクリア
    }
}

// UMEHOSHI ITA のRAMプログラムの設定希望プログラム(0x80005000番地より起動)
__attribute__((address( 0x80005000 ))) void start (void);
void start()
{
	_clear_beep_code(); // _debug_hex? 関数利用の初期化
	_UM_PTR_GOTO_BEEP = NULL; // debug_hex?の出力をループしない設定

	_HANDLES[_IDX_SPI2_FUNC] = (void *)SPI2_Slave_func;// SPI割り込み関数の登録

	((void (**)(void) )_PTR_HANDLERS)[_IDX_INIT_SPI2_SLAVE]();// SPI2の初期化
	SPI2BUF = spi_out_v;// 最初の送信データのセット（重要）

	_RB5 = 0; // D1 LED の消灯
	_debug_hex4(0,0x0f,1);//  _debug_hex? の動作確認用
	_send_string("start END\r\n");//起動関数終了の表示
}

Raspberry Pi 3 Model A+側はモータPWM情報をSPIでマスターで送り、受信した「UMEHOSHI ITA」でモータを制御する。

1. スレーブ側は、次に受信を希望する識別番号をslv_req_idxに記憶して、それを送信データとしてセットしておきます。
   （最初のslv_req_idxは、1を記憶しておくことになります。）
2. マスターは、最初にmas_req_idxに識別番号の0を記憶しておきます。
3. 送受信の繰り返し
   1. マスターは、mas_req_idxに記憶される識別番号が示す送信データを送信します。同時にスレーブからの受信要求番号を、mas_req_idxに記憶します。
      （マスター側は同じ要求番号を連続して受信した場合、スレーブ側が取り損なったと判断して、チョット待ってからmas_req_idxに識別番号の0にして次の繰り返しに進む。）
   2. スレーブは、sla_req_idxに記憶される識別番号から前に要求した識別を得てそれが示す受信データとして格納します。
      格納データが左モータのPWMデータのMSByteであれば、「右モータのPWMデータ（符号あり16bit）と右モータのPWMデータ（符号あり16bit）」の計32ビットが更新されたととしてPWM制御行います。
      格納データの次に要求すべき識別番号をreq_idxに記憶して、それを送信データとしてセットしておきます。
      sla_req_idxに記憶される識別番号を次の要求番号へ変更する順番は　1→2→3→0→1→2→3→0→・・・と続く
      （スレーブ側で受信側の読み取りが間に合わないエラー割り込みがあった場合、更新されていない判断しslv_req_idxに1記憶して、それを送信データとしてセットしておきます。）

import spidev
import time

def spi_send(spi: spidev.SpiDev, datas: list):
    # spiで、listのbyte列を送る。　（送信成功で、Trueを返す）
    mas_req_idx:int=0 # 要求されたデータの添え字
    last_idx = len(datas)-1
    fail_count=0
    while True:
        send_data = datas[mas_req_idx]
        rec_data = spi.xfer2([send_data])[0] # 1byte送受信
        print(f"send[{mas_req_idx}]:{send_data},receive: {rec_data}")
        if rec_data > last_idx: return False # スレーブから想定外の受信
        if rec_data == mas_req_idx: # 受信エラーと判断し、先頭から送り直す。
            mas_req_idx = 0
            fail_count+=1 # 失敗数カウント
            if fail_count > 5: return False # 送信失敗
            time.sleep(0.001) # チョット待つ
            continue
        if mas_req_idx == last_idx: return True # 送信成功終了
        mas_req_idx = rec_data # 次のデータの添え字に更新
    #
#

spi = spidev.SpiDev() # SPI操作オブジェクト生成
spi.open(0, 0)        # bus=0 device=0 (CE0) でSPIオープン

spi.max_speed_hz = 7812500 # 7.8 MHzSPIクロック
spi.mode = 0 # クロック信号（SCLK）はLow待機、LowからHighに立ち上がる瞬間でデータを読み取り

value=4847 # 0x12ef
#value=20479 # 0x4fff
data_list = list(value.to_bytes(2, byteorder='big' , signed=True))
value=-479 # 0xfe21
data_list += list(value.to_bytes(2, byteorder='big', signed=True))

print(data_list)
print(spi_send(spi, data_list)) # 送信、確認

while True:
    value = int(input("PWM data>>"))
    data_list = list(value.to_bytes(2, byteorder='big', signed=True))  # 右 PWM
    print(f"{data_list[0]:X}, {data_list[1]:X}")
    data_list += list(value.to_bytes(2, byteorder='big', signed=True)) # 左 PWM
    print(spi_send(spi, data_list)) # 送信、確認
#

上記の
Parspberry PIのSPIマスター用コードからのPWMの情報を受け取るUMEHOSHI ITA基板側のSPIスレーブコードを以下に示します。

//UMEHOSHI ITA基板側のSPIスレーブコード(pwm_pi3ma_spi.c)  
#include <xc.h> 
#include <stdio.h>
#include <stdlib.h>
#include "common.h"
#include <proc/p32mx270f256b.h>
#include <sys/attribs.h>//割込み関係の定義

int reset_on_zero_sequence(int val); // ゼロが連続したときにモータなどを初期化する
void set_PWM(int16_t right_pwm, int16_t left_pwm ); // 右と左のPWMデータをセット

#define LEN 4
int slv_req_idx=1; // 次に要求する識別番号
uint8_t datas[LEN]={0,0,0,0};// 受信データ記憶域

void SPI2_Slave_func(void)// スレーブ用割り込み処理から呼ばれる
{
	/* ---- 受信割り込み ---- */ 
	if (IFS1bits.SPI2RXIF)// SPI2RXIF = 受信バッファにデータが入った 
	{
		//_send_hex_low(TMR2); 		// 確認用
		//_send_hex_low(PR2); 		// 確認用
		//_send_hex_low(IEC0bits.T2IE ); // 確認用
		//_send_hex_low(T2CONbits.ON); // 確認用
		//_send_string("\r\n"); 	// 確認用

		uint8_t val;
		val = SPI2BUF; // SPI受信データを読む(読まないとSPIROVのオーバーフローが発生)

		// 前に要求した識別番号を得てそれが示す受信データとして格納
		if( slv_req_idx == 0) {
			datas[LEN -1] = val; // バッファの最後に要求した受信データを格納
		} else {
			datas[slv_req_idx -1] = val;// 要求した受信データを格納
		}
		// 次に要求するデータを記憶する添え字に更新
		slv_req_idx ++;
		if( slv_req_idx == LEN ){
			 slv_req_idx = 0;
		}

		if( reset_on_zero_sequence(val) ) {// ゼロが連続したときの初期化
			slv_req_idx = 1;// 初期化要求なら次の要求番号を1
			SPI2BUF = slv_req_idx;// 次の送信データをセット
			IFS1bits.SPI2RXIF = 0;   // 受信割り込みフラグクリア
			return;
		}

		while(SPI2STATbits.SPITBE == 0); // 送信バッファ空待ち
		SPI2BUF = slv_req_idx;// 次の送信データをセット

		// バッファに溜まった情報をデコードしてPWMデータに戻す。
		if (slv_req_idx == 1){// PIC32はリトルエンディアンに合わせてデコード
			int16_t right_pwm= ((uint16_t)datas[0] << 8) | datas[1]; 
			int16_t left_pwm = ((uint16_t)datas[2] << 8) | datas[3]; 
			set_PWM(right_pwm, left_pwm );// 右と左のPWMデータをセット
		}
	      IFS1bits.SPI2RXIF = 0;   // 受信割り込みフラグクリア
	}

	/* ---- エラー割り込み ---- */
	if (IFS1bits.SPI2EIF)
	{
		SPI2STATbits.SPIROV = 0;   // 受信オーバーフロー解除
		slv_req_idx = 1;
		SPI2BUF = slv_req_idx;// 次の送信データ先頭用にセット

		IFS1bits.SPI2EIF = 0;      // エラーフラグクリア
		_debug_hex16(9, 0x0ffff  , 1); 
	}
}

int reset_on_zero_sequence(int val) // ゼロが連続したときにモータなどを初期化する
{
	static int zero_count=0;// 初期化判定用　0のbyteを連続5個受け取りで初期化
	if( val == 0){ // zero の連続5個受信で、次の受信を先頭データと初期化
		zero_count++;
		if( zero_count == 5){
			zero_count = 0;

			// モータ全停止
			OC4RS = 0;	// CN7[3-4]制御（正転情報）（RB13）
			OC3RS = 0;	// CN7[1-2]制御（逆転情報）（RB14）
			OC5RS = 0;	// CN6[1-2]制御（正転情報）（RB2）
			OC1RS = 0;	// CN6[3-4]制御（逆転情報）（RB3）
			_debug_hex8(8, 0, 1); // 16ビット受信データ確認
			return 1;
		}
	} else zero_count = 0;
	return 0;
}

void set_PWM(int16_t right_pwm, int16_t left_pwm )// 右と左のPWMデータをセット
{
	_send_decimal(right_pwm,10);// _send_hex_low(right_pwm);	// 確認用
	_send_string(": right_pwm , ");					// 確認用
	_send_decimal(left_pwm,10);	// _send_hex_low(left_pwm);	// 確認用
	_send_string(": left_pwm \r\n");				// 確認用
	//_clear_beep_code(); //デバック用ビープ領域初期化
	//_debug_hex16(0, right_pwm , 1); // 16ビット受信データ確認
	//_debug_hex16(1, left_pwm , 1); // 16ビット受信データ確認
	//OC4RS = 0;//0x3fff; 実験用コード 

	if(right_pwm >= 0){
		OC4RS = right_pwm;	// CN7[3-4]制御（RB13）
		OC3RS = 0x0;		// CN7[1-2]制御（RB14）
	} else {
		OC4RS = 0x0;		// CN7[3-4]制御（RB13）
		OC3RS = -right_pwm;;	// CN7[1-2]制御（RB14）
	}
	if(left_pwm  >= 0){
		OC5RS = left_pwm;	// CN6[1-2]制御（RB2）
		OC1RS = 0x0;	// CN6[3-4]制御（RB3）
	} else {
		OC5RS = 0x0;	// CN6[1-2]制御（RB2）
		OC1RS = -left_pwm;	// CN6[3-4]制御（RB3）
	}
}

// UMEHOSHI ITA のRAMプログラムの設定用プログラム(0x80005000番地より起動)
__attribute__((address( 0x80005000 ))) void start (void);
void start()
{
	//__builtin_disable_interrupts();// PIC32MX270F256B-50I/SPの割り込みをまとめて一括禁止（Disable）
	// これは、コンパイラ（XC32）の組み込み関数（Built-in Function） 「asm volatile ("di");」でも良い

	_clear_beep_code(); // _debug_hex? 関数利用の初期化
	_UM_PTR_GOTO_BEEP = NULL; // debug_hex?の出力をループしない設定

	_set_pwd_mode(1); // PWM モードへ変更（ SPI2の初期化の前に行う。そうしないとPWMが動かない：理由不明）

	_HANDLES[_IDX_SPI2_FUNC] = (void *)SPI2_Slave_func;// 上記のSPI割り込み関数の登録
	((void (**)(void) )_PTR_HANDLERS)[_IDX_INIT_SPI2_SLAVE]();// SPI2の初期化

	slv_req_idx=1;	// 最初設定
	SPI2BUF = slv_req_idx;// 最初の送信データのセット（重要）

	_RB5 = 0; // D1 LED の消灯

	T2CONbits.ON = 1; //Timer2の機能を有効(PWM利用ではこの有効設定が必要)
	PR2=0x09c3F;	// Timer2の周期設定
	//IEC0bits.T2IE = 0; // Timer2 Interrupt Enable を 0 (禁止) にする。（割り込み必要無し)

	//OC4RS = 0x1fff;  //CN7のPWMの値設定確認用

	//_debug_hex4(0,slv_req_idx,1);//  _debug_hex? の動作確認用
	//_send_string("start END\r\n");//起動関数終了の表示

	//__builtin_enable_interrupts();// PIC32MX270F256B-50I/SPの割り込みをまとめて一括許可（Enable）
	// 上記は、組み込み関数（asm volatile ("ei");でも良い　
}

suzuki@raspberrypi:/usr/local/apps $ sudo python  spimaslist.py
[18, 239, 254, 33]　　←　送信予定のリスト
send[0]:18,receive: 1 ←18が1byte送信で、1の受信が要求番号
send[1]:239,receive: 2 ←1の受信で、send[1]の239の1byteを送信
send[2]:254,receive: 3
send[3]:33,receive: 0
True
PWM data>>0　←　以降はキー入力データを送信（0のPWMはモータ停止)
0, 0
send[0]:0,receive: 1
send[1]:0,receive: 2
send[2]:0,receive: 3
send[3]:0,receive: 0
True
PWM data>>-32768 　←　このキー入力値はモータの正転の最大値
80, 0
send[0]:128,receive: 1
send[1]:0,receive: 2
send[2]:128,receive: 3
send[3]:0,receive: 0
True
PWM data>>32767
7F, FF
send[0]:127,receive: 1
send[1]:255,receive: 2
send[2]:127,receive: 3
send[3]:255,receive: 0
True
PWM data>>0
0, 0
send[0]:0,receive: 1
send[1]:0,receive: 2
send[2]:0,receive: 3
send[3]:0,receive: 0
True
PWM data>>

START:80005000		← 起動の表示（80005000番地より実行）





     +4847: right_pwm ,       -479: left_pwm ←32bit受信でモータが回る 





         0: right_pwm ,          0: left_pwm ←0の受信でモータが停止






    -32768: right_pwm ,     -32768: left_pwm ←この受信値はモータ正転最大






    +32767: right_pwm ,     +32767: left_pwm ←この受信値はモータ逆転最大






         0: right_pwm ,          0: left_pwm ←0の受信でモータが停止

RasPiよりPWM情報を上記SPIで送信して、ロボットの倒立振子の検討を行う。

umeusb.send_cmdfile("/usr/local/apps/pwm_pi3ma_spi.c.umh") # SPIでPWM送信するモジュールを初期化

#!/usr/bin/python3 # -*- coding: utf-8 -*- # ssd1306.py のファイル名で、ssd1306を使った128×64ドット有機ELディスプレイをI2Cで操作するdraw_text関数の定義 # 利用する場合、『from SSD1306 import i2c, draw_text』 を記述して使うとよい。 import board import busio from adafruit_ssd1306 import SSD1306_I2C # SSD1306ディスプレイ用 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): '''txtの文字列を、row行目に設定する。 showFlagをFalseにすると表示はしないで、次の表示に使うイメージに内部を更新する。 newImageFlagをTrueにすると、以前の描画イメージをクリアして、新しい文字列として更新する。 ''' 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() if __name__ == '__main__': draw_text(f"UMEHOSHI ITA",0,showFlag=False,newImageFlag=True) draw_text(f"123456789ABCDEFGHIJKLMN",1,showFlag=False) draw_text(f"SSD1306 Display",2)

PWMの幅と角度の関係

import math
# test_pwmexp.py PWMの幅と角度の関係の検討用

# 「現在と目標角の角度の差」を引数にモータを制御する関数
def update_motor_by_angle(current_angle:float, target_angle:float):
   max_pwm = 0x7fff # PWM設定の最大値
   cr=2 # 時定数
   dir='F'
   if current_angle < target_angle: # 逆回転
      dir='B'
      value = int(max_pwm * (1-math.exp(-(target_angle-current_angle)/cr)))
      pwm_list =  list( (-value).to_bytes(2, byteorder='big', signed=True))
   else: # current_angle >= target_angle  正回転
      value = int(max_pwm * (1-math.exp(-(current_angle-target_angle)/cr)))
      pwm_list =  list( value.to_bytes(2, byteorder='big', signed=True))
   #spi_send(spi, pwm_list * 2   ) # PWM送信(左右共通値)
   return value, dir

import numpy as np
import matplotlib.pyplot as plt # pltを使うこと

target_angle=23.5 # 目標角度
xs = np.arange(0, 90 , 0.5)
ys = []
for x in xs:
   y,div=update_motor_by_angle(x, target_angle)
   ys.append( y/0x7fff )

fig = plt.figure() # ◎ファイル保存の準備
plt.plot(xs,ys)
plt.xlabel("Current angle (degree)")
plt.ylabel("Absolute value of PWM data")
plt.title("PWM width relative to angle")
plt.legend() # グラフ内にも上記のlabel表示をつける
plt.grid() # グリッドの表示
fig.savefig("img.png")# ◎表示イメージのファイル保存
plt.show() #画面に表示して閉じるまで待つ（表示表示されると、plt内容はクリア）

前進や後進による移動の判定と目標角の補正値を検討

• 倒立角度変化(青線)で、現在の谷の値が前の谷の値より大きい場合、後ろへ走行していると判定し、-2.0の値のオレンジ線
• 倒立角度変化(青線)で、現在の谷の値が前の谷の値より小さい場合、前へ走行していると判定し、2.0の値のオレンジ線

#!/usr/bin/python3
# -*- coding: utf-8 -*-
#  [Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車のraspipwmspi.pyから呼び出される
# 倒立振子をPWM制御で使われる走行判定モジュール detector2.py

#  前進中か後進中の移動中を判断を、波の勾配（微分値）を使って判断すし、目標角の補正値を取得するクラス
#  一つのキューを同じサイズの2つのキューで実現することで、前半に格納されるキューと、後半に格納されるキューに記憶
#  記憶されるデータ群で、傾きを判断して、山か谷かを判定している。
#  キューへ追加はappend(データ)メソッドで行い、その都度に前半と後半それぞれの合計をsum_formerとsum_latterを記憶して、
#  傾きを計算している。
#  appendの戻り値で、前進中か後進中を判断、倒立目標角度を調整するたも角度を返す。
from collections import deque
class Detector2:
   def __init__(self, size: int = 6):
      self.dq_former = deque()# 前半の値を保持する変数（データはこちらからを入れる。注意：最新情報はこちら）
      self.dq_latter = deque()# 後半の値を保持する変数(注意：キューなのでこちらが時間的に前になる)
      self.size = size
      # 前半と後半の合計値を保持する変数
      self.sum_former = 0.0   # 「差分更新（スライディング・ウィンドウ）」という手法で合計を求める
      self.sum_latter = 0.0
      self.prev_valley = 0 # 前の谷の値
      self.rtnVal = 0 # 戻り値となる目標角度の補正値
      self.isValley = False # もう一つの戻り値で谷の判定でTrue
      self.change_flag = True # 谷の頂点と判断した時、１回だけTrueとするための制御用
   #
   def append(self, data: float) -> float:
      # 前進中か後進中を判断、倒立目標角度を調整するたも角度と、谷と判断した時にTrueを返す。
      self.dq_former.append(data) # 新しいデータを最後に追加       
      self.sum_former += data
      if len(self.dq_former) <= self.size:
         return 0,False
      #　以下は前半のキューが一杯になった後の追加処理
      removed = self.dq_former.popleft() # 前半の先頭の追い出し   
      self.sum_former -= removed
      self.dq_latter.append(removed) # データを後半キューに移動
      self.sum_latter += removed
      # 後半キューが一杯になるまでの処理
      if len(self.dq_latter) <= self.size:
         return 0,False # 後半キューが一杯になっていない。
      removed = self.dq_latter.popleft() # 後半キューの先頭の追い出し   
      self.sum_latter -= removed
      #        
      # -----以下は前半と後半が一杯になった以降の処理になる------
      tilt_former = self.dq_former[-1]-self.dq_former[0] # 最近の傾き
      tilt_latter = self.dq_latter[-1]-self.dq_latter[0] # 直前の傾き
      peak_flag = tilt_latter > 0 and tilt_former < 0 # 山の頂点
      valley_flag = tilt_latter < -0.07 and tilt_former > 0.07 # 谷の頂点
      if abs(self.sum_former-self.sum_latter) > 2: # 頂点でないと判断を修正
          peak_flag = False
          valley_flag = False
      #
      # print(f"self.sum_former-self.sum_latter:{self.sum_former-self.sum_latter}") # ２つのキューの合計比較
      self.isValley=False
      if peak_flag :                  # -------------------上に山の変曲点 
         self.change_flag = True
      elif valley_flag :              # -------------------下に谷の変曲点
         if self.change_flag :
            self.change_flag = False
            if self.prev_valley != 0 and data>self.prev_valley+1: # 谷の変曲点が以前が上がっている？
                  #self.rtnVal = -2.0 # 後進と判断した時の補正値
                  self.rtnVal = (self.prev_valley-data) # 負の値
            elif self.prev_valley != 0 and data<self.prev_valley-1: # 谷の変曲点が以前が下がっている？
                  #self.rtnVal = 2.0 # 前進と判断した時の補正値
                  self.rtnVal = (self.prev_valley-data)  # 正の値
            else: self.rtnVal = 0
            #
            self.prev_valley = data
            self.isValley=True
         #           
         self.prev_valley = data # 谷の頂点を記憶を比較のために記憶
      #
      return self.rtnVal, self.isValley
   #
   def sum_tilt_former(self):
      return self.dq_former[-1]-self.dq_former[0] # 最近の傾き

if __name__ == '__main__':
   import matplotlib.pyplot as plt
   # テスト
   detector = Detector2(size=6)

   def pitch_txt_load():
      with open('../log2.txt', 'r') as fr: # ロボット動作の制御ログ情報
      #with open('log20260410.txt', 'r') as fr: # ロボット動作の制御ログ情報
         lines = fr.readlines()
      tims = [] # 横軸（倒立角度の想定時間）
      pitchs = [] # 倒立角度の測定値
      ajvs = [] # 前進・後進を判定し、その結果情報（Adjustment values）
      timing=0
      for s in lines:
         items = s.split(",")
         if not (items[0][0] in ['F','B','S']): continue
         v = items[1][len("pitch:"):]
         pitchs.append(float(v))
         #
         ajv, b = detector.append(float(v)) # ファイルから判定器してから取得()
         #ajv = float(items[4][len("ajv:"):]) # 判定器を使った実行結果のファイルより取得
         #
         ajvs.append(ajv) # 調整値（判定値
         #
         v = items[2][len("periodo:"):]
         timing += float(v)
         tims.append(timing)    
      return tims,pitchs,ajvs

   tims, pitchs, ajvs = pitch_txt_load()
   fig = plt.figure() # ◎ファイル保存の準備
   plt.plot(tims, pitchs,label="pitchs") 
   plt.plot(tims, ajvs,label="ajvs") 
   plt.legend() # グラフ内にも上記のlabel表示をつける
   plt.grid() # グリッドの表示
   fig.savefig("img.png")# ◎表示イメージのファイル保存
   plt.show() #画面に表示して閉じるまで待つ

#!/usr/bin/python3
# -*- coding: utf-8 -*-
#  [Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車のサービスから呼び出される
# 倒立振子をPWM制御で試すコード raspipwmspi.py

import os # ファイル有無を調べるため追加 ★
import spidev
import time
import math
import socket
import umetcp
import umeusb
from ssd1306 import i2c, draw_text
import threading
import subprocess
from detector2 import Detector2

def spi_send(spi: spidev.SpiDev, datas: list):
    # spiで、listのbyte列を送る。　（送信成功で、Trueを返す）
    mas_req_idx:int=0 # 要求されたデータの添え字
    last_idx = len(datas)-1
    fail_count=0
    while True:
        send_data = datas[mas_req_idx]
        rec_data = spi.xfer2([send_data])[0] # 1byte送受信
        # print(f"send[{mas_req_idx}]:{send_data},receive: {rec_data}")
        if rec_data > last_idx: return False # スレーブから想定外の受信
        if rec_data == mas_req_idx: # 受信エラーと判断し、先頭から送り直す。
            print(f"----- Error rec_data:{ rec_data }")
            mas_req_idx = 0
            fail_count+=1 # 失敗数カウント
            if fail_count > 5: return False # 送信失敗
            time.sleep(0.001) # チョット待つ
            continue
        if mas_req_idx == last_idx: return True # 送信成功終了
        mas_req_idx = rec_data # 次のデータの添え字に更新
    #
#

spi = spidev.SpiDev() # SPI操作オブジェクト生成
spi.open(0, 0)        # bus=0 device=0 (CE0) でSPIオープン

spi.max_speed_hz = 7812500 # 7.8 MHzSPIクロック
spi.mode = 0 # クロック信号（SCLK）はLow待機、LowからHighに立ち上がる瞬間でデータを読み取り

'''
value=0x0123
pwm_list =  list(value.to_bytes(2, byteorder='big', signed=True))
spi_send(spi, pwm_list * 2   ) # PWM送信、確認
'''

# 「現在と目標角の角度の差」を引数にモータを制御する関数
def update_motor_by_angle(current_angle:float, target_angle:float):
   max_pwm = 0x7fff # PWM設定の最大値
   #cr=2 # 時定数
   cr=6 # 時定数 に変更
   dir='F'
   if current_angle < target_angle: # 逆回転
      dir='B'
      value = int(max_pwm * (1-math.exp(-(target_angle-current_angle)/cr)))
      pwm_list =  list( (-value).to_bytes(2, byteorder='big', signed=True))
   else: # current_angle >= target_angle  正回転
      value = int(max_pwm * (1-math.exp(-(current_angle-target_angle)/cr)))
      pwm_list =  list( value.to_bytes(2, byteorder='big', signed=True))
   spi_send(spi, pwm_list * 2   ) # PWM送信(左右共通値)
   return value, dir

# umeusb.send_cmdfile("/usr/local/apps/uStartInit.umh") # ロボット初期化("R009D020010004E")
# umeusb.send_cmdfile("/usr/local/apps/pwm_pi3ma_spi.c.umh") # ロボット初期化("R009D020010004E")
server_addr =(umetcp.get_wlan0_ip(), 59154)
hostname=socket.gethostname()

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)

# ---- 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初期化 終了 -------------------------------------


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

# 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を記憶

#  VL53L1X使用 レーザー測距センサー-----------------------------------------------------
import adafruit_vl53l1x # VL53L1X使用 レーザー測距センサーモジュール用
# オープンソースハードウェアの設計・製造・販売を行うアメリカの企業のAdafruit（エイダフルート）モジュール利用

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")

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]},avg:{t[4]},ajv:{t[5]}\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.2 # 制御開始前のモータ前進を行う期間(秒)
   target_angle=23.6 # 目標角度　15→ 20→ 25← 23→23.2 23.4
   results = [] # 制御履歴を残すリスト（制御終了時にファイル化）
   detector = None # 変更点判定
   ajv=0 # target_angleに対する調整値
   pwmV=0 # モータ制御用のPWM設定値
   

   time_now=0 # 現在の測定時間
   time_bak=0 # 一つ前の測定時間
   fw = None
   #
   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, last_str="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
         detector = Detector2(size=6)
         ajv = 0
      #
      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: # 倒立制御に入る前の処理
         pwmV=20479 # 0x4fff
         if time.time() < start_control_time + start_forward_time: # Forward 最初の助走
            spi_send(spi, list((pwmV).to_bytes(2, byteorder='big', signed=True))*2) # 前転
         else: # Back 最初の倒立のための逆転
            pwmV=19000 # ★　逆転のデューティ比を小さく変更
            spi_send(spi, list((-pwmV).to_bytes(2, byteorder='big', signed=True)) * 2) # 逆転
            results.append( ( f'B{pwmV}', startM, pitch, endM-startM, 0, ajv ) ) # ファイル化情報  
            if pitch > target_angle: # 倒立角度に達した？
               flag_control = True
               results.append( ( f'S{0}', startM, pitch, endM-startM, 0, ajv ) ) # ファイル化情報  
            elif time.time() > start_control_time + start_forward_time + 0.5: # 倒立しない。
               pwmV=0 
               spi_send(spi, list((pwmV).to_bytes(2, byteorder='big', signed=True)) * 2) # ストップ
               flag_push = False        
      #
      deadband=0
      if flag_control: # 倒立振子制御中
         ajv, b = detector.append(pitch) # 走行方向の判定、調整値取得
         if ajv < -10: ajv = 0.0 # ★ 大きすぎる時は、異常と判断して無視
         ajv *= 0.2 # ★ 目標補正値の倍率が0.5では大きいと判断し、0.2に変更
         pwmV,dir=update_motor_by_angle(pitch, target_angle + ajv) # モータのPWM制御
         results.append( ( f'{dir}{pwmV}', startM, pitch, endM-startM, 0, ajv ) ) # ファイル化情報
         #
         if pitch < 5 or pitch > 70 or startM > start_control_time + 10: #この時間で制御を終了
            spi_send(spi, list((0).to_bytes(2, byteorder='big', signed=True)) * 2) # ストップ
            flag_push = False
            flag_control = False
            save_results(results, last_str=f"End pitch:{pitch} time:{startM-start_control_time}.\n") # 結果を保存
         #
   #

t_id2 = threading.Thread(target=measure_loop)
t_id2.start()

のXへの動画リンク

以前の指数係数（cr=2）	変更後の指数係数（cr=6）