A program that receives the servo command of the radio control, interrupts the Raspberry Pi and logs it

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



Recommended Posts

A program that receives the servo command of the radio control, interrupts the Raspberry Pi and logs it
Make a thermometer with Raspberry Pi and make it visible on the browser Part 3
[Python] A program that calculates the number of updates of the highest and lowest records
How to make a Raspberry Pi that speaks the tweets of the specified user
[Python] A program that counts the number of valleys
I made a system with Raspberry Pi that regularly measures the discomfort index of the room and sends a LINE notification if it is a dangerous value
A story about getting the Atom field (XML telegram) of the Japan Meteorological Agency with Raspberry Pi and tweeting it
[Python] A program that compares the positions of kangaroos.
Easily make a TweetBot that notifies you of temperature and humidity with Raspberry Pi + DHT11.
[Python] A program to find the number of apples and oranges that can be harvested
Production of temperature control system with Raspberry Pi and ESP32 (1)
Create a partition and then install the Raspberry Pi OS
A program that just presses and releases the Esc key
Finally ... Make a radio control using python on Raspberry Pi 3! (When the magnet is brought closer, the motor moves and stops automatically)
Control the motor with a motor driver using python on Raspberry Pi 3!
Make a thermometer with Raspberry Pi and make it viewable with a browser Part 4
Try to write a program that abuses the program and sends 100 emails
A program that answers a few questions and predicts the next answer
[Python] A program that rotates the contents of the list to the left
Servo motor control with Raspberry Pi
[Python / Jupyter] Translate the comment of the program copied to the clipboard and insert it in a new cell
Finally ... Make a radio control using python on Raspberry Pi 3! (The motor moves while the button is pressed)
Read the data of the NFC reader connected to Raspberry Pi 3 with Python and send it to openFrameworks with OSC
The story of writing a program
Upload data to s3 of aws with a command and update it, and delete the used data (on the way)
[Python] A program that calculates the number of chocolate segments that meet the conditions
[Python] A program that calculates the number of socks to be paired
A python program that resizes a video and turns it into an image
Production of temperature control system with Raspberry Pi and ESP32 (2) Production of transmission device
Finally ... Make a radio control using python on Raspberry Pi 3! (The motor moves while the magnet is brought closer)
A simple mock server that simply embeds the HTTP request header in the body of the response and returns it.
Gently explain the process of making a simple serverless surveillance camera using Raspberry Pi, Gmail API and Line API
Create a simple app that incorporates the Fetch API of Ajax requests in Flask and explain it quickly
This and that of the inclusion notation.
[Raspberry Pi] Add a thermometer and a hygrometer
Notice the completion of a time-consuming command
[Python] A program that rounds the score
Delete files that have passed a certain period of time with Raspberry PI
Make a Python program a daemon and run it automatically when the OS starts
Connect to the console of Raspberry PI and display local IP and SD information
I tweeted the illuminance of the room with Raspberry Pi, Arduino and optical sensor
The story of making a box that interconnects Pepper's AL Memory and MQTT
Publishing and using a program that automatically collects facial images of specified people
Creating a temperature control system with Raspberry Pi and ESP32 (3) Recipient Python file
[Python] A program that finds the minimum and maximum values without using methods
Use python on Raspberry Pi 3 and turn on the LED when it gets dark!
I made a Python program for Raspberry Pi that operates Omron's environmental sensor in the mode with data storage
A story that I wanted to realize the identification of parking lot fullness information using images obtained with a Web camera and Raspberry Pi and deep learning.