def handle_event(payload):
type = payload['type']
if type == 'move':
x = payload['x']
y = payload['y']
#print str(x) + ' ' + str(y)
motor.move(x, y)
elif type == 'up':
motor.up()
elif type == 'down':
motor.down()
elif type == 'left':
motor.left()
elif type == 'right':
motor.right()
def motorControl():
global leftMotorSpeed
global initialMotorSpeed
global PD_value
global rightMotorSpeed
leftMotorSpeed = initialMotorSpeed - PD_value;
rightMotorSpeed = initialMotorSpeed + PD_value;
#leftMotorSpeed = leftMotorSpeed + 10
leftMotorSpeed = np.clip(leftMotorSpeed, 0, maxSpeed)
rightMotorSpeed = np.clip(rightMotorSpeed, 0, maxSpeed)
motor.setLeftMotorSpeed(leftMotorSpeed)
motor.setRightMotorSpeed(rightMotorSpeed)
motor.move()
def search(rssi, angle):
'''
Searches for the position of stringest RSSI
:param rssi: The current RSSI value
:param angle: The current angle of the servo motor
:return: The stringest RSSI value found, and the angle of that reading
'''
direction = -1 #default is left
dir_change = 0
times_moved = 0
rssi_dict = {}
rssi_dict[angle] = rssi
while (1):
print("searhing...")
angle = motor.move(angle, direction)
times_moved += 1
rssi_new_list, tx_power_left = getRSSIandTX()
rssi_new = rssi_new_list[0]
print "rssi: ", rssi
rssi_dict[angle] = rssi_new
if rssi > rssi_new and rssi_new != 0:
print "Changeing Direction"
direction = direction * -1
dir_change += 1
angle = motor.move(angle, direction)
if (dir_change >= 2 and times_moved < 2) or (dir_change < 2
and times_moved > 2):
print rssi_dict
print angle
return angle, rssi_new
times_moved = 0
elif angle == 0 or angle == 180:
print "Changeing Direction"
direction = direction * -1
dir_change += 1
angle = motor.move(angle, direction)
times_moved = 0
elif rssi_new != 0:
rssi = rssi_new
rssi_dict[angle] = rssi_new
return angle, rssi
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 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)
pwmRight = maxSpeed - abs(power)
pwmLeft = maxSpeed + abs(power)
else:
pwmRight = maxSpeed + power
pwmLeft = maxSpeed - power
pwmRight = pwmRight + 3
if (pwmRight > 100):
pwmRight = 100
elif (pwmRight < -100):
pwmRight = -100
if (pwmLeft > 100):
pwmLeft = 100
elif (pwmLeft < -100):
pwmLeft = -100
print str(pwmLeft) + "\t" + str(pwmRight) + "\t" + str(
power) + "\t" + str(lost)
motor.setLeftMotorSpeed(pwmLeft)
motor.setRightMotorSpeed(pwmRight)
motor.move()
finally:
print "Goodbye :)"
motor.clear()
#os.system('sudo halt')
time.sleep(0.05)
l = j.argmax() - k.astype(int)
if l != 0:
points.append([l, base-i*50])
cv.arrowedLine(frame, (320,480),(l, base-i*50), (170,255-(i*40),255-(i*40)),thickness=2)
angle = abs(np.rad2deg(np.arctan2((base-i*50) - 480, l - 320))) - 90
angles.append(angle)
org = (10, 20*i)
fontScale = .5
font = cv.FONT_HERSHEY_SIMPLEX
color = (170, 255-(i*40), 255-(i*40))
thickness = 2
cv.putText(frame, f'Angle {i}: {round(angle,2)}', org, font, fontScale, color, thickness, cv.LINE_AA)
# else:
# pass
# points.append(False)
# angles.append(False)
# if angles:
# approach(angles)
if points:
approach(points[-1][0])
else:
move(timed=True)
print('Lost the line...')
sys.exit()
cv.imshow('mask',mask)
cv.imshow('frame', frame)
k = cv.waitKey(5) & 0xFF
if k == 27:
break
cv.destroyAllWindows()
def customCallback(client, userdata, message):
global speed, direction
print("Received a new message: ", message.payload)
print("--------------\n\n")
try:
cmd_dict = json.loads(message.payload.decode('utf-8'))
# cmd_gyro = json.loads(message.payload)
#cmd_dict = json.loads(message.payload)
#print (cmd_dict)
cmd = cmd_dict["cmd"]
val = cmd_dict["val"]
if cmd == carCmd[0]:
speed = val
elif cmd == carCmd[1]:
direction = val
elif cmd == "MovePiCam":
duty_cycle_UD = pi.get_servo_pulsewidth(servo_pin_UD)
duty_cycle_RL = pi.get_servo_pulsewidth(servo_pin_RL)
if (val == "Right"):
duty_cycle_RL = duty_cycle_RL - 30
if (val == "Left"):
duty_cycle_RL = duty_cycle_RL + 30
if (val == "Up"):
duty_cycle_UD = duty_cycle_UD + 30
if (val == "Down"):
duty_cycle_UD = duty_cycle_UD - 30
if (len(val.split()) == 2):
# if (len(message.payload.split()) == 2):
print(
"$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$"
)
try:
angle_x = float(val.split()[0]) #Range 0 - 90 deg
angle_y = float(val.split()[1]) #Range 0 - 90 deg
duty_cycle_RL = MIN_DUTY + ((1300 * angle_x) / 90)
duty_cycle_UD = MIN_DUTY + ((1300 * angle_y) / 90)
except:
print("ERROR: angles are not floats!")
#---STAY IN LIMITS---
if (duty_cycle_UD < MIN_DUTY):
duty_cycle_UD = MIN_DUTY
if (duty_cycle_UD > MAX_DUTY):
duty_cycle_UD = MAX_DUTY
if (duty_cycle_RL < MIN_DUTY_RL):
duty_cycle_RL = MIN_DUTY_RL
if (duty_cycle_RL > MAX_DUTY_RL):
duty_cycle_RL = MAX_DUTY_RL
#---SET SERVO---
print("Tilt axis: [Up/Down] ")
print(duty_cycle_UD)
print("Pan axis: [Right/Left]: ")
print(duty_cycle_RL)
pi.set_servo_pulsewidth(servo_pin_UD, duty_cycle_UD)
pi.set_servo_pulsewidth(servo_pin_RL, duty_cycle_RL)
print("cmd is : ", cmd, "------val is : ", val)
#------------------------CALLBACKS DONE----------------------------
except KeyboardInterrupt:
speed = 5
direction = 5
except ValueError:
print("MQTT message is not a valid JSON")
except KeyError:
print("MQTT JSON object does not contain the key power_on")
motor.move(speed, direction)
def left():
motor.move(7.5, 7.5)
return 'left'
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)
