ラジコンのサーボ司令を受け取りラズパイを割り込ませてロギングするプログラム

ラズパイ用備忘録的コード

filename.py

import pigpio
import time
import signal
import csv

#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼　st チューニング要素
wfilename='wlogfile_1.csv'
rfilename='rlogfile_1.csv'

time_HI_STR_N=1520#ステア側入力ニュートラル状態のHIGH時間[us]
time_HI_STR_MAX=2020
time_HI_STR_MIN=1020
time_HI_DRV_N=1520#駆動側入力ニュートラル状態のHIGH時間[us]
time_HI_DRV_MAX=2020
time_HI_DRV_MIN=1020
DUTY_MOT_MAX=20

frec_PWM_STR=50#Hz
frec_PWM_DRV=50#Hz
frec_logic=50#Hz

GAIN_DRV=DUTY_MOT_MAX/(time_HI_DRV_N-time_HI_DRV_MIN) #HIGH時間[us]×定数＝デューティー比
DRV_TH=1*10000 #不感帯スレッショルド[%*10000]

#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲　ed チューニング要素


#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼　st　ハード設定　変更禁止
#ピン番号設定
#出力用　ハードウェアPWM対応ピン２つ　変更不可
pin_PWM_STR=12
pin_PWM_DRVF=13
pin_PWM_DRVR=19
#入力用　変更可
pin_IN_STR=16
pin_IN_DRV=20

#pigpioインスタンス作成
pi=pigpio.pi()

#output pin setup
pi.set_mode(pin_PWM_STR,pigpio.OUTPUT)
pi.set_mode(pin_PWM_DRVF,pigpio.OUTPUT)
pi.set_mode(pin_PWM_DRVR,pigpio.OUTPUT)

#input pin setup
pi.set_mode(pin_IN_STR,pigpio.INPUT)
pi.set_pull_up_down(pin_IN_STR,pigpio.PUD_DOWN)
pi.set_mode(pin_IN_DRV,pigpio.INPUT)
pi.set_pull_up_down(pin_IN_DRV,pigpio.PUD_DOWN)

HITIME_TO_OUTDUTY=frec_PWM_STR/(1000*1000)*100*10000#HIGHの時間[us]×定数＝デューティー指令値[%*10^4]へ
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲　ed ハード設定　変更禁止

#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼　st　変数設定
#入力デューティー算出用変数
time_UP_STR=[0,0]
time_DW_STR=[0,0]
time_UP_DRV=[0,0]
time_DW_DRV=[0,0]

#入力デューティー比記録用変数
duty_IN_STR=0
duty_IN_DRV=0
#入力HIGH時間記録用変数
time_HI_STR=0
time_HI_DRV=0

#状態管理
mod_req=0
mod_state=0
MODE_INIT=0
MODE_LOGING=1
MODE_REPLAY=2

duty_OUT_STR=0
duty_OUT_DRV=0


#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲　ed 変数設定

#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼　st　入力割り込み
#ステアリング用PWM入力　割り込みコールバック
def callback_IN(gpio,level,tick):
    #グローバル変数の使用手続き
    global time_UP_STR
    global time_DW_STR
    global duty_IN_STR
    global time_HI_STR
    global time_UP_DRV
    global time_DW_DRV
    global duty_IN_DRV
    global time_HI_DRV

    if gpio==pin_IN_STR:#割り込みがステアリング起因
        if level == 1:#立ち上がり
            time_UP_STR[1]=time_UP_STR[0]
            time_UP_STR[0]=tick
            duty_IN_STR=(time_DW_STR[0]-time_UP_STR[1])/(time_UP_STR[0]-time_UP_STR[1])*100
            time_HI_STR=(time_DW_STR[0]-time_UP_STR[1])
        else:#立ち下がり
            time_DW_STR[1]=time_DW_STR[0]
            time_DW_STR[0]=tick
    else:#割り込みがステア起因でない（駆動起因）
        if level == 1:#立ち上がり
            time_UP_DRV[1]=time_UP_DRV[0]
            time_UP_DRV[0]=tick
            duty_IN_DRV=(time_DW_DRV[0]-time_UP_DRV[1])/(time_UP_DRV[0]-time_UP_DRV[1])*100
            time_HI_DRV=(time_DW_DRV[0]-time_UP_DRV[1])
        else:#立ち下がり
            time_DW_DRV[1]=time_DW_DRV[0]
            time_DW_DRV[0]=tick

    #print(time_HI_STR,time_HI_DRV,duty_IN_STR,duty_IN_DRV)
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲　ed 入力割り込み


#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼　st　出力用割り込み
def logic_main(arg1, arg2):
    #グローバル変数使用手続き
    global time_HI_STR#[us]
    global time_HI_DRV#[us]
    global time_HI_STRtmp#[us]
    global time_HI_DRVtmp#[us]
    global index,index_lim
    #▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼　st　ロジック本体


    duty_OUT_STR=0
    duty_OUT_DRV=0

    if mod_state==MODE_REPLAY:
        if index<index_lim:
            duty_OUT_STR=int(data[index][0])
            duty_OUT_DRV=int(data[index][1])
            index=index+1
        elif index==index_lim:
            print("再生おわり！エンターキーを押してね")
            index=index+1
        else:
            duty_OUT_STR=0
            duty_OUT_DRV=0

    else:
        #ステア操作量
        duty_OUT_STR=int(max(min(time_HI_STRtmp*HITIME_TO_OUTDUTY,100*10000),0))#上下限制限と変換
        #駆動量
        duty_OUT_DRV=int(max(min((time_HI_DRVtmp-time_HI_DRV_N)*GAIN_DRV,DUTY_MOT_MAX),-DUTY_MOT_MAX)*10000)

    #出力
    pi.hardware_PWM(pin_PWM_STR,frec_PWM_STR,duty_OUT_STR)
    if duty_OUT_DRV>DRV_TH:#正転
        pi.hardware_PWM(pin_PWM_DRVF,frec_PWM_DRV,duty_OUT_DRV)
        pi.write(pin_PWM_DRVR,0)
        #print(duty_OUT_STR,duty_OUT_DRV,"F")
    elif duty_OUT_DRV<-DRV_TH:#逆転
        pi.write(pin_PWM_DRVF,0)
        pi.hardware_PWM(pin_PWM_DRVR,frec_PWM_DRV,-duty_OUT_DRV)
        #print(duty_OUT_STR,duty_OUT_DRV,"R")
    else:#停止
        pi.write(pin_PWM_DRVF,0)
        pi.write(pin_PWM_DRVR,0)
        #print(duty_OUT_STR,duty_OUT_DRV)

    if mod_state==MODE_LOGING:
        w.writerow([duty_OUT_STR,duty_OUT_DRV])

    #▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲　ed ロジック本体
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲　ed 出力用割り込み


#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼　st　割り込み設定
pi.callback(pin_IN_STR,pigpio.EITHER_EDGE,callback_IN)#ステア側
pi.callback(pin_IN_DRV,pigpio.EITHER_EDGE,callback_IN)#駆動側
signal.signal(signal.SIGALRM,logic_main)#シグナルが来たらメインロジック実行と設定（タイマ割り込み）
signal.setitimer(signal.ITIMER_REAL,0.1,0.02)#50Hzのシグナルでタイマ割り込み
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲　ed 割り込み設定


#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼　st　メイン処理

time_HI_STRtmp=5000
time_HI_DRVtmp=5000

while 1:
    print('1:ロギング　2:再生　３:終了')
    try:
        tmp_req=input('> ')
    except KeyboardInterrupt:
        print("stop!")
        break

    try:
        mod_req=int(tmp_req)
    except:
        print("miss")
        break
#開始    
    if mod_req==MODE_LOGING:
        file=open(wfilename,'w')
        w=csv.writer(file)
        mod_state=MODE_LOGING
        print("ロギング中...")
        print("停止するにはエンターキーを押してね")

    elif mod_req==MODE_REPLAY:
        file=open(rfilename,'r')
        r=csv.reader(file)
        data=[row for row in r]
        index=0
        index_lim=len(data)
        mod_state=MODE_REPLAY
        print("再生中...")
    else:
        print("END!")
        break


#終了
    try:
        input('> ')
    except KeyboardInterrupt:
        print("stop!")
        break

    if mod_req==MODE_LOGING:
        mod_state=MODE_INIT
        mod_req=MODE_INIT
        file.close()
    elif mod_req==MODE_REPLAY:
        mod_state=MODE_INIT
        mod_req=MODE_INIT
        file.close()
    else:
        print("END!")
        break

time.sleep(0.1)
#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼　st　終了手続き
pi.set_mode(pin_PWM_STR,pigpio.INPUT)
pi.set_mode(pin_PWM_DRVF,pigpio.INPUT)
pi.set_mode(pin_PWM_DRVR,pigpio.INPUT)
pi.stop()
signal.setitimer(signal.ITIMER_REAL,0)
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲　ed 終了手続き