不带舵机驱动代码:
有障碍物的话------就右转或者左转避开
#!/usr/bin/env python
import RPi.GPIO as GPIO
import time
import sys
#BCM编码
PWMA = 18
AIN1 = 22
AIN2 = 27
PWMB = 23
BIN1 = 25
BIN2 = 24
BtnPin = 19#这三个引脚不晓得干啥的
Gpin = 5
Rpin = 6
#超声波工作引脚
TRIG = 20 ##设置Trig和ECHO俩个引脚
ECHO = 21
def t_up(speed,t_time):
L_Motor.ChangeDutyCycle(speed)
GPIO.output(AIN2,False)#AIN2
GPIO.output(AIN1,True) #AIN1
R_Motor.ChangeDutyCycle(speed)
GPIO.output(BIN2,False)#BIN2
GPIO.output(BIN1,True) #BIN1
time.sleep(t_time)
def t_stop(t_time):
L_Motor.ChangeDutyCycle(0)
GPIO.output(AIN2,False)#AIN2
GPIO.output(AIN1,False) #AIN1
R_Motor.ChangeDutyCycle(0)
GPIO.output(BIN2,False)#BIN2
GPIO.output(BIN1,False) #BIN1
time.sleep(t_time)
def t_down(speed,t_time):
L_Motor.ChangeDutyCycle(speed)
GPIO.output(AIN2,True)#AIN2
GPIO.output(AIN1,False) #AIN1
R_Motor.ChangeDutyCycle(speed)
GPIO.output(BIN2,True)#BIN2
GPIO.output(BIN1,False) #BIN1
time.sleep(t_time)
def t_left(speed,t_time):
L_Motor.ChangeDutyCycle(speed)
GPIO.output(AIN2,True)#AIN2
GPIO.output(AIN1,False) #AIN1
R_Motor.ChangeDutyCycle(speed)
GPIO.output(BIN2,False)#BIN2
GPIO.output(BIN1,True) #BIN1
time.sleep(t_time)
def t_right(speed,t_time):
L_Motor.ChangeDutyCycle(speed)
GPIO.output(AIN2,False)#AIN2
GPIO.output(AIN1,True) #AIN1
R_Motor.ChangeDutyCycle(speed)
GPIO.output(BIN2,True)#BIN2
GPIO.output(BIN1,False) #BIN1
time.sleep(t_time)
def keysacn():##按键开关引脚
val = GPIO.input(BtnPin)#输入引脚
while GPIO.input(BtnPin) == False:#低电平
val = GPIO.input(BtnPin)
while GPIO.input(BtnPin) == True:#高电平
time.sleep(0.01)
val = GPIO.input(BtnPin)
if val == True:
GPIO.output(Rpin,1)
while GPIO.input(BtnPin) == False:
GPIO.output(Rpin,0)
else:
GPIO.output(Rpin,0)
def setup():
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.setup(Gpin, GPIO.OUT) # Set Green Led Pin mode to output
GPIO.setup(Rpin, GPIO.OUT) # Set Red Led Pin mode to output
GPIO.setup(BtnPin, GPIO.IN, pull_up_down=GPIO.PUD_UP) # Set BtnPin's mode is input, and pull up to high level(3.3V)
GPIO.setup(AIN2,GPIO.OUT)
GPIO.setup(AIN1,GPIO.OUT)
GPIO.setup(PWMA,GPIO.OUT)
GPIO.setup(BIN1,GPIO.OUT)
GPIO.setup(BIN2,GPIO.OUT)
GPIO.setup(PWMB,GPIO.OUT)
def distance():#同超声波测量距离原理
GPIO.output(TRIG, 0)
time.sleep(0.000002)
GPIO.output(TRIG, 1)
time.sleep(0.00001)
GPIO.output(TRIG, 0)#准备开始阶段
while GPIO.input(ECHO) == 0:
a = 0
time1 = time.time()
while GPIO.input(ECHO) == 1:
a = 1
time2 = time.time()
during = time2 - time1
return during * 340 / 2 * 100
def loop():#判断要不要规避
while True:
dis = distance()
if (dis < 40) == True: #前方有障碍物,进行规避右转
while (dis < 40) == True:#距离《40
t_down(30,0.5) #占空比改变速度
t_right(30,0.1)
dis = distance()
else:
t_up(50,0)
print(dis, 'cm')
print('')
def destroy():
GPIO.cleanup()
if __name__ == "__main__":
setup() #设置对应输入输出引脚
L_Motor= GPIO.PWM(PWMA,100) #左边使能A#
L_Motor.start(0)
R_Motor = GPIO.PWM(PWMB,100)
R_Motor.start(0)
t_stop(.1)
# keysacn()#不需要这一步也行!!!
try:
loop()
except KeyboardInterrupt:
destroy()
二、带舵机模块:
舵机运转:(如果舵机不是由树莓派供电的话,需要将该电源与树莓派共地,也就是说电源的负极必须与树莓派任意一个GND连接,否则会出现舵机控制失常等现象。)如果
- DutyCycle = angle/18 + 2-------通过改变占空比控制舵机转换角度
- 0度对应——2%占空比
- 90度对应——7%占空比
- 180度——12%占空比
工作原理:
控制电路板接受来自信号线的控制信号,控制电机转动,电机带动一系列齿轮组,减速后传动至输出舵盘。舵机的输出轴和位置反馈电位计是相连的,舵盘转动的同时,带动位置反馈电位计,电位计将输出一个电压信号到控制电路板,进行反馈,然后控制电路板根据所在位置决定电机的转动方向和速度,从而达到目标停止。
带舵机小车代码 :
#!/usr/bin/env python
from Adafruit_PWM_Servo_Driver import PWM
import RPi.GPIO as GPIO
import time
import sys
PWMA = 18
AIN1 = 22
AIN2 = 27
PWMB = 23
BIN1 = 25
BIN2 = 24
BtnPin = 19#舵机接口
Gpin = 5
Rpin = 6
TRIG = 20
ECHO = 21
# Initialise the PWM device using the default address
# bmp = PWM(0x40, debug=True)
pwm = PWM(0x40,debug = False)
servoMin = 150 # Min pulse length out of 4096
servoMax = 600 # Max pulse length out of 4096
def setServoPulse(channel, pulse):
pulseLength = 1000000.0 # 1,000,000 us per second
pulseLength /= 50.0 # 60 Hz
print("%d us per period" % pulseLength)
pulseLength /= 4096.0 # 12 bits of resolution
print("%d us per bit" % pulseLength)
pulse *= 1000.0
pulse /= (pulseLength*1.0)
# pwmV=int(pluse)
print("pluse: %f " % (pulse))
pwm.setPWM(channel, 0, int(pulse))#调用其他函数库
#Angle to PWM
def write(servonum,x): #改变占空比-----》改变舵机角度!!!!!!!!!!!!!!!!!!!!
y=x/90.0+0.5
y=max(y,0.5)
y=min(y,2.5)
setServoPulse(servonum,y)
def t_up(speed,t_time):
L_Motor.ChangeDutyCycle(speed)
GPIO.output(AIN2,False)#AIN2
GPIO.output(AIN1,True) #AIN1
R_Motor.ChangeDutyCycle(speed)
GPIO.output(BIN2,False)#BIN2
GPIO.output(BIN1,True) #BIN1
time.sleep(t_time)
def t_stop(t_time):
L_Motor.ChangeDutyCycle(0)
GPIO.output(AIN2,False)#AIN2
GPIO.output(AIN1,False) #AIN1
R_Motor.ChangeDutyCycle(0)
GPIO.output(BIN2,False)#BIN2
GPIO.output(BIN1,False) #BIN1
time.sleep(t_time)
def t_down(speed,t_time):
L_Motor.ChangeDutyCycle(speed)
GPIO.output(AIN2,True)#AIN2
GPIO.output(AIN1,False) #AIN1
R_Motor.ChangeDutyCycle(speed)
GPIO.output(BIN2,True)#BIN2
GPIO.output(BIN1,False) #BIN1
time.sleep(t_time)
def t_left(speed,t_time):
L_Motor.ChangeDutyCycle(speed)
GPIO.output(AIN2,True)#AIN2
GPIO.output(AIN1,False) #AIN1
R_Motor.ChangeDutyCycle(speed)
GPIO.output(BIN2,False)#BIN2
GPIO.output(BIN1,True) #BIN1
time.sleep(t_time)
def t_right(speed,t_time):
L_Motor.ChangeDutyCycle(speed)
GPIO.output(AIN2,False)#AIN2
GPIO.output(AIN1,True) #AIN1
R_Motor.ChangeDutyCycle(speed)
GPIO.output(BIN2,True)#BIN2
GPIO.output(BIN1,False) #BIN1
time.sleep(t_time)
def keysacn():
val = GPIO.input(BtnPin)
while GPIO.input(BtnPin) == False:
val = GPIO.input(BtnPin)
while GPIO.input(BtnPin) == True:
time.sleep(0.01)
val = GPIO.input(BtnPin)
if val == True:
GPIO.output(Rpin,1)
while GPIO.input(BtnPin) == False:
GPIO.output(Rpin,0)
else:
GPIO.output(Rpin,0)
def setup():
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.setup(Gpin, GPIO.OUT) # Set Green Led Pin mode to output
GPIO.setup(Rpin, GPIO.OUT) # Set Red Led Pin mode to output
GPIO.setup(BtnPin, GPIO.IN, pull_up_down=GPIO.PUD_UP) # Set BtnPin's mode is input, and pull up to high level(3.3V)
GPIO.setup(AIN2,GPIO.OUT)
GPIO.setup(AIN1,GPIO.OUT)
GPIO.setup(PWMA,GPIO.OUT)
GPIO.setup(BIN1,GPIO.OUT)
GPIO.setup(BIN2,GPIO.OUT)
GPIO.setup(PWMB,GPIO.OUT)
pwm.setPWMFreq(50) # Set frequency to 60 Hz
def distance(): #测量距离函数
GPIO.output(TRIG, 0)
time.sleep(0.000002)
GPIO.output(TRIG, 1)
time.sleep(0.00001)
GPIO.output(TRIG, 0)
while GPIO.input(ECHO) == 0:
a = 0
time1 = time.time()
while GPIO.input(ECHO) == 1:
a = 1
time2 = time.time()
during = time2 - time1
return during * 340 / 2 * 100
def front_detection():#测量前后间距
write(0,90)
time.sleep(0.5)
dis_f = distance()
return dis_f
def left_detection():#
write(0, 175)
time.sleep(0.5)
dis_l = distance()
return dis_l
def right_detection():
write(0,5)
time.sleep(0.5)
dis_r = distance()
return dis_r
def loop():
while True:
dis1 = front_detection()
if (dis1 < 40) == True:
t_stop(0.2)
t_down(50,0.5)
t_stop(0.2)
dis2 = left_detection()
dis3 = right_detection()
if (dis2 < 40) == True and (dis3 < 40) == True:
t_left(50,1)
elif (dis2 > dis3) == True:
t_left(50,0.3)
t_stop(0.1)
else:
t_right(50,0.3)
t_stop(0.1)
else:
t_up(60,0)
# print dis1, 'cm'
# print ''
def destroy():
GPIO.cleanup()
if __name__ == "__main__":
setup()
L_Motor= GPIO.PWM(PWMA,100)
L_Motor.start(0)
R_Motor = GPIO.PWM(PWMB,100)
R_Motor.start(0)
keysacn()
try:
loop()
except KeyboardInterrupt:
destroy()