2019-09-24 21:09:34 -04:00
|
|
|
import time
|
|
|
|
|
from Motor import *
|
|
|
|
|
import RPi.GPIO as GPIO
|
|
|
|
|
from servo import *
|
|
|
|
|
from PCA9685 import PCA9685
|
|
|
|
|
class Ultrasonic:
|
2020-07-07 20:24:44 -04:00
|
|
|
def __init__(self):
|
|
|
|
|
GPIO.setwarnings(False)
|
|
|
|
|
self.trigger_pin = 27
|
|
|
|
|
self.echo_pin = 22
|
|
|
|
|
GPIO.setmode(GPIO.BCM)
|
|
|
|
|
GPIO.setup(self.trigger_pin,GPIO.OUT)
|
|
|
|
|
GPIO.setup(self.echo_pin,GPIO.IN)
|
2019-09-24 21:09:34 -04:00
|
|
|
def send_trigger_pulse(self):
|
2020-07-07 20:24:44 -04:00
|
|
|
GPIO.output(self.trigger_pin,True)
|
2019-09-24 21:09:34 -04:00
|
|
|
time.sleep(0.00015)
|
2020-07-07 20:24:44 -04:00
|
|
|
GPIO.output(self.trigger_pin,False)
|
2019-09-24 21:09:34 -04:00
|
|
|
|
|
|
|
|
def wait_for_echo(self,value,timeout):
|
|
|
|
|
count = timeout
|
2020-07-07 20:24:44 -04:00
|
|
|
while GPIO.input(self.echo_pin) != value and count>0:
|
2019-09-24 21:09:34 -04:00
|
|
|
count = count-1
|
|
|
|
|
|
|
|
|
|
def get_distance(self):
|
|
|
|
|
distance_cm=[0,0,0,0,0]
|
|
|
|
|
for i in range(3):
|
|
|
|
|
self.send_trigger_pulse()
|
|
|
|
|
self.wait_for_echo(True,10000)
|
|
|
|
|
start = time.time()
|
|
|
|
|
self.wait_for_echo(False,10000)
|
|
|
|
|
finish = time.time()
|
|
|
|
|
pulse_len = finish-start
|
|
|
|
|
distance_cm[i] = pulse_len/0.000058
|
|
|
|
|
distance_cm=sorted(distance_cm)
|
|
|
|
|
return int(distance_cm[2])
|
|
|
|
|
def run_motor(self,L,M,R):
|
|
|
|
|
if (L < 30 and M < 30 and R <30) or M < 30 :
|
|
|
|
|
self.PWM.setMotorModel(-1450,-1450,-1450,-1450)
|
|
|
|
|
time.sleep(0.1)
|
|
|
|
|
if L < R:
|
|
|
|
|
self.PWM.setMotorModel(1450,1450,-1450,-1450)
|
|
|
|
|
else :
|
|
|
|
|
self.PWM.setMotorModel(-1450,-1450,1450,1450)
|
|
|
|
|
elif L < 30 and M < 30:
|
|
|
|
|
PWM.setMotorModel(1500,1500,-1500,-1500)
|
|
|
|
|
elif R < 30 and M < 30:
|
|
|
|
|
PWM.setMotorModel(-1500,-1500,1500,1500)
|
|
|
|
|
elif L < 20 :
|
|
|
|
|
PWM.setMotorModel(2000,2000,-500,-500)
|
|
|
|
|
if L < 10 :
|
|
|
|
|
PWM.setMotorModel(1500,1500,-1000,-1000)
|
|
|
|
|
elif R < 20 :
|
|
|
|
|
PWM.setMotorModel(-500,-500,2000,2000)
|
|
|
|
|
if R < 10 :
|
|
|
|
|
PWM.setMotorModel(-1500,-1500,1500,1500)
|
|
|
|
|
else :
|
|
|
|
|
self.PWM.setMotorModel(600,600,600,600)
|
|
|
|
|
|
|
|
|
|
def run(self):
|
|
|
|
|
self.PWM=Motor()
|
|
|
|
|
self.pwm_S=Servo()
|
|
|
|
|
for i in range(30,151,60):
|
|
|
|
|
self.pwm_S.setServoPwm('0',i)
|
|
|
|
|
time.sleep(0.2)
|
|
|
|
|
if i==30:
|
|
|
|
|
L = self.get_distance()
|
|
|
|
|
elif i==90:
|
|
|
|
|
M = self.get_distance()
|
|
|
|
|
else:
|
|
|
|
|
R = self.get_distance()
|
|
|
|
|
while True:
|
|
|
|
|
for i in range(90,30,-60):
|
|
|
|
|
self.pwm_S.setServoPwm('0',i)
|
|
|
|
|
time.sleep(0.2)
|
|
|
|
|
if i==30:
|
|
|
|
|
L = self.get_distance()
|
|
|
|
|
elif i==90:
|
|
|
|
|
M = self.get_distance()
|
|
|
|
|
else:
|
|
|
|
|
R = self.get_distance()
|
|
|
|
|
self.run_motor(L,M,R)
|
|
|
|
|
for i in range(30,151,60):
|
|
|
|
|
self.pwm_S.setServoPwm('0',i)
|
|
|
|
|
time.sleep(0.2)
|
|
|
|
|
if i==30:
|
|
|
|
|
L = self.get_distance()
|
|
|
|
|
elif i==90:
|
|
|
|
|
M = self.get_distance()
|
|
|
|
|
else:
|
|
|
|
|
R = self.get_distance()
|
|
|
|
|
self.run_motor(L,M,R)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ultrasonic=Ultrasonic()
|
|
|
|
|
# Main program logic follows:
|
|
|
|
|
if __name__ == '__main__':
|
|
|
|
|
print ('Program is starting ... ')
|
|
|
|
|
try:
|
|
|
|
|
ultrasonic.run()
|
|
|
|
|
except KeyboardInterrupt: # When 'Ctrl+C' is pressed, the child program destroy() will be executed.
|
|
|
|
|
PWM.setMotorModel(0,0,0,0)
|
|
|
|
|
ultrasonic.pwm_S.setServoPwm('0',90)
|
|
|
|
|
|
