Raspberry Pi car obstacle avoidance - ultrasonic control (2.1)

Language 2023-07-12 00:27:47 views: null

 Driver code without servo:

If there is an obstacle --- turn right or left to avoid it

#!/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()

 2. Module with steering gear:

Servo running: ( If the servo is not powered by the Raspberry Pi, the power supply needs to be shared with the Raspberry Pi, that is to say, the negative pole of the power supply must be connected to any GND of the Raspberry Pi, otherwise the servo control will be abnormal etc. ) If

  1. DutyCycle = angle/18 + 2-------Control the conversion angle of the servo by changing the duty cycle
  2. 0 degrees corresponds to - 2% duty cycle
  3. 90 degrees corresponding - 7% duty cycle
  4. 180 degrees - 12% duty cycle

working principle:

The control circuit board receives the control signal from the signal line to control the rotation of the motor, and the motor drives a series of gear sets, which are decelerated and then transmitted to the output steering wheel. The output shaft of the steering gear is connected to the position feedback potentiometer. When the steering wheel rotates, the position feedback potentiometer is driven. The potentiometer will output a voltage signal to the control circuit board for feedback, and then the control circuit board determines the motor according to its position. The direction and speed of rotation, so as to achieve the target stop.

The code of the car with steering gear:

#!/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()

Car running video:

Guess you like

Origin blog.csdn.net/JLwwfs/article/details/126116854
Recommended
Ranking
Déploiement de consul autonome
Flutter uses the running "flutter pub get" command to freeze solution-window practice solution
Helmet wearing detection - from data processing, training data to model deployment (with data set, training code, C++ model deployment code that can use GPU)
idea 2020 common configuration
Figure bed test
To_Heart—游记——CSP-2021
【Go Actual | E-commerce Platform】(9) Display Products
Try to play Sekiro with DQN (pytorch framework)
rubygems switching mirror to solve the problem of pod to download the update download
Wireless communication technology basics
Daily
More
2024-07-02(0)
2024-07-01(0)
2024-06-30(0)
2024-06-29(0)
2024-06-28(0)
2024-06-27(0)
2024-06-26(0)
2024-06-25(0)
2024-06-24(0)
2024-06-23(0)

Code World

© 2018 codetd.com