def move_servo_to_ang_pimoroni(self, axis, angle_to_reach): if axis == "PAN": sleepTime = self._settings.get_int(["sleepTimeX"]) actual_angle = self.pimoroni_to_angle(pantilthat.get_pan()) if self._settings.get_boolean(["xInvert"]): angle_to_reach = 180 - angle_to_reach if angle_to_reach - actual_angle >= 0: incrementSign = 1 else: incrementSign = -1 for x in range(actual_angle + 1, angle_to_reach + 1, incrementSign): angle_current = self.pimoroni_to_angle(pantilthat.get_pan()) if angle_current > 179: self._logger.info( "PAN reached his boundaries with a {} pulse width". format(angle_current)) pantilthat.pan(self.angle_to_pimoroni(179)) break elif angle_current < 1: self._logger.info( "PAN reached his boundaries with a {} pulse width". format(angle_current)) pantilthat.pan(self.angle_to_pimoroni(1)) break pantilthat.pan(self.angle_to_pimoroni(x)) #self._logger.info("Setting the width of PAN at {} deg at pimoroni format {}".format(x, self.angle_to_pimoroni(x))) time.sleep(sleepTime / 1000) if axis == "TILT": sleepTime = self._settings.get_int(["sleepTimeY"]) actual_angle = self.pimoroni_to_angle(pantilthat.get_tilt()) if self._settings.get_boolean(["yInvert"]): angle_to_reach = 180 - angle_to_reach if angle_to_reach - actual_angle >= 0: incrementSign = 1 else: incrementSign = -1 for x in range(actual_angle + 1, angle_to_reach + 1, incrementSign): angle_current = self.pimoroni_to_angle(pantilthat.get_tilt()) if self._settings.get_boolean(["yInvert"]): self._settings.set(["currentY"], 180 - angle_current) else: self._settings.set(["currentY"], angle_current) self._settings.save() if angle_current > 179: self._logger.info( "TILT reached his boundaries with a {} pulse width". format(angle_current)) pantilthat.tilt(self.angle_to_pimoroni(179)) break elif angle_current < 1: self._logger.info( "TILT reached his boundaries with a {} pulse width". format(angle_current)) pantilthat.tilt(self.angle_to_pimoroni(1)) break pantilthat.tilt(self.angle_to_pimoroni(x)) #self._logger.info("Setting the width of TILT at {} deg at pimoroni format {}".format(x, self.angle_to_pimoroni(x))) time.sleep(sleepTime / 1000)
def get_pan(): """Get the current pan value Returns: int: Current pan value """ return pantilthat.get_pan()
def get_pan(): """Get the current pan value Returns: int: Current pan value """ return pantilthat.get_pan()
def center_camera(objxy, screencenter, debug=False): ''' from an (X,Y), it tries to pan/tilt to minimize the distance with the center of the "view" NOTE: From the viewer's pov: If face is upper left: -> dX < 0 & dY >0 -> pan++ aims left, tilt-- aims up Args: objxy: Tuple (x,y) of the detected object screencenter: Tuple(x,y) with the center of the screen Returns: 1 if moved 0 if not ''' max_angle = 80 # To stay safe and not exeed the max angle of the servo stepx = MINANGLESTEP # 1 by default #+ abs(int(objxy[0]/100)) stepy = MINANGLESTEP dX = screencenter[0] - objxy[0] dY = screencenter[1] - objxy[1] stepx = int(abs(dX)/10 - IMGTHRESHOLD/10) # Empirical value. 10% of the distance, good enough stepy = int(abs(dY)/10 - IMGTHRESHOLD/10) currentPan = pantilthat.get_pan() currentTilt = pantilthat.get_tilt() # Computing the next angle, starting on the current newPan = currentPan newTilt = currentTilt if dX < 0 - IMGTHRESHOLD: # and abs(currentPan) + stepx < max_angle: newPan = currentPan + stepx newPan = newPan if newPan < max_angle else max_angle elif dX > 0 + IMGTHRESHOLD: # and abs(currentPan) + stepx < max_angle: newPan = currentPan - stepx newPan = newPan if newPan > -max_angle else -max_angle if dY < 0 - IMGTHRESHOLD: # and abs(currentTilt) + stepy < max_angle: newTilt = currentTilt + stepy newTilt = newTilt if newTilt < max_angle else max_angle elif dY > 0 + IMGTHRESHOLD: # and abs(currentTilt) + stepy < max_angle: newTilt = currentTilt - stepy newTilt = newTilt if newTilt > -max_angle else -max_angle if debug: print(f"({objxy}) status: pan:{currentPan}, tilt:{currentTilt}; (dX:{dX}, dy:{dY}, step:{stepx},{stepy})({newPan},{newTilt})") result = 1 if newPan != currentPan or newTilt != currentTilt else 0 pantilthat.pan(newPan) pantilthat.tilt(newTilt) return result
def pan_till_detected(direction, s): global pan_running global found global recieved_y_axis print("pan till detected is running") pan_angle = pantilthat.get_pan() turn_amt = 1 sleep_interval = .07 while (abs(pan_angle) + turn_amt < 90 and not found): pantilthat.pan(pan_angle + (-1 * direction) * turn_amt) pantilthat.tilt(recieved_y_axis) time.sleep(.07) pan_angle = pantilthat.get_pan() msg = '3' print(msg) s.send(msg.encode('utf-8')) '''
def moveRelative(dir, angle): with VAR.lock: if dir == 'left' or dir == 'down': angle = -angle if dir == 'left' or dir == 'right': angle += pantilthat.get_pan() pantilthat.pan(median([-90, angle, 90])) elif dir == 'up' or dir == 'down': angle += pantilthat.get_tilt() pantilthat.tilt(median([-90, angle, 90]))
def directionPositive(direction, angle): #Function for setting the new angle #and for turing on the LED's if direction == "tilt": currAngle = pantilthat.get_tilt() newAngle = servoFormulaPositive(currAngle, angle) os.system('sudo ./piiotest blink 20 1') pantilthat.tilt(newAngle) os.system('sudo ./piiotest blink 20 0') elif direction == "pan": currAngle = pantilthat.get_pan() newAngle = servoFormulaPositive(currAngle, angle) os.system('sudo ./piiotest writepin 20 1') pantilthat.pan(newAngle) os.system('sudo ./piiotest writepin 20 0')
def set_servos(pan, tlt, toCenter): # signal trap to handle keyboard interrupt signal.signal(signal.SIGINT, signal_handler) # loop indefinitely while True: # the pan and tilt angles are reversed # prevPan = panAngle # prevTilt = tltAngle panAngle = -1 * pan.value tltAngle = -1 * tlt.value if toCenter.value == 1: panAngle = 0 tltAngle = 0 waitForServo = False if abs(panAngle - pth.get_pan()) > MIN_ACCEPTABLE_ROTATION: waitForServo = True #print("detected suden shift in angle") if abs(tltAngle - pth.get_tilt()) > MIN_ACCEPTABLE_ROTATION: waitForServo = True #print("detected suden shift in angle") # if the pan angle is within the range, pan if in_range(panAngle, servoRangePan[0], servoRangePan[1]): # if abs(prevPan - panAngle) < 60 : # pth.pan(panAngle) #else: # pth.pan(int(panAngle/2)) pth.pan(panAngle) #print(panAngle) # if the tilt angle is within the range, tilt if in_range(tltAngle, servoRangeTilt[0], servoRangeTilt[1]): #if abs(prevTilt - tltAngle) < 60 : # pth.tilt(tltAngle) #else: # pth.tilt(int(tltAngle/2)) pth.tilt(tltAngle) #print(tltAngle) if waitForServo is True: time.sleep(0.6) print("waiting cuz camera stabilization") time.sleep(0.1)
def move_motor(): getvar_dict = request.query.decode() pantilt = request.query.pantilt direction = int(request.query.direction) if pantilt == "pan": pan_value = pantilthat.get_pan() if direction == -1: if pan_value - STEP_SIZE >= -90: pantilthat.pan(pan_value - STEP_SIZE) return ("Pan right") else: return ("Pan right limit reached") elif direction == 1: if pan_value + STEP_SIZE <= 90: pantilthat.pan(pan_value + STEP_SIZE) return ("Pan left") else: return ("Pan left limit reached") else: return ("Invalid direction") elif pantilt == "tilt": tilt_value = pantilthat.get_tilt() if direction == -1: if tilt_value - STEP_SIZE >= -90: pantilthat.tilt(tilt_value - STEP_SIZE) return ("Tilt up") else: return ("Tilt up limit reached") elif direction == 1: if tilt_value + STEP_SIZE <= 90: pantilthat.tilt(tilt_value + STEP_SIZE) return ("Tilt down") else: return ("Tilt down limit reached") else: return ("Invalid direction") else: return ("Invalid command")
def getPosition(): return jsonify(pan=pantilthat.get_pan() + 90, tilt=pantilthat.get_tilt() + 90)
Shelf(68, 24, -28, -40), Shelf(80, 26, -25, 0), Shelf(88, 25, -26, +40), Shelf(68, 26, -26, +65) ] # sanity checks if len(sys.argv) != 3: print "Usage: <shelf id> <shelf pos>\n" exit() # setup servo_ranges.calibrate() # read last cmd orig_pan = pantilthat.get_pan() orig_tilt = pantilthat.get_tilt() print "found pan: %i; tilt: %i" % (orig_pan, orig_tilt) # get args in_id = int(sys.argv[1]) in_id = (in_id - 1) % max_shelves # convert to C array notation in_pos = int(sys.argv[2]) print "searching: %i %i" % (in_id, in_pos) # find new_pan = shelves[in_id].map_pos_to_angles(in_pos) new_tilt = shelves[in_id].tilt_pos # debug print "output: %i %i" % (new_pan, new_tilt)
def ball_tracking(): # construct the argument parse and parse the arguments ap = argparse.ArgumentParser() ap.add_argument("-b", "--buffer", type=int, default=64, help="max buffer size") args = vars(ap.parse_args()) # define the lower and upper boundaries of the "green" # ball in the HSV color space, then initialize the # list of tracked points greenLower = (0, 150, 150) greenUpper = (64, 255, 255) pts = deque(maxlen=args["buffer"]) vs = VideoStream(src=0).start() pantilthat.pan(0) pantilthat.tilt(0) # allow the camera or video file to warm up time.sleep(2.0) # keep looping keep_looping = True while keep_looping: # grab the current frame frame = vs.read() # handle the frame from VideoCapture or VideoStream frame = frame[1] if args.get("video", False) else frame # resize the frame, blur it, and convert it to the HSV # color space frame = imutils.resize(frame, width=600) blurred = cv2.GaussianBlur(frame, (11, 11), 0) hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV) # construct a mask for the color "yellow", then perform # a series of dilations and erosions to remove any small # blobs left in the mask mask = cv2.inRange(hsv, greenLower, greenUpper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) # find contours in the mask and initialize the current # (x, y) center of the ball cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) center = None # only proceed if at least one contour was found if len(cnts) > 0: # find the largest contour in the mask, then use # it to compute the minimum enclosing circle and # centroid 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"])) # only proceed if the radius meets a minimum size if radius > 10: # draw the circle and centroid on the frame, # then update the list of tracked points cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(frame, center, 5, (0, 0, 255), -1) pantilthat.pan(pantilthat.get_pan() + (center[0] - 300) / 50) pantilthat.tilt(pantilthat.get_tilt() - (center[1] - 240) / 50) # update the points queue pts.appendleft(center)
#find middle x_medium = int((x + x + w) / 2) y_medium = int((y + y + h) / 2) ##Draw line on x axis cv2.line(frame, (x_medium, 0), (x_medium, 480), (0, 255, 0), 2) cv2.line(frame, (0, y_medium), (680, y_medium), (0, 255, 0), 2) ##print('X: ' + str(x_medium ) + ' Y: ' + str(y_medium ) ) ##move servo print('X: ' + str(x_medium - 340) + ' Y: ' + str(y_medium)) ##X axis if (x_medium - 340) > 40: if (pantilthat.get_pan() + 1) < 90: position = pantilthat.get_pan() + 1 pantilthat.servo_one(position) if (x_medium - 340) < -40: if (pantilthat.get_pan() - 1) > -90: position = pantilthat.get_pan() - 1 pantilthat.servo_one(position) #y axis if (y_medium - 240) < 40: if (pantilthat.get_tilt() + 1) < 90: position = pantilthat.get_tilt() + 1 pantilthat.servo_two(position) if (y_medium - 240) > -40:
def servoPan_pos(event): b = pantilthat.get_pan()+1 print(b) pantilthat.pan(b)
def do_PAN( self, value = 0, maximum = 70 ) : target = pantilthat.get_pan() + value if target < - maximum : target = - maximum if target > maximum : target = maximum print( value, target ) pantilthat.pan( target )
def move_left(): pantilthat.pan(pantilthat.get_pan()+3) print("moved left")
def handleMessage(self): global speaking global t1 command = self.data print("command",command) if(command == "left_a_bit"): try: p = pantilthat.get_pan() print("p",p) smooth_pan(p,p+10) except: traceback.print_exc() if(command == "right_a_bit"): try: p = pantilthat.get_pan() print("p",p) smooth_pan(p,p-10) except: traceback.print_exc() if(command == "down_a_bit"): t = pantilthat.get_tilt() try: print("t - down a bit",t) smooth_tilt(t,t+10) except: traceback.print_exc() if(command == "up_a_bit"): t = pantilthat.get_tilt() try: smooth_tilt(t,t-10) except: traceback.print_exc() if(command == "stopspeaking"): speaking = False print("Got command stopspeaking",command) print("speaking is false") try: t1.join() except: traceback.print_exc() smile.smile() if(command == "speaking"): speaking = True print("Got command speaking") try: t1 = threading.Thread(target=foo, args=()) t1.start() except: traceback.print_exc() # used by the software mostly if(command == "gone"): t = pantilthat.get_tilt() p = pantilthat.get_pan() smooth_tilt(t,80) smooth_pan(t,0) if(command == "arrived"): t = pantilthat.get_tilt() p = pantilthat.get_pan() smooth_tilt(t,0) smooth_pan(t,0) # used by the end user / frontend if(command == "hello"): smile.smile() t = pantilthat.get_tilt() smooth_tilt(t,40,0.01) smooth_tilt(40,-70,0.01) smooth_tilt(-70,0,0.01) if(command == "leaving"): t = pantilthat.get_tilt() smooth_tilt(t,90) if(command == "left"): p = pantilthat.get_pan() smooth_pan(p,60) if(command == "right"): p = pantilthat.get_pan() smooth_pan(p,-60) if(command == "halt"): t = pantilthat.get_tilt() p = pantilthat.get_pan() smooth_tilt(t,80) smooth_pan(p,0) # sleep time.sleep(5) result = os.system("sudo halt") if(command == "reboot"): t = pantilthat.get_tilt() p = pantilthat.get_pan() smooth_tilt(t,80) smooth_pan(p,0) time.sleep(5) result = os.system("sudo reboot")
def servoPan_neg(event): a = pantilthat.get_pan()-1 print(a) pantilthat.pan(a)
if (len(list) >= 1): command = list[0]; if (command == 'exit'): exit(); else: try: if (command == 'pan' and len(list)>=2): pantilthat.pan(float(list[1])); elif (command == 'tilt' and len(list)>=2): pantilthat.tilt(float(list[1])); elif (command == 'goto' and len(list)>=3): pantilthat.pan(float(list[1])); pantilthat.tilt(float(list[2])); elif (command == 'get_pan'): print(pantilthat.get_pan()); elif (command == 'get_tilt'): print(pantilthat.get_tilt()); elif( command == 'light_type' and len(list)>=2): if( list[1] == 'RGB' ): pantilthat.light_type(pantilthat.RGB); elif( list[1] == 'GRB' ): pantilthat.light_type(pantilthat.GRB); elif( list[1] == 'RGBW' ): pantilthat.light_type(pantilthat.RGBW); elif( list[1] == 'GRBW' ): pantilthat.light_type(pantilthat.GRBW); else: print('error: light_type requires parameter RBB, GRB, RGBW or GRBW'); elif( command == 'light_mode' and len(list)>=2): if( list[1] == 'PWM' ):
def on_api_command(self, command, data): if command == "EASYSERVO_REL": if len(data) == 3: if pigpioUsed: GPIO, ang = data["pin"], data["angle"] if int(GPIO) == self._settings.get_int(["GPIOX"]) or int(GPIO) == self._settings.get_int(["GPIOY"]): thread = threading.Thread(target=self.move_servo_by, args=(int(GPIO), int(ang))) thread.daemon = True thread.start() else: self._logger.info("unknown GPIO %d" % int(GPIO)) else: axis, ang = data["pin"], data["angle"] if axis == "PAN" or axis == "TILT": thread = threading.Thread(target=self.move_servo_by_pimoroni, args=(str(axis), int(ang))) thread.daemon = True thread.start() else: self._logger.info("please use PAN or TILT instead of '" + str(axis)) + "'" else: self._logger.info("please use the EASYSERVO_REL PIN/AXIS ANGLE instead of '{} {}'".format(str(command), str(parameters))) if command == 'EASYSERVO_ABS': if len(data) == 3: if pigpioUsed: GPIO, ang = data["pin"], data["angle"] #self._logger.info("data {} GPIO {} ang {}".format(data, GPIO, ang)) if int(GPIO) == self._settings.get_int(["GPIOX"]) or int(GPIO) == self._settings.get_int(["GPIOY"]): thread = threading.Thread(target=self.move_servo_to_ang, args=(int(GPIO), int(ang))) thread.daemon = True thread.start() else: self._logger.info("unknown GPIO %d" % int(GPIO)) else: axis, ang = data["pin"], data["angle"] if str(axis) == "PAN" or str(axis) == "TILT": thread = threading.Thread(target=self.move_servo_to_ang_pimoroni, args=(str(axis), int(ang))) thread.daemon = True thread.start() else: self._logger.info("please use PAN or TILT instead of '" + str(axis)) + "'" else: self._logger.info("please use EASYSERVO_ABS PIN/AXIS ANGLE instead of '{} {}'".format(str(command), str(parameters))) if command == 'EASYSERVOAUTOHOME': xAutoAngle = self._settings.get_int(["xAutoAngle"]) yAutoAngle = self._settings.get_int(["yAutoAngle"]) if len(data) == 3: if pigpioUsed: GPIO1, GPIO2 = data["pin1"], data["pin2"] if (int(GPIO1) == self._settings.get_int(["GPIOX"]) and int(GPIO2) == self._settings.get_int(["GPIOY"])) or \ (int(GPIO1) == self._settings.get_int(["GPIOY"]) and int(GPIO2) == self._settings.get_int(["GPIOX"])): thread_x = threading.Thread(target=self.move_servo_to_ang, args=(int(GPIO1), xAutoAngle)) thread_x.daemon = True thread_x.start() thread_y = threading.Thread(target=self.move_servo_to_ang, args=(int(GPIO2), yAutoAngle)) thread_y.daemon = True thread_y.start() else: self._logger.info("unknown GPIO1 {} or GPIO2 {}".format(int(GPIO1), int(GPIO2))) else: axis1, axis2 = data["pin1"], data["pin2"] if (str(axis1) == "PAN" and str(axis2) == "TILT") or (str(axis1) == "TILT" and str(axis2) == "PAN"): if not self._settings.get_boolean(["axisInvert"]): xAxis = "PAN" yAxis = "TILT" else: xAxis = "TILT" yAxis = "PAN" thread_x = threading.Thread(target=self.move_servo_to_ang_pimoroni, args=(xAxis, xAutoAngle)) thread_x.daemon = True thread_x.start() thread_y = threading.Thread(target=self.move_servo_to_ang_pimoroni, args=(yAxis, yAutoAngle)) thread_y.daemon = True thread_y.start() else: self._logger.info("unknown AXIS1 {} or AXIS2 {}".format(str(axis1), str(axis2))) elif len(data) == 2: if pigpioUsed: GPIO = data["pin"] if int(GPIO) == self._settings.get_int(["GPIOX"]): thread = threading.Thread(target=self.move_servo_to_ang, args=(int(GPIO), xAutoAngle)) thread.daemon = True thread.start() elif int(GPIO) == self._settings.get_int(["GPIOY"]): thread = threading.Thread(target=self.move_servo_to_ang, args=(int(GPIO), yAutoAngle)) thread.daemon = True thread.start() else: self._logger.info("unknown GPIO %d" % int(GPIO)) else: axis = data["pin"] if str(axis) == "PAN": if not self._settings.get_boolean(["axisInvert"]): xAxis = "PAN" else: xAxis = "TILT" thread = threading.Thread(target=self.move_servo_to_ang_pimoroni, args=(str(xAxis), xAutoAngle)) thread.daemon = True thread.start() if str(axis) == "TILT": if not self._settings.get_boolean(["axisInvert"]): yAxis = "TILT" else: yAxis = "PAN" thread = threading.Thread(target=self.move_servo_to_ang_pimoroni, args=(str(yAxis), yAutoAngle)) thread.daemon = True thread.start() else: self._logger.info("unknown axis {}".format(str(axis))) if command == "EASYSERVO_GET_POSITION": if pigpioUsed: if self._settings.get_boolean(["xInvert"]): currentX = 180 - self.width_to_angle(self.pi.get_servo_pulsewidth(self._settings.get_int(["GPIOX"]))) else: currentX = self.width_to_angle(self.pi.get_servo_pulsewidth(self._settings.get_int(["GPIOX"]))) if self._settings.get_boolean(["yInvert"]): currentY = 180 - self.width_to_angle(self.pi.get_servo_pulsewidth(self._settings.get_int(["GPIOY"]))) else: currentY = self.width_to_angle(self.pi.get_servo_pulsewidth(self._settings.get_int(["GPIOY"]))) self._plugin_manager.send_plugin_message("EasyServo", "{} {}".format(currentX, currentY)) else: if self._settings.get_boolean(["xInvert"]): currentX = 180 - self.pimoroni_to_angle(pantilthat.get_pan()) else: currentX = self.pimoroni_to_angle(pantilthat.get_pan()) if self._settings.get_boolean(["yInvert"]): currentY = 180 - self.pimoroni_to_angle(pantilthat.get_tilt()) else: currentY = self.pimoroni_to_angle(pantilthat.get_tilt()) if self._settings.get_boolean(["axisInvert"]): self._plugin_manager.send_plugin_message("EasyServo", "{} {}".format(currentY, currentX)) else: self._plugin_manager.send_plugin_message("EasyServo", "{} {}".format(currentX, currentY))
def move_servo_by_pimoroni(self, axis, angle_difference): #Relative positioning #self._logger.info("Just received a command with axis {} and angle {}".format(axis, angle_difference)) if axis == "PAN": sleepTime = self._settings.get_int(["sleepTimeX"]) minAngle = self._settings.get_int(["xMinAngle"]) maxAngle = self._settings.get_int(["xMaxAngle"]) actual_angle = self.pimoroni_to_angle(pantilthat.get_pan()) if self._settings.get_boolean(["xInvert"]): direction = -1 else: direction = 1 angle_to_reach = actual_angle + angle_difference * direction if angle_to_reach-actual_angle >= 0: incrementSign = 1 else: incrementSign = -1 #self._logger.info("actual_angle {} angle_to_reach {} incrementSign {}".format(actual_angle, angle_to_reach, incrementSign)) for x in range(actual_angle+1, angle_to_reach+1, incrementSign): angle_current = self.pimoroni_to_angle(pantilthat.get_pan()) if angle_current > maxAngle: self._logger.info("PAN reached his boundaries with a {} pulse width".format(angle_current)) pantilthat.pan(self.angle_to_pimoroni(maxAngle)) break elif angle_current < minAngle: self._logger.info("PAN reached his boundaries with a {} pulse width".format(angle_current)) pantilthat.pan(self.angle_to_pimoroni(minAngle)) break pantilthat.pan(self.angle_to_pimoroni(x)) #self._logger.info("Setting the angle of PAN at {} deg at pimoroni format {}".format(x, self.angle_to_pimoroni(x))) time.sleep(sleepTime / 1000) if axis == "TILT": sleepTime = self._settings.get_int(["sleepTimeY"]) minAngle = self._settings.get_int(["yMinAngle"]) maxAngle = self._settings.get_int(["yMaxAngle"]) actual_angle = self.pimoroni_to_angle(pantilthat.get_tilt()) if self._settings.get_boolean(["yInvert"]): direction = -1 else: direction = 1 angle_to_reach = actual_angle + angle_difference * direction if angle_to_reach-actual_angle >= 0: incrementSign = 1 else: incrementSign = -1 #self._logger.info("actual_angle {} angle_to_reach {} incrementSign {}".format(actual_angle, angle_to_reach, incrementSign)) for x in range(actual_angle+1, angle_to_reach+1, incrementSign): angle_current = self.pimoroni_to_angle(pantilthat.get_tilt()) if self._settings.get_boolean(["yInvert"]): self._settings.set(["currentY"], 180 - angle_current) else: self._settings.set(["currentY"], angle_current) self._settings.save() if angle_current > maxAngle: self._logger.info("TILT reached his boundaries with a {} pulse width".format(angle_current)) pantilthat.tilt(self.angle_to_pimoroni(maxAngle)) break elif angle_current < minAngle: self._logger.info("TILT reached his boundaries with a {} pulse width".format(angle_current)) pantilthat.tilt(self.angle_to_pimoroni(minAngle)) break pantilthat.tilt(self.angle_to_pimoroni(x)) #self._logger.info("Setting the angle of TILT at {} deg at pimoroni format {}".format(x, self.angle_to_pimoroni(x))) time.sleep(sleepTime / 1000)
import pantilthat import time import math # Set the maximum high pulse for a servo in microseconds. # pantilthat.servo_pulse_max(index, 100) # set position of servo one in degrees pantilthat.servo_one(-90) # set position of servo two in degrees pantilthat.servo_two(-10) # get position of servo one pantilthat.get_pan() # ----- a = 0 stop = 0 while True: # Get the time in seconds t = time.time() # G enerate an angle using a sine wave (-1 to 1) multiplied by 90 (-90 to 90) a = math.sin(t * 2) * 90 # Cast a to int for v0.0.2 a = int(a) pantilthat.pan(a) pantilthat.tilt(0) # Two decimal places is quite enough! print(a, t)
def get_angles(): pan = pantilthat.get_pan() tilt = pantilthat.get_tilt() return '{{"pan":{}, "tilt":{}}}'.format(pan + 90, tilt + 90)
# it to compute the minimum enclosing circle and # centroid 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"])) # only proceed if the radius meets a minimum size if radius > 10: # draw the circle and centroid on the frame, # then update the list of tracked points cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(frame, center, 5, (0, 0, 255), -1) pantilthat.pan(pantilthat.get_pan()+(center[0]-300)/50) pantilthat.tilt(pantilthat.get_tilt()-(center[1]-240)/50) # update the points queue pts.appendleft(center) ''' # loop over the set of tracked points for i in range(1, len(pts)): # if either of the tracked points are None, ignore # them if pts[i - 1] is None or pts[i] is None: continue
def main(): global found global client_input global within_range global past_range # construct the argument parse and parse the arguments ap = argparse.ArgumentParser() ap.add_argument("-b", "--buffer", type=int, default=64, help="max buffer size") args = vars(ap.parse_args()) # define the lower and upper boundaries of the "green" # ball in the HSV color space, then initialize the # list of tracked points greenLower = (0, 150, 150) greenUpper = (64, 255, 255) pts = deque(maxlen=args["buffer"]) vs = VideoStream(src=0).start() pantilthat.pan(0) pantilthat.tilt(0) # allow the camera or video file to warm up time.sleep(2.0) print("Starting server now") host = '192.168.1.78' port = 12000 # asks for user name file = open("../number.txt", "r") name = file.read() file.close() s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.settimeout(2) # connecting host try: s.connect((host, port)) except: print("\33[31m\33[1m Can't connect to the server \33[0m") sys.exit() # if connected s.send(name.encode('utf-8')) # display() # get_input = threading.Thread(target=wait_for_input()) # get_input.start() while 1: socket_list = [s] # Get the list of sockets which are readable # MAKE SURE TO USE 0 at the end # 0 means that select will not wait for server messages or sys.stdin to actually have an output. Otherwise the while loop gets blocked here and the ball_tracking section will not run. rList, wList, error_list = select.select(socket_list, [], [], 0) for sock in rList: # incoming message from server if sock == s: data = sock.recv(4096).decode() if not data: print('\33[31m\33[1m \rDISCONNECTED!!\n \33[0m') sys.exit() else: # read the data from other pis # sys.stdout.write(data) analyze(data, s) # display() # print("running ball tracking") # grab the current frame frame = vs.read() # handle the frame from VideoCapture or VideoStream frame = frame[1] if args.get("video", False) else frame # resize the frame, blur it, and convert it to the HSV # color space frame = imutils.resize(frame, width=600) blurred = cv2.GaussianBlur(frame, (11, 11), 0) hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV) # construct a mask for the color "yellow", then perform # a series of dilations and erosions to remove any small # blobs left in the mask mask = cv2.inRange(hsv, greenLower, greenUpper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) # find contours in the mask and initialize the current # (x, y) center of the ball cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) center = None # only proceed if at least one contour was found if len(cnts) > 0: if (not found): msg = '0' # print(msg.encode('utf-8')) s.send(msg.encode('utf-8')) # display() found = True # find the largest contour in the mask, then use # it to compute the minimum enclosing circle and # centroid 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"])) # need to set bounds for the values here eventually so it doesn't crash when the desired angle is over 90 or under -90 pan = pantilthat.get_pan() + (center[0] - 300) / 50 tilt = pantilthat.get_tilt() - (center[1] - 240) / 50 # if past the tick, send message to other pis so they can start tracking too if (pan > 50): msg = '1,' + str(tilt) print(msg) s.send(msg.encode('utf-8')) past_range = True elif (pan < -50): msg = '-1,' + str(tilt) print(msg) s.send(msg.encode('utf-8')) past_range = True else: if (past_range): print("came back to range") msg = '2' print(msg) s.send(msg.encode('utf-8')) #within_range = False past_range = False if (pan > 90): pantilthat.pan(90) elif (pan < -90): pantilthat.pan(-90) else: pantilthat.pan(pan) if (tilt > 90): pantilthat.tilt(90) elif (tilt < -90): pantilthat.tilt(-90) else: pantilthat.tilt(tilt) # pantilthat.pan(pantilthat.get_pan() + (center[0] - 300) / 50) # pantilthat.tilt(pantilthat.get_tilt() - (center[1] - 240) / 50) else: # if( found = False # update the points queue pts.appendleft(center)
def index(): response.headers['Access-Control-Allow-Origin'] = '*' response.headers[ 'Access-Control-Allow-Methods'] = 'PUT, GET, POST, DELETE, OPTIONS' response.headers[ 'Access-Control-Allow-Headers'] = 'Origin, Accept, Content-Type, X-Requested-With, X-CSRF-Token' command = "none" if request.method == 'POST': command = request.POST['command'] print command arduino_command = None if (command): # used by the software mostly if (command == "gone"): t = pantilthat.get_tilt() p = pantilthat.get_pan() smooth_tilt(t, 80) smooth_pan(t, 0) if (command == "arrived"): t = pantilthat.get_tilt() p = pantilthat.get_pan() smooth_tilt(t, 0) smooth_pan(t, 0) # used by the end user / frontend if (command == "hello"): t = pantilthat.get_tilt() smooth_tilt(t, 40, 0.01) smooth_tilt(40, -70, 0.01) smooth_tilt(-70, 0, 0.01) if (command == "leaving"): t = pantilthat.get_tilt() smooth_tilt(t, 90) if (command == "left"): p = pantilthat.get_pan() smooth_pan(p, 60) if (command == "right"): p = pantilthat.get_pan() smooth_pan(p, -60) if (command == "left_a_bit"): p = pantilthat.get_pan() smooth_pan(p, p + 10) if (command == "right_a_bit"): p = pantilthat.get_pan() smooth_pan(p, p - 10) if (command == "down_a_bit"): t = pantilthat.get_tilt() print("t - down a bit", t) smooth_tilt(t, t + 10) if (command == "up_a_bit"): t = pantilthat.get_tilt() smooth_tilt(t, t - 10) if (command == "halt"): t = pantilthat.get_tilt() p = pantilthat.get_pan() smooth_tilt(t, 80) smooth_pan(p, 0) # sleep time.sleep(5) result = os.system("sudo halt") if (command == "reboot"): t = pantilthat.get_tilt() p = pantilthat.get_pan() smooth_tilt(t, 80) smooth_pan(p, 0) time.sleep(5) result = os.system("sudo reboot") response.set_header('Access-Control-Allow-Origin', '*') result = "ok: " + command return result
def analyze(message, s): print('\x1b[4;33;40m' + message + '\x1b[0m') messages = message.rstrip().split(',') try: int(messages[0]) int(messages[1]) except: # print("unreadable") return rpi_number = int(messages[0]) if (int(messages[1]) == 2): print("reverting back to original position") pan = pantilthat.get_pan() tilt = pantilthat.get_tilt() while (abs(pan) > 5 and abs(tilt) > 5): pan = pantilthat.get_pan() tilt = pantilthat.get_tilt() if (pan > 0): pantilthat.pan(pan - 1) else: pantilthat.pan(pan + 1) if (tilt > 0): pantilthat.tilt(tilt - 1) else: pantilthat.tilt(tilt + 1) time.sleep(0.02) pantilthat.pan(0) pantilthat.tilt(0) return file = open('../number.txt', 'r') number = int(file.read()) file.close() global recieved_y_axis global pan_running try: float(messages[2]) except: return direction = int(messages[1]) y_axis = float(messages[2]) # checks each kind of message that could be delivered # first, checks if message was sent for this pi by seeing if the ball is coming towards it if (rpi_number + direction == number): recieved_y_axis = y_axis if (not pan_running): thread = threading.Thread(target=pan_till_detected, args=( direction, s, )) thread.start()
def move_right(): print("moved right") pantilthat.pan(pantilthat.get_pan()-3)
import pantilthat import math import time # Set the maximum high pulse for a servo in microseconds. # pantilthat.servo_pulse_max(index, 100) # set position of servo one in degrees pantilthat.servo_one(90) # set position of servo two in degrees pantilthat.servo_two(-10) # get position of servo one pantilthat.get_pan() # class pantilthat.PanTilt(enable_lights=True, idle_timeout=2, light_mode=1, light_type=0, servo1_min=575, servo1_max=2325, servo2_min=575, servo2_max=2325, address=21, i2c_bus=None) # -------- #!/usr/bin/env python l = [0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60] # for i in range(len(l)): # pantilthat.tilt(i) # ---- lo = 0 for i in range(len(l)): while (True): pantilthat.tilt(lo) lo += 5 if lo + l[i] > 115: break