Raspberry Pi 2Pin = Register 1K pin = Switch first pin
Raspberry Pi 4pin = ultrasonic sensor Vcc pin
Raspberry Pi 5pin = Switch second pin
Raspberry Pi 6pin = ultrasonic sensor GND pin
Raspberry Pi 16pin = ultrasonic sensor Trig pin
Raspberry Pi 18pin = Register 2K pin = ultrasonic sensor Echo pin
Raspberry Pi 20pin = Register 1K pin = ultrasonic sensor Echo pin
Raspberry Pi 39pin = Switch third pin

import RPi.GPIO as GPIO
from time import sleep
import os
import argparse
import cv2
import numpy as np
import sys
import time
from threading import Thread
import importlib.util
import signal

left_switch = 3 #Switch assumed as signal light

#GPIO Setting
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(left_switch,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(right_switch,GPIO.IN,GPIO.PUD_UP)

def distanceInCm(duration): #Function that returns duration as distance in cm
return (duration/2)/29.1

def print_distance(distance): #A function that sets the distanceMsg differently depending on the distance value.
if distance == 0:
distanceMsg = ‘Distance : out of range \r’
else:
distanceMsg = ‘Distance : ‘+str(distance)+’cm’+’ \r’
return distanceMsg

#Set pin number of ultrasonic sensor
TRIG = 23 #TRIG Numbering
ECHO = 24 #ECHO Numbering

MAX_DISTANCE_CM = 300
MAX_DURATION_TIMEOUT = (MAX_DISTANCE_CM * 2 * 29.1)

GPIO.setmode(GPIO.BCM)

GPIO.setup(TRIG, GPIO.OUT) #TRIG Ultrasonic Signal Transmission Pin Designation and Specify Output
GPIO.setup(ECHO, GPIO.IN) #Specify ECHO Ultrasonic Signal Receiving Pin and Specify Output

#Thread to manage webcam
class VideoStream:
“”"Camera object that controls video streaming from the Picamera”"”
def __init__(self, resolution=(640,480), framerate=30):
self.stream = cv2.VideoCapture(0) #Trun on the camera
ret = self.stream.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*’MJPG’))
#frame size init
ret = self.stream.set(3, resolution[0])
ret = self.stream.set(4, resolution[1])

# Read first frame from the stream
(self.grabbed, self.frame) = self.stream.read()

# Variable to control when the camera is stopped
self.stopped = False

def start(self): # Start the thread that reads frames from the video stream
Thread(target=self.update,args=()).start()
return self
def update(self): #Keep looping indefinitely until the thread is stopped
while True:
#If the camera is stopped, stop the thread
if self.stopped:
#Close camera resources
self.stream.release()
return
#Otherwise, grab the next frame from the stream
(self.grabbed, self.frame) = self.stream.read()

def read(self): #Return the most recent frame
return self.frame

def stop(self): #Indicate that the camera and thread should be stopped
self.stopped = True

#Define and parse input arguments
parser = argparse.ArgumentParser()
parser.add_argument(‘–modeldir’, help=’Folder the .tflite file is located in’, required=True)

parser.add_argument(‘–edgetpu’, help=’Use Coral Edge TPU Accelerator to speed up detection’, action=’store_true’)

args = parser.parse_args()

MODEL_NAME = args.modeldir
GRAPH_NAME = args.graph
LABELMAP_NAME = args.labels
min_conf_threshold = float(args.threshold)
resW, resH = args.resolution.split(‘x’)
imW, imH = int(resW), int(resH)
use_TPU = args.edgetpu

#Import TensorFlow libraries
#If tensorflow is not installed, import interpreter from tflite_runtime, else import from regular tensorflow
#If using Coral Edge TPU, import the load_delegate library
pkg = importlib.util.find_spec(‘tensorflow’)
if pkg is None:
from tflite_runtime.interpreter import Interpreter
if use_TPU:
from tflite_runtime.interpreter import load_delegate
else:
from tensorflow.lite.python.interpreter import Interpreter
if use_TPU:
from tensorflow.lite.python.interpreter import load_delegate

#If using Edge TPU, assign filename for Edge TPU model
if use_TPU: #If user has specified the name of the .tflite file, use that name, otherwise use default ‘edgetpu.tflite’
if (GRAPH_NAME == ‘detect.tflite’):
GRAPH_NAME = ‘edgetpu.tflite’

#Get path to current working directory
CWD_PATH = os.getcwd()

#Path to .tflite file, which contains the model that is used for object detection
PATH_TO_CKPT = os.path.join(CWD_PATH,MODEL_NAME,GRAPH_NAME)

#Path to label map file
PATH_TO_LABELS = os.path.join(CWD_PATH,MODEL_NAME,LABELMAP_NAME)

#Load the label map
with open(PATH_TO_LABELS, ‘r’) as f:
labels = [line.strip() for line in f.readlines()]

#Have to do a weird fix for label map if using the COCO “starter model” from
#https://www.tensorflow.org/lite/models/object_detection/overview
#First label is ‘???’, which has to be removed.
if labels[0] == ‘???’:
del(labels[0])

#Load the Tensorflow Lite model.
#If using Edge TPU, use special load_delegate argument
if use_TPU:
interpreter = Interpreter(model_path=PATH_TO_CKPT,
experimental_delegates=[load_delegate('libedgetpu.so.1.0')])
print(PATH_TO_CKPT)
else:
interpreter = Interpreter(model_path=PATH_TO_CKPT)

interpreter.allocate_tensors()

#Get model details
input_details = interpreter.get_input_details()
output_details = interpreter.get_output_details()
height = input_details[0]['shape'][1] width = input_details[0]['shape'][2]

floating_model = (input_details[0]['dtype'] == np.float32)

input_mean = 127.5
input_std = 127.5

#Initialize frame rate calculation
frame_rate_calc = 1
freq = cv2.getTickFrequency()

#Initialize video stream
videostream = VideoStream(resolution=(imW,imH),framerate=30).start()
time.sleep(1)

#for frame1 in camera.capture_continuous(rawCapture, format=”bgr”,use_video_port=True):
while True:
if GPIO.input(left_switch) == 1: #Start timer (for calculating frame rate)
t1 = cv2.getTickCount()

#Grab frame from video stream
frame1 = videostream.read()

#Acquire frame and resize to expected shape [1xHxWx3] frame = frame1.copy()
frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
frame_resized = cv2.resize(frame_rgb, (width, height))
input_data = np.expand_dims(frame_resized, axis=0)

#Normalize pixel values if using a floating model (i.e. if model is non-quantized)
if floating_model:
input_data = (np.float32(input_data) – input_mean) / input_std

#Perform the actual detection by running the model with the image as input
interpreter.set_tensor(input_details[0]['index'],input_data)
interpreter.invoke()

#Retrieve detection results
boxes = interpreter.get_tensor(output_details[0]['index'])[0] #Bounding box coordinates of detected objects
classes = interpreter.get_tensor(output_details[1]['index'])[0] #Class index of detected objects
scores = interpreter.get_tensor(output_details[2]['index'])[0] #Confidence of detected objects
#num = interpreter.get_tensor(output_details[3]['index'])[0] #Total number of detected objects (inaccurate and not needed)

pulse_start=0
pulse_end=0

GPIO.output(TRIG, False) #Keep the trigger pin off.
time.sleep(2)

GPIO.output(TRIG, True) #Exits 10us pulse.
time.sleep(0.00001) #At Python, this pulse will be almost 100 us.
GPIO.output(TRIG, False)

timeout = time.time()
while GPIO.input(ECHO) == 0:
pulse_start = time.time() #Set the start time when the echo pin is turned on.
if ((pulse_start – timeout)*1000000) >= MAX_DURATION_TIMEOUT:
fail = True
break

timeout = time.time()
while GPIO.input(ECHO) == 1:
pulse_end = time.time() #Set the reflector receiving time when the echo pin is turned off.
if ((pulse_end – pulse_start)*1000000) >= MAX_DURATION_TIMEOUT:
print_distance(0)
fail = True
break

pulse_duration = (pulse_end – pulse_start) * 1000000
pulse_duration=pulse_end-pulse_start #Calculate pulse length
distance = distanceInCm(pulse_duration)
distance=(pulse_duration*34000.0)/2 #The duration is divided in half.
distance = round(distance, 2)
MSG=print_distance(distance)

#Loop over all detections and draw detection box if confidence is above minimum threshold
for i in range(len(scores)):
if ((scores[i] > min_conf_threshold) and (scores[i] <= 1.0)):

#Get bounding box coordinates and draw box
#Interpreter can return coordinates that are outside of image dimensions, need to force them to be within image using max() and min()
ymin = int(max(1,(boxes[i][0] * imH)))
xmin = int(max(1,(boxes[i][1] * imW)))
ymax = int(min(imH,(boxes[i][2] * imH)))
xmax = int(min(imW,(boxes[i][3] * imW)))

cv2.rectangle(frame, (xmin, ymin), (xmax, ymax), (10, 255, 0), 2)
a=(xmax-xmin)/2
b = int(a)
if distance < 16:
frame = cv2.line(frame, (650, 720), (xmin+b, ymax), (0, 0, 255), 3)
elif distance >=16 and distance < 20:
frame = cv2.line(frame, (650, 720), (xmin+b, ymax), (0, 150, 255), 3)
else:
frame = cv2.line(frame, (650, 720), (xmin+b, ymax), (0, 255, 0), 3)

#Draw label
object_name = labels[int(classes[i])] #Look up object name from “labels” array using class index
label = ‘%s: %d%%’ % (object_name, int(scores[i]*100)) #Example: ‘person: 72%’
labelSize, baseLine = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.7, 2) #Get font size
label_ymin = max(ymin, labelSize[1] + 10) #Make sure not to draw label too close to top of window
cv2.rectangle(frame, (xmin, label_ymin-labelSize[1]-10), (xmin+labelSize[0], label_ymin+baseLine-10), (255, 255, 255), cv2.FILLED) #Draw white box to put label text in
cv2.putText(frame, label, (xmin, label_ymin-7), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 0), 2) #Draw label text

#Draw distance in corner of frame
cv2.putText(frame, MSG,(30,50),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,0),2,cv2.LINE_AA)

#All the results have been drawn on the frame, so it’s time to display it.
cv2.imshow(‘Object detector’, frame)

#Calculate framerate
t2 = cv2.getTickCount()
time1 = (t2-t1)/freq
frame_rate_calc= 1/time1

#Press ‘q’ to quit
if cv2.waitKey(1) == ord(‘q’):
break
else: #Exit program if switch off
break
#Clean up
GPIO.cleanup()
cv2.destroyAllWindows()
videostream.stop()

#include <Servo.h>
Servo servo;
// 각 핀 번호 설정
int value=0;
int echo1 = 8;
int trig1 = 9;
int echo2 = 10;
int trig2 = 11;
int red=4;
int green=3;
// 각 핀의 입출력 설정
void setup() {
Serial.begin(9600);
pinMode(trig1, OUTPUT);
pinMode(echo1, INPUT);
pinMode(trig2, OUTPUT);
pinMode(echo2, INPUT);
pinMode(red,OUTPUT);
pinMode(green,OUTPUT);
servo.attach(7);
}

void loop() {
// 1번 초음파센서에서 초음파 발사 후 초음파가 다시 돌아오는데 걸리는 시간 저장
digitalWrite(trig1, LOW);
digitalWrite(echo1, LOW);
delayMicroseconds(2);
digitalWrite(trig1, HIGH);
delayMicroseconds(10);
digitalWrite(trig1, LOW);
unsigned long duration1 = pulseIn(echo1, HIGH);
// 2번 초음파센서에서 초음파 발사 후 초음파가 다시 돌아오는데 걸리는 시간 저장
digitalWrite(trig2, LOW);
digitalWrite(echo2, LOW);
delayMicroseconds(2);
digitalWrite(trig2, HIGH);
delayMicroseconds(10);
digitalWrite(trig2, LOW);
unsigned long duration2 = pulseIn(echo2, HIGH);
// 초음파를 발산한 후 다시 돌아온 시간을 가지고 거리 측정
float distance1 = ((float)(340 * duration1) / 10000) / 2;
float distance2 = ((float)(340 * duration2) / 10000) / 2;

// 1번 초음파센서로 측정한 거리가 13cm미만일 경우 (쓰레기통에 근접하는 물체가 감지 될 경우)
if(distance1<13) // 쓰레기통의 문을 연다
{
for(;value>=0;value -=2)
{
servo.write(value);
delay(10);
}
delay(3000); // 문이 열린 후 3초의 지연시간을 준다
}else // 아닐 경우
{
for(;value<=100;value +=2) // 쓰레기통의 문이 닫힌다
{
servo.write(value);
delay(10);
}
}
// 2번 초음파센서로 측정한 거리가 9.5cm초과일 경우(쓰레기통 안의 내용물이 다 찼을 경우)
if(distance2<9.5)
{
// LED 빨간불 ON
digitalWrite(green,HIGH);
digitalWrite(red,LOW);
}
else // 아닐 경우
{
// LED 초록불 ON
digitalWrite(green,LOW);
digitalWrite(red,HIGH);
}
// 측정된 거리 값을 시리얼 모니터에 출력
Serial.print(“1 : “);
Serial.print(distance1);
Serial.print(“cm “);
Serial.print(“2 : “);
Serial.print(distance2);
Serial.println(“cm”);
delay(100);
}

from sense_hat import SenseHat
import time
import imageprocessing_03 as stair
import MotorController_03 as motor
import cv2
import os
import RPi.GPIO as GPIO

## MOTOR config setting ##

# 모터 상태
STOP = 0
FORWARD = 1
BACKWORD = 2

# 모터 채널
CH1 = 0
CH2 = 1

# PIN 입출력 설정
OUTPUT = 1
INPUT = 0

# PIN 설정
HIGH = 1
LOW = 0

# 실제 핀 정의
#PWM PIN
ENA = 26 #37 pin
ENB = 0 #27 pin

#GPIO PIN
IN1 = 19 #35 pin
IN2 = 13 #33 pin
IN3 = 6 #31 pin
IN4 = 5 #29 pin
SW1 = 17 #13 pin
SW2 = 27 #15 pin
SW3 = 16
SW4 = 20

# 핀 설정 함수
def setPinConfig(EN, INA, INB):
GPIO.setup(EN, GPIO.OUT)
GPIO.setup(INA, GPIO.OUT)
GPIO.setup(INB, GPIO.OUT)
# 100khz 로 PWM 동작 시킴
pwm = GPIO.PWM(EN, 100)
# 우선 PWM 멈춤.
pwm.start(0)
return pwm

# 스위치 핀 함수
def setPinSwitch(SW1, SW2, SW3, SW4) :
GPIO.setwarnings(False)
GPIO.setup(SW1,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(SW2,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(SW3,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(SW4,GPIO.IN,GPIO.PUD_UP)

# 모터 제어 함수
def setMotorContorl(pwm, INA, INB, speed, stat):

#모터 속도 제어 PWM
pwm.ChangeDutyCycle(speed)

if stat == FORWARD:
GPIO.output(INA, HIGH)
GPIO.output(INB, LOW)

#뒤로
elif stat == BACKWORD:
GPIO.output(INA, LOW)
GPIO.output(INB, HIGH)

#정지
elif stat == STOP:
GPIO.output(INA, LOW)
GPIO.output(INB, LOW)
# 모터 제어함수 간단하게 사용하기 위해 한번더 래핑(감쌈)
def setMotor(ch, speed, stat):
if ch == CH1:
#pwmA는 핀 설정 후 pwm 핸들을 리턴 받은 값이다.
setMotorContorl(pwmA, IN1, IN2, speed, stat)
else:
#pwmB는 핀 설정 후 pwm 핸들을 리턴 받은 값이다.
setMotorContorl(pwmB, IN3, IN4, speed, stat)

def RC():
if GPIO.input(SW1) == 0 : #forward

SPEED = 12 #LEFT MOTOR
SPEED1 = 12
setMotor(CH1, SPEED, FORWARD)
setMotor(CH2, SPEED1, BACKWORD)
print(“forward”)

elif GPIO.input(SW2) == 0 : #back

SPEED = 12 #LEFT MOTOR
SPEED1 = 12
setMotor(CH1, SPEED, BACKWORD)
setMotor(CH2, SPEED1, FORWARD)
print(“back”)

elif GPIO.input(SW3) == 0 : #LEFT
SPEED = 1 #LEFT MOTOR
SPEED1 = 15
setMotor(CH1, SPEED, FORWARD)
setMotor(CH2, SPEED1, BACKWORD)
print(“here”)
print(“left”)

elif GPIO.input(SW4) == 0 : #right
SPEED = 15 #LEFT MOTOR
SPEED1 = 1
setMotor(CH1, SPEED, FORWARD)
setMotor(CH2, SPEED1, BACKWORD)
print(“right”)

else :
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)
print(“stop”)

###########################

def stair_detection():
while True:
print(‘in rc’)
RC() #..
i = 0
while i < 50 :
sense.get_orientation_degrees()['pitch'] i = i + 1

if stair_threshold <= sense.get_orientation_degrees()['pitch']:
print(“up stair”)
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)
os.system(“sudo service uv4l_raspicam restart”)
os.system(“sudo dd if=/dev/video0 of=image.png bs=11M count=1″)

return stair.stair_detection()

elif sense.get_orientation_degrees()['pitch'] <= -stair_threshold :
print(“down stair”)
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)
os.system(“sudo service uv4l_raspicam restart”)
# down stair sound

return ‘down’
if __name__ == “__main__” :
os.system(“sudo service uv4l_raspicam restart”)
sense = SenseHat()
stair_threshold = 15 # …
roll_threshold = 10 #

# GPIO 모드 설정
GPIO.setmode(GPIO.BCM)

#모터 핀 설정
#핀 설정후 PWM 핸들 얻어옴
pwmA = setPinConfig(ENA, IN1, IN2)
pwmB = setPinConfig(ENB, IN3, IN4)
setPinSwitch(SW1, SW2, SW3, SW4)

SPEED = 12
SPEED1 = 12

while True:
print(” ##### program start ##### “)

detection = stair_detection()

if detection == True: #up stair

print(‘up stair’)
#up stair sound mp3
while sense.get_orientation_degrees()['pitch'] > 3 :

if sense.get_orientation_degrees()['roll'] > roll_threshold:
setMotor(CH1, 10, FORWARD)
setMotor(CH2, 0, STOP)
print(‘left move’)

elif sense.get_orientation_degrees()['roll'] < -roll_threshold:
setMotor(CH1, 0, STOP)
setMotor(CH2, 10, BACKWORD)
print(‘rigth move’)
elif -roll_threshold < sense.get_orientation_degrees()['roll'] < roll_threshold:
setMotor(CH1, 13, FORWARD)
setMotor(CH2, 13, BACKWORD)
print(‘straight’)

elif detection == ‘down’ :

print(‘down stair’)
while sense.get_orientation_degrees()['pitch'] < -3 :

if sense.get_orientation_degrees()['roll'] > roll_threshold:
setMotor(CH1, 0, STOP)
setMotor(CH2, 13, BACKWORD)
print(‘right move’)

elif sense.get_orientation_degrees()['roll'] < -roll_threshold:
setMotor(CH1, 13, FORWARD)
setMotor(CH2, 0, STOP)
print(‘left move’)
elif -roll_threshold < sense.get_orientation_degrees()['roll'] < roll_threshold:
setMotor(CH1, 10, FORWARD)
setMotor(CH2, 10, BACKWORD)
print(‘straight’)

elif detection == False : #wall or 1 stair

print(“wall”)
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)

GPIO.cleanup()