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
