def run():
v_command
# obtain audio from the microphone
r = sr.Recognizer()
with sr.Microphone(device_index =2,sample_rate=48000) as source:
r.record(source,duration=2)
#r.adjust_for_ambient_noise(source)
headlights.turn(headlights.BOTH, headlights.YELLOW)
print("Command?")
audio = r.listen(source)
headlights.turn(headlights.BOTH, headlights.BLUE)
try:
v_command = r.recognize_sphinx(audio,
keyword_entries=[('forward',1.0),('backward',1.0),
('left',1.0),('right',1.0),('stop',1.0)]) #You can add your own command here
print(v_command)
headlights.turn(headlights.BOTH, headlights.CYAN)
except sr.UnknownValueError:
print("say again")
headlights.turn(headlights.BOTH, headlights.RED)
except sr.RequestError as e:
headlights.turn(headlights.BOTH, headlights.RED)
pass
#print('pre')
if 'forward' in v_command:
motor.move(motor.FORWARD, right_spd*spd_ad_2)
time.sleep(2)
motor.motorStop()
elif 'backward' in v_command:
motor.move(motor.BACKWARD, right_spd)
time.sleep(2)
motor.motorStop()
elif 'left' in v_command:
servos.steerLeft()
motor.move(motor.FORWARD, right_spd*spd_ad_2)
time.sleep(2)
motor.motorStop()
elif "right" in v_command:
servos.steerRight()
motor.move(motor.FORWARD, right_spd*spd_ad_2)
time.sleep(2)
motor.motorStop()
elif 'stop' in v_command:
motor.motorStop()
else:
pass
def cleanup():
print("server module: executing cleanup()")
colorWipe(ledStrip, rpi_ws281x.Color(0, 0, 0))
headlights.turn(headlights.BOTH, headlights.OFF)
GPIO.cleanup()
try:
camera.close()
except:
print(
"cleanup: caught known false picamera exception that it is ignoring"
)
pass # ignore known random exception in picamera library
print("cleanup: completed")
def connect():
global ap_status, blinkThreadStatus
#Start server, and wait for client
HOST = ''
PORT = 10223 #Define port serial
ADDR = (HOST, PORT)
tcpSerSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
tcpSerSock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
tcpSerSock.bind(ADDR)
tcpSerSock.listen(5)
# setup server to accept client connection
while True:
try:
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.connect(("1.1.1.1", 80))
ipaddr_check = s.getsockname()[0]
s.close()
print("connect: robot's IP address = %s" % str(ipaddr_check))
wifi_status = 1
except:
if ap_status == 0:
ap_threading = threading.Thread(
target=ap_thread) #Define a thread for wifi hot spot
ap_threading.setDaemon(
True
) #True means it is a front thread, closes when the mainloop() closes
ap_threading.start() #Thread starts
headlights.turn(headlights.BOTH, headlights.YELLOW)
time.sleep(5)
wifi_status = 0
if wifi_status == 1:
# blink headlights to indicate SW is running and waiting for client
ledBlinkThread = threading.Thread(target=blinkHeadlightsThread)
ledBlinkThread.setDaemon(True)
ledBlinkThread.start() #Thread starts
print('waiting for connection...')
Socket, addr = tcpSerSock.accept() #Determine whether to connect
while blinkThreadStatus != BLINK_THREAD_STOPPED:
blinkThreadStatus = BLINK_THREAD_STOP
time.sleep(.1)
headlights.turn(headlights.BOTH, headlights.GREEN)
print('...connected from :', addr)
return Socket, addr
else:
headlights.turn(headlights.BOTH, headlights.BLUE)
print('waiting for connection...')
Socket, addr = tcpSerSock.accept() #Determine whether to connect
headlights.turn(headlights.BOTH, headlights.GREEN)
print('...connected from :', addr)
ap_status = 1
return Socket, addr
def blinkHeadlightsThread():
global blinkThreadStatus
blinkThreadStatus = BLINK_THREAD_RUNNING
headlights.turn(headlights.BOTH, headlights.OFF)
while blinkThreadStatus != BLINK_THREAD_STOP:
headlights.turn(headlights.LEFT, headlights.RED)
for i in range(10):
time.sleep(.1)
if blinkThreadStatus == BLINK_THREAD_STOP:
blinkThreadStatus = BLINK_THREAD_STOPPED
return
headlights.turn(headlights.LEFT, headlights.OFF)
headlights.turn(headlights.RIGHT, headlights.RED)
for i in range(10):
time.sleep(.1)
if blinkThreadStatus == BLINK_THREAD_STOP:
blinkThreadStatus = BLINK_THREAD_STOPPED
return
headlights.turn(headlights.RIGHT, headlights.OFF)
blinkThreadStatus = BLINK_THREAD_STOPPED
def loop(distance_stay, distance_range): #Tracking with Ultrasonic
servos.lookAhead()
servos.steerMiddle()
dis = checkDistance()
if dis < distance_range: #Check if the target is in diatance range
if dis > (distance_stay+0.1) : #If the target is in distance range and out of distance stay, then move forward to track
moving_time = (dis-distance_stay)/0.38
if moving_time > 1:
moving_time = 1
headlights.turn(headlights.BOTH, headlights.CYAN)
motor.move(motor.FORWARD, right_spd*spd_ad_u)
time.sleep(moving_time)
motor.motorStop()
elif dis < (distance_stay-0.1) : #Check if the target is too close, if so, the car move back to keep distance at distance_stay
moving_time = (distance_stay-dis)/0.38
headlights.turn(headlights.BOTH, headlights.PINK)
motor.move(motor.BACKWARD, right_spd*spd_ad_u)
time.sleep(moving_time)
motor.motorStop()
else: #If the target is at distance, then the car stay still
motor.motorStop()
headlights.turn(headlights.BOTH, headlights.YELLOW)
else:
motor.motorStop()
def run():
status_right = GPIO.input(line_pin_right)
status_middle = GPIO.input(line_pin_middle)
status_left = GPIO.input(line_pin_left)
if status_left == 1:
servos.steerFullLeft()
headlights.turn(headlights.BOTH, headlights.OFF)
headlights.turn(headlights.LEFT, headlights.RED)
motor.move(motor.FORWARD, right_spd * spd_ad_2)
elif status_middle == 1:
servos.steerMiddle()
headlights.turn(headlights.BOTH, headlights.YELLOW)
motor.move(motor.FORWARD, right_spd * spd_ad_1)
elif status_right == 1:
servos.steerFullRight()
headlights.turn(headlights.BOTH, headlights.OFF)
headlights.turn(headlights.RIGHT, headlights.RED)
motor.move(motor.FORWARDBACKWARD, right_spd * spd_ad_2)
else:
servos.steerMiddle()
headlights.turn(headlights.BOTH, headlights.CYAN)
motor.move(motor.BACKWARD, right_spd)
def mainLoop(socket):
global led_status, auto_status, opencv_mode, findline_mode, speech_mode, \
auto_mode, command, ap_status, turn_status, blinkThreadStatus
BUFSIZ = 1024 #Define buffer size
while True:
command = socket.recv(BUFSIZ).decode()
print("mainLoop: received %s" % command)
if not command:
continue
# strip end-of-cmd off
if END_OF_CMD in command:
command = command[:-len(END_OF_CMD)]
# TODO: add code to handle receiving multiple commands delimited by end-of-cmd
if SOUND in command:
sounds.playSound(command[len(SOUND):])
elif SPEAK in command:
speak.say(command[len(SPEAK):])
elif VOLUME in command:
speak.changeVolume(command[len(VOLUME):])
elif RATE in command:
speak.changeRate(command[len(RATE):])
elif SHAKE in command:
servos.shakeHead()
elif NOD in command:
servos.nodHead()
elif 'exit' in command:
os.system("sudo shutdown -h now\n")
elif 'quit' in command:
print('shutting down')
return
elif 'spdset' in command:
global spd_adj
spd_adj = float((str(command))[7:]) #Speed Adjustment
print("speed adjustment set to %s" % str(spd_adj))
elif 'scan' in command:
dis_can = servos.scan() # Start Scanning
str_list_1 = dis_can # Divide the list to make it samller to send
str_index = ' ' # Separate the values by space
str_send_1 = str_index.join(str_list_1) + ' '
socket.sendall((str(str_send_1)).encode()) # Send Data
socket.send(
'finished'.encode()
) # Sending 'finished' tell the client to stop receiving the list of dis_can
elif 'EC1set' in command: # pitch Adjustment
newValue = int((str(command))[7:])
config.exportConfig('pitch_middle', newValue)
servos.HEAD_PITCH_MIDDLE = newValue
servos.headPitch(newValue)
elif 'LDMset' in command: # look down max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('look_down_max', newValue)
servos.HEAD_PITCH_DOWN_MAX = newValue
servos.headPitch(newValue)
elif 'LUMset' in command: # look up max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('look_up_max', newValue)
servos.HEAD_PITCH_UP_MAX = newValue
servos.headPitch(newValue)
elif 'EC2set' in command: # yaw Adjustment
newValue = int((str(command))[7:])
config.exportConfig('yaw_middle', newValue)
servos.HEAD_YAW_MIDDLE = newValue
servos.headYaw(newValue)
elif 'LLMset' in command: # look left max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('look_left_max', newValue)
servos.HEAD_YAW_LEFT_MAX = newValue
servos.headYaw(newValue)
elif 'LRMset' in command: # look right max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('look_right_max', newValue)
servos.HEAD_YAW_RIGHT_MAX = newValue
servos.headYaw(newValue)
elif 'STEERINGset' in command: #Motor Steering center Adjustment
newValue = int((str(command))[12:])
config.exportConfig('turn_middle', newValue)
servos.STEERING_MIDDLE = newValue
servos.steerMiddle()
elif 'SLMset' in command: # Steering left max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('turn_left_max', newValue)
servos.STEERING_LEFT_MAX = newValue
servos.steerFullLeft()
elif 'SRMset' in command: # Steering right max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('turn_right_max', newValue)
servos.STEERING_RIGHT_MAX = newValue
servos.steerFullRight()
elif 'EM1set' in command: #Motor A Speed Adjustment
newValue = int((str(command))[7:])
config.exportConfig('E_M1', newValue)
elif 'EM2set' in command: #Motor B Speed Adjustment
newValue = int((str(command))[7:])
config.exportConfig('E_M2', newValue)
elif 'stop' in command: #When server receive "stop" from client,car stops moving
socket.send('9'.encode())
motor.motorStop()
#setup()
if led_status == 0:
headlights.turn(headlights.BOTH, headlights.OFF)
colorWipe(ledStrip, rpi_ws281x.Color(0, 0, 0))
elif 'lightsON' in command: #Turn on the LEDs
headlights.turn(headlights.BOTH, headlights.WHITE)
led_status = 1
socket.send('lightsON'.encode())
elif 'lightsOFF' in command: #Turn off the LEDs
headlights.turn(headlights.BOTH, headlights.OFF)
led_status = 0
socket.send('lightsOFF'.encode())
elif 'SteerLeft' in command: #Turn more to the left
headlights.turn(headlights.RIGHT, headlights.OFF)
headlights.turn(headlights.LEFT, headlights.YELLOW)
servos.turnSteering(servos.LEFT)
turn_status = LEFT_TURN
elif 'SteerRight' in command: #Turn more to the Right
headlights.turn(headlights.LEFT, headlights.OFF)
headlights.turn(headlights.RIGHT, headlights.YELLOW)
servos.turnSteering(servos.RIGHT)
turn_status = RIGHT_TURN
elif 'middle' in command: #Go straight
headlights.turn(headlights.BOTH, headlights.BLUE)
turn_status = NO_TURN
servos.steerMiddle()
elif 'Left' in command: #Turn hard left
headlights.turn(headlights.RIGHT, headlights.OFF)
headlights.turn(headlights.LEFT, headlights.YELLOW)
servos.steerFullLeft()
turn_status = LEFT_TURN
socket.send('3'.encode())
elif 'Right' in command: #Turn hard right
headlights.turn(headlights.LEFT, headlights.OFF)
headlights.turn(headlights.RIGHT, headlights.YELLOW)
servos.steerFullRight()
turn_status = RIGHT_TURN
socket.send('4'.encode())
elif 'backward' in command: #When server receive "backward" from client,car moves backward
socket.send('2'.encode())
motor.move(motor.BACKWARD, right_spd * spd_adj)
elif 'forward' in command: #When server receive "forward" from client,car moves forward
socket.send('1'.encode())
motor.move(motor.FORWARD, right_spd * spd_adj)
elif 'l_up' in command: #Camera look up
servos.changePitch(servos.UP)
socket.send('5'.encode())
elif 'l_do' in command: #Camera look down
servos.changePitch(servos.DOWN)
socket.send('6'.encode())
elif 'l_le' in command: #Camera look left
servos.changeYaw(servos.LEFT)
socket.send('7'.encode())
elif 'l_ri' in command: #Camera look right
servos.changeYaw(servos.RIGHT)
socket.send('8'.encode())
elif 'ahead' in command: #Camera look ahead
servos.lookAhead()
elif 'Off' in command: #When server receive "Off" from client, Auto Mode switches off
opencv_mode = 0
findline_mode = 0
speech_mode = 0
auto_mode = 0
auto_status = 0
time.sleep(.3) # wait for threads to detect the off state
dis_scan = 1
socket.send('auto_status_off'.encode())
motor.motorStop()
headlights.turn(headlights.BOTH, headlights.OFF)
servos.steerMiddle()
turn_status = NO_TURN
elif 'auto' in command: #When server receive "auto" from client,start Auto Mode
if auto_status == 0:
socket.send('0'.encode())
auto_status = 1
auto_mode = 1
dis_scan = 0
elif 'opencv' in command: #When server receive "auto" from client,start Auto Mode
if auto_status == 0:
auto_status = 1
opencv_mode = 1
socket.send('oncvon'.encode())
elif 'findline' in command: #Find line mode start
if auto_status == 0:
socket.send('findline'.encode())
auto_status = 1
findline_mode = 1
elif 'voice_3' in command: #Speech recognition mode start
if auto_status == 0:
auto_status = 1
speech_mode = 1
socket.send('voice_3'.encode())
def opencv_thread(): #OpenCV and FPV video
hoz_mid_orig = servos.yawPosition
vtr_mid_orig = servos.pitchPosition
font = cv2.FONT_HERSHEY_SIMPLEX
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
image = frame.array
cv2.line(image, (300, 240), (340, 240), (128, 255, 128), 1)
cv2.line(image, (320, 220), (320, 260), (128, 255, 128), 1)
if opencv_mode == 1:
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, colorLower, colorUpper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
if len(cnts) > 0:
headlights.turn(headlights.BOTH, headlights.GREEN)
cv2.putText(image, 'Target Detected', (40, 60), font, 0.5,
(255, 255, 255), 1, cv2.LINE_AA)
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
X = int(x)
Y = int(y)
if radius > 10:
cv2.rectangle(image, (int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(255, 255, 255), 1)
if X < 310:
mu1 = int((320 - X) / 3)
hoz_mid_orig += mu1
hoz_mid_orig = servos.headYaw(hoz_mid_orig)
#print('x=%d'%X)
elif X > 330:
mu1 = int((X - 330) / 3)
hoz_mid_orig -= mu1
hoz_mid_orig = servos.headYaw(hoz_mid_orig)
#print('x=%d'%X)
else:
servos.steerMiddle()
mu_t = 390 - (servos.STEERING_MIDDLE - hoz_mid_orig)
servos.steer(mu_t)
dis = dis_data
if dis < (distance_stay - 0.1):
headlights.turn(headlights.BOTH, headlights.RED)
motor.move(motor.BACKWARD, right_spd * spd_ad_u)
cv2.putText(image, 'Too Close', (40, 80), font, 0.5,
(128, 128, 255), 1, cv2.LINE_AA)
elif dis > (distance_stay + 0.1):
motor.move(motor.FORWARD, right_spd * spd_ad_2)
cv2.putText(image, 'OpenCV Tracking', (40, 80), font,
0.5, (128, 255, 128), 1, cv2.LINE_AA)
else:
motor.motorStop()
headlights.turn(headlights.BOTH, headlights.BLUE)
cv2.putText(image, 'In Position', (40, 80), font, 0.5,
(255, 128, 128), 1, cv2.LINE_AA)
if dis < 8:
cv2.putText(image, '%s m' % str(round(dis, 2)),
(40, 40), font, 0.5, (255, 255, 255), 1,
cv2.LINE_AA)
if Y < 230:
mu2 = int((240 - Y) / 5)
vtr_mid_orig += mu2
vtr_mid_orig = servos.headPitch(vtr_mid_orig)
elif Y > 250:
mu2 = int((Y - 240) / 5)
svtr_mid_orig = servos.headPitch(vtr_mid_orig)
if X > 280:
if X < 350:
cv2.line(image, (300, 240), (340, 240),
(64, 64, 255), 1)
cv2.line(image, (320, 220), (320, 260),
(64, 64, 255), 1)
cv2.rectangle(image,
(int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(64, 64, 255), 1)
else:
headlights.turn(headlights.BOTH, headlights.YELLOW)
cv2.putText(image, 'Target Detecting', (40, 60), font, 0.5,
(255, 255, 255), 1, cv2.LINE_AA)
led_y = 1
motor.motorStop()
for i in range(1, len(pts)):
if pts[i - 1] is None or pts[i] is None:
continue
thickness = int(np.sqrt(args["buffer"] / float(i + 1)) * 2.5)
cv2.line(image, pts[i - 1], pts[i], (0, 0, 255), thickness)
else:
dis = dis_data
if dis < 8:
cv2.putText(image, '%s m' % str(round(dis, 2)), (40, 40), font,
0.5, (255, 255, 255), 1, cv2.LINE_AA)
encoded, buffer = cv2.imencode('.jpg', image)
jpg_as_text = base64.b64encode(buffer)
footage_socket.send(jpg_as_text)
rawCapture.truncate(0)