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 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)