def isInside(self, border, target):
degree = 0
for i in range(len(border) - 1):
a = border[i]
b = border[i + 1]
# calculate distance of vector
A = getDistance(a[0], a[1], b[0], b[1]);
B = getDistance(target[0], target[1], a[0], a[1])
C = getDistance(target[0], target[1], b[0], b[1])
# calculate direction of vector
ta_x = a[0] - target[0]
ta_y = a[1] - target[1]
tb_x = b[0] - target[0]
tb_y = b[1] - target[1]
cross = tb_y * ta_x - tb_x * ta_y
clockwise = cross < 0
# calculate sum of angles
if(clockwise):
degree = degree + math.degrees(math.acos((B * B + C * C - A * A) / (2.0 * B * C)))
else:
degree = degree - math.degrees(math.acos((B * B + C * C - A * A) / (2.0 * B * C)))
if(abs(round(degree) - 360) <= 3):
return True
return False
def cb_coordinates(latitude, ns, longitude, ew, pdop, hdop, vdop, epe):
fix, NULL, NULL = gps.get_status()
course, speed = gps.get_motion()
if fix == 3 or fix == 2:
latitude = latitude/1000000.0
longitude = longitude/1000000.0
if ns == "S":
latitude = 0 - latitude
if ew == "W":
longitude = 0 - longitude
distance = getDistance(latitude, longitude, point1[0], point1[1])
bearing = getBearing(latitude, longitude, point1[0], point1[1])
bearing = (bearing + 360) % 360
#course = (course - 180)
course = course/100.0
speed = speed/100.0
bcdiff = (bearing - course)
#print('Latitude: ' + str(latitude) + '° ' + ns)
#print('Longitude: ' + str(longitude) + '° ' + ew)
#print('Course: ' + str(course) + ' speed: ' + str(speed) )
print("Distace: " + str(distance) )
print("Course: " + str(course))
print("Bearing " + str(bearing) )
print("bc diff: " + str(bcdiff))
#return distance, bearing, course, speed, bcdiff
if distance > 10.0:
servo.set_position(steeringsrv, bcdiff * 10)
servo.set_position(motor, speed)
else:
servo.set_position(steeringsrv, mid)
servo.set_position(motor, stop)
else:
print('STOP! ' + str(fix) + " " + str(type(fix)))
servo.set_position(steeringsrv, mid)
servo.set_position(motor, stop)
