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)