Memorandum code for Raspberry Pi
filename.py
import pigpio
import time
import signal
import csv
#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼st Tuning element
wfilename='wlogfile_1.csv'
rfilename='rlogfile_1.csv'
time_HI_STR_N=1520#HIGH time in steer side input neutral state[us]
time_HI_STR_MAX=2020
time_HI_STR_MIN=1020
time_HI_DRV_N=1520#Drive side input Neutral state HIGH time[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 time[us]× Constant = Duty ratio
DRV_TH=1*10000 #Dead zone threshold[%*10000]
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲ ed Tuning element
#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼ No change
#Pin number setting
#Two hardware PWM compatible pins for output cannot be changed
pin_PWM_STR=12
pin_PWM_DRVF=13
pin_PWM_DRVR=19
#Changeable for input
pin_IN_STR=16
pin_IN_DRV=20
#Create a pigpio instance
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 time[us]× Constant = Duty command value[%*10^4]What
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲ ed Hard setting No change
#▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼
#Variable for input duty calculation
time_UP_STR=[0,0]
time_DW_STR=[0,0]
time_UP_DRV=[0,0]
time_DW_DRV=[0,0]
#Input duty ratio recording variable
duty_IN_STR=0
duty_IN_DRV=0
#Input HIGH time recording variable
time_HI_STR=0
time_HI_DRV=0
#State management
mod_req=0
mod_state=0
MODE_INIT=0
MODE_LOGING=1
MODE_REPLAY=2
duty_OUT_STR=0
duty_OUT_DRV=0
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲ ed Variable setting
#▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼
#PWM input for steering interrupt callback
def callback_IN(gpio,level,tick):
#Procedure for using global variables
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:#Interruption caused by steering
if level == 1:#Rise
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:#Fall down
time_DW_STR[1]=time_DW_STR[0]
time_DW_STR[0]=tick
else:#Interrupt is not caused by steer (caused by driving)
if level == 1:#Rise
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:#Fall down
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 Input interrupt
#▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ interrupt
def logic_main(arg1, arg2):
#Global variable usage procedure
global time_HI_STR#[us]
global time_HI_DRV#[us]
global time_HI_STRtmp#[us]
global time_HI_DRVtmp#[us]
global index,index_lim
#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼
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("End of playback! Press the enter key")
index=index+1
else:
duty_OUT_STR=0
duty_OUT_DRV=0
else:
#Steer operation amount
duty_OUT_STR=int(max(min(time_HI_STRtmp*HITIME_TO_OUTDUTY,100*10000),0))#Upper and lower limit limits and conversion
#Drive amount
duty_OUT_DRV=int(max(min((time_HI_DRVtmp-time_HI_DRV_N)*GAIN_DRV,DUTY_MOT_MAX),-DUTY_MOT_MAX)*10000)
#output
pi.hardware_PWM(pin_PWM_STR,frec_PWM_STR,duty_OUT_STR)
if duty_OUT_DRV>DRV_TH:#Forward rotation
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:#Reversal
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:#Stop
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])
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲ interrupt
#▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼
pi.callback(pin_IN_STR,pigpio.EITHER_EDGE,callback_IN)#Steer side
pi.callback(pin_IN_DRV,pigpio.EITHER_EDGE,callback_IN)#Drive side
signal.signal(signal.SIGALRM,logic_main)#Main logic execution and setting (timer interrupt) when a signal comes
signal.setitimer(signal.ITIMER_REAL,0.1,0.02)#Timer interrupt with 50Hz signal
#▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲ ed interrupt setting
#▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼
time_HI_STRtmp=5000
time_HI_DRVtmp=5000
while 1:
print('1:Logging 2:Playback 3:End')
try:
tmp_req=input('> ')
except KeyboardInterrupt:
print("stop!")
break
try:
mod_req=int(tmp_req)
except:
print("miss")
break
#start
if mod_req==MODE_LOGING:
file=open(wfilename,'w')
w=csv.writer(file)
mod_state=MODE_LOGING
print("Logging...")
print("Press enter to stop")
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("Playing...")
else:
print("END!")
break
#End
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)
#▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼ ▼
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 Termination procedure