ラズパイでAGVを制御する（その2）【サンプルコード付き】：ラズパイで製造業のお手軽IoT活用（9）（4/4 ページ）

[株式会社アムイ 代表取締役 山田浩貢，MONOist]

※本記事はアフィリエイトプログラムによる収益を得ています

記事に使用したサンプルコードのテキスト

#!/usr/bin/env python				
# -*- coding: utf-8 -*-				
				
import time				
import math				
import threading				
import seigyo				
from time import sleep				
from datetime import datetime				
from pathlib import Path				
import sys				
import concurrent.futures				
import rospy				
from sensor_msgs.msg import Image 				
from cv_bridge import CvBridge				
import cv2				
from pyzbar.pyzbar import decode				
import numpy as np				
				
import RPi.GPIO as GPIO				
GPIO.setwarnings(False)				
				
GPIO.setmode(GPIO.BOARD)				
GPIO.cleanup()				
				
#GPIOへピン番号でアクセスすることを宣言				
GPIO.setmode(GPIO.BOARD)				
				
#正転時のGPIOピン番号を指定				
#右前輪				
Right_front = 11				
#左前輪				
Left_front = 16				
#右後輪				
Right_rear = 29				
#左後輪				
Left_rear = 33				
				
#後転時のGPIOピン番号を指定				
#右前輪				
Right_front_back = 13				
#左前輪				
Left_front_back = 18				
#右後輪				
Right_rear_back = 31				
#左後輪				
Left_rear_back = 35				
				
#GPIOピン番号を配列に格納				
read = [Right_front, Left_front, Right_rear, Left_rear]				
back = [Right_front_back, Left_front_back, Right_rear_back, Left_rear_back]				
left = [Right_front, Left_front_back, Right_rear, Left_rear_back]				
right = [Right_front_back, Left_front, Right_rear_back, Left_rear]				
				
#超音波センサのGPIOピン番号を指定				
#超音波センサ出力				
TRIG = 36				
#超音波センサ受信				
ECHO = 38				
				
#超音波センサに出力に設定				
GPIO.setup(TRIG,GPIO.OUT)				
				
#超音波センサに受信に設定				
GPIO.setup(ECHO,GPIO.IN)				
				
#障害物との距離が30cm未満になれば停止する				
flg = 0				
def ult():				
	global flg			
	while True:			
		if flg == 0:		
			GPIO.setup(TRIG,GPIO.OUT)	
			GPIO.setup(ECHO,GPIO.IN)	
				
			#GPIOに出力	
			GPIO.output(TRIG, GPIO.LOW)	
			time.sleep(0.3)	
			GPIO.output(TRIG, GPIO.HIGH)	
			#処理を一旦停止する	
			time.sleep(0.00001)	
			#GPIO.output(TRIG, False)	
			GPIO.output(TRIG, GPIO.LOW)	
			#超音波を出して距離を測定	
			while GPIO.input(ECHO) == 0:	
				signaloff = time.time()
				
			while GPIO.input(ECHO) == 1:	
				signalon = time.time()
			timepassed = signalon - signaloff	
			#測定した距離をcmの値に変換	
			distance = timepassed * 17000	
				
			#測定した値を整数値に変換	
			measure=int(distance)	
				
			if measure < 20:	
				#一時停止
				flg = 1
				print('障害物までの距離が20cm未満になったので停止します。')
				
			elif flg == 4:	
				GPIO.cleanup()
				sys.exit()
				
#前進・後退・右左折の制御				
def adv():				
	global flg			
	while True:			
		if flg == 0:		
			seigyo.go()	
		elif flg == 1:		
			seigyo.stop()	
			seigyo.avoid()	
			seigyo.stop()	
			flg =0	
		elif flg == 2:		
			print('左に曲がります。')	
			seigyo.stop()	
			seigyo.turn_ll()	
			seigyo.stop()	
			flg =0	
		elif flg == 3:		
			print('右に曲がります。')	
			seigyo.stop()	
			seigyo.turn_rr()	
			seigyo.stop()	
			flg =0	
		elif flg == 4:		
			print('制御を終了します。')	
			sys.exit()	
				
#QR読み取りの制御				
def process_image(msg):				
	try:			
		global flg		
		if flg == 0:		
			bridge = CvBridge()	
			orig = bridge.imgmsg_to_cv2(msg, "bgr8")	
			img = cv2.cvtColor(orig, cv2.COLOR_BGR2GRAY)	
			data = decode(img)	
				
			code = data[0][0].decode('utf-8', 'ignore')	
			print(code)	
				
			#左折	
			if code == "left":	
				flg = 2
			#右折	
			elif code == "right":	
				flg = 3
			#停止	
			elif code == "stop":	
				flg = 4
				sys.exit()
	except Exception as err:			
		pass		
				
def start_node():				
	rospy.spin()			
				
if __name__ == "__main__":				
	try:			
				
		rospy.init_node('img_proc')		
		rospy.loginfo('img_proc node started')		
		rospy.Subscriber("image_raw", Image, process_image)		
				
		th1 = threading.Thread(target=start_node)		
		th1.setDaemon(True)		
		th1.start()		
				
		th2 = threading.Thread(target=adv)		
		th2.start()		
				
		th3 = threading.Thread(target=ult)		
		th3.setDaemon(True)		
		th3.start()		
				
	#例外処理(Ctrl-C含む)			
				
	except KeyboardInterrupt:			
		#GPIOポート開放		
		print('終了')		
		GPIO.cleanup()		

リスト1　「main.py」のサンプルコード

#!/usr/bin/env python			
# -*- coding: utf-8 -*-			
			
#GPIOにアクセスするライブラリをimport			
import RPi.GPIO as GPIO			
import time			
GPIO.setwarnings(False)			
			
#GPIOへピン番号でアクセスすることを宣言			
GPIO.setmode(GPIO.BOARD)			
			
#前進処理			
def go():			
	GPIO.setup(read, GPIO.OUT)		
	GPIO.output(read, GPIO.HIGH)		
	time.sleep(0.01)		
	GPIO.output(read, GPIO.LOW)		
	time.sleep(0.01)		
	GPIO.cleanup(read)		
			
#後退処理			
def rese():			
	GPIO.setup(back, GPIO.OUT)		
	GPIO.output(back, GPIO.HIGH)		
	time.sleep(0.01)		
	GPIO.output(back, GPIO.LOW)		
	time.sleep(0.01)		
	GPIO.cleanup(back)		
			
#左折処理			
def turn_left():			
	GPIO.setup(left, GPIO.OUT)		
	GPIO.output(left, GPIO.HIGH)		
	time.sleep(0.01)		
	GPIO.output(left, GPIO.LOW)		
	time.sleep(0.01)		
	GPIO.cleanup(left)		
			
#右折処理			
def turn_right():			
	GPIO.setup(right, GPIO.OUT)		
	GPIO.output(right, GPIO.HIGH)		
	time.sleep(0.01)		
	GPIO.output(right, GPIO.LOW)		
	time.sleep(0.01)		
	GPIO.cleanup(right)		
			
#停止確認処理			
def stop():			
	time.sleep(1.0)		
			
#回転の制御			
			
#strは直進する時間			
str = 100			
#ttimeは回転する時間			
ttime = 60			
#shortは回転した後に進む時間			
short = 60			
#hidは後退する時間			
hid = 15			
			
def turn_r():			
	for i in range(ttime):		
		turn_right()	
	stop()		
	for i in range(short):		
		go()	
	stop()		
			
def turn_rr():			
	for i in range(ttime):		
		turn_right()	
			
def turn_ll():			
	for i in range(ttime):		
		turn_left()	
	stop()		
	for i in range(str):		
		go()	
	stop()		
def turn_l():			
	for i in range(ttime):		
		turn_left()	
	stop()		
	for i in range(short):		
		go()	
	stop()		
			
def leave():			
	for i in range(hid):		
		rese()	
	stop()		
			
def adv():			
	while True:		
			go()
			
def avoid():			
	stop()		
	leave()		
	turn_r()		
	turn_ll()		
	turn_l()		
	turn_rr()		

リスト2　「seigyo.py」のサンプルコード

本連載執筆の山田浩貢氏のIoT関連書籍が好評発売中です！

筆者紹介

株式会社アムイ 代表取締役
山田 浩貢（やまだ　ひろつぐ）

NTTデータ東海にて1990年代前半より製造業における生産管理パッケージシステムの企画開発・ユーザー適用および大手自動車部品メーカーを中心とした生産系業務改革、

原価企画・原価管理システム構築のプロジェクトマネージメントに従事。2013年に株式会社アムイを設立し大手から中堅中小製造業の業務改革、業務改善に伴うIT推進コンサルティングを手掛けている。「現場目線でのものづくり強化と経営効率向上にITを生かす」活動を展開中。