简单的写了程序,跑起来还是不太理想。
# -*- coding:utf-8 -*-
import RPi.GPIO as gpio
import
time
import sys
#L298N接口
def init_298():
gpio.setmode(gpio.BOARD)
gpio.setup(32,gpio.OUT)
gpio.setup(33,gpio.OUT)
gpio.setup(35,gpio.OUT)
gpio.setup(38,gpio.OUT)
#HC-SR04接口
def init_SR04():
gpio.setmode(gpio.BOARD)
gpio.setup(11,gpio.IN)
gpio.setup(12,gpio.OUT,initial=gpio.LOW)
#光感接口定义
def init_MH():
gpio.setmode(gpio.BOARD)
gpio.setup(13,gpio.IN)
gpio.setup(15,gpio.IN)
gpio.setup(16,gpio.IN)
gpio.setup(18,gpio.IN)
#前进
def forward(time_sleep):
init_298()
gpio.output(32,gpio.LOW)
gpio.output(33,gpio.HIGH)
gpio.output(35,gpio.LOW)
gpio.output(38,gpio.HIGH)
time.sleep(time_sleep)
gpio.cleanup()
#后退
def rever(time_sleep):
init_298()
gpio.output(32,gpio.HIGH)
gpio.output(33,gpio.LOW)
gpio.output(35,gpio.HIGH)
gpio.output(38,gpio.LOW)
time.sleep(time_sleep)
gpio.cleanup()
#左转
def left(time_sleep):
init_298()
gpio.output(32,True)
gpio.output(33,True)
gpio.output(35,gpio.LOW)
gpio.output(38,gpio.HIGH)
time.sleep(time_sleep)
gpio.cleanup()
#右转
def right(time_sleep):
init_298()
gpio.output(32,gpio.LOW)
gpio.output(33,gpio.HIGH)
gpio.output(35,True)
gpio.output(38,True)
time.sleep(time_sleep)
gpio.cleanup()
def left_r(time_sleep):
init_298()
gpio.output(32,gpio.HIGH)
gpio.output(33,gpio.LOW)
gpio.output(35,gpio.LOW)
gpio.output(38,gpio.HIGH)
time.sleep(time_sleep)
gpio.cleanup()
def right_r(time_sleep):
init_298()
gpio.output(32,gpio.LOW)
gpio.output(33,gpio.HIGH)
gpio.output(35,gpio.HIGH)
gpio.output(38,gpio.LOW)
time.sleep(time_sleep)
gpio.cleanup()
#
def CHSR04():
init_SR04()
gpio.output(12,gpio.HIGH)
time.sleep(0.000015)
gpio.output(12,gpio.LOW)
while gpio.input(11) == 0:
# pass
t1 = time.time()
while gpio.input(11) == 1:
# pass
t2 = time.time()
return (t2-t1)/0.000058
gpio.cleanup()
#
def check_front():
while True:
CHSR04()
dist = CHSR04()
init_MH()
if gpio.input(16) == 1 or dist < 30:
if gpio.input(18) == 0 and gpio.input(13) == 1 and gpio.input(15) == 1:
rever(0.3)
right(0.3)
# time.sleep(1)
init_MH()
elif gpio.input(13) == 0:
left(0.5)
# time.sleep(0.5)
else:
right(0.5)
# time.sleep(0.5)
else:
init_MH()
if gpio.input(13) == 1 and gpio.input(15) == 0:
right_r(0.5)
# time.sleep(0.5)
init_MH()
elif gpio.input(13) == 0 and gpio.input(15) == 1:
left_r(0.5)
# time.sleep(0.5)
else:
forward(0.3)
#
check_front()
等有时间了再改改……