def setUp(self):
self.source1 = datatypes.GPSCoordinate(100,100)
self.dest1 = datatypes.GPSCoordinate(300,100)
self.angle1 = standardcalc.angleBetweenTwoCoords(self.source1, self.dest1)
self.angle2 = standardcalc.angleBetweenTwoCoords(self.dest1, self.source1)
self.source2 = datatypes.GPSCoordinate(0,0)
self.dest2 = datatypes.GPSCoordinate(1,1)
self.angle3 = standardcalc.angleBetweenTwoCoords(self.source2, self.dest2)
self.angle4 = standardcalc.angleBetweenTwoCoords(self.dest2, self.source2)
def setUp(self):
self.source1 = datatypes.GPSCoordinate(100,100)
self.dest1 = datatypes.GPSCoordinate(300,100)
self.angle1 = standardcalc.angleBetweenTwoCoords(self.source1, self.dest1)
self.angle2 = standardcalc.angleBetweenTwoCoords(self.dest1, self.source1)
self.source2 = datatypes.GPSCoordinate(0,0)
self.dest2 = datatypes.GPSCoordinate(1,1)
self.angle3 = standardcalc.angleBetweenTwoCoords(self.source2, self.dest2)
self.angle4 = standardcalc.angleBetweenTwoCoords(self.dest2, self.source2)
def canLayMarkWithoutTack(self):
if standardcalc.isWPNoGoAWA(self.AWA, self.hog, self.Dest,self.sog,self.GPSCoord):
return False
else:
windDirection = standardcalc.boundTo180(self.AWA + self.hog)
bearing = standardcalc.angleBetweenTwoCoords(self.GPSCoord,self.Dest)
return not standardcalc.isAngleBetween(bearing,windDirection,self.hog)
def roundBuoyStbd(BuoyLoc, FinalBearing):
currentData = gVars.currentData
GPSCoord = currentData[gps_index]
appWindAng = currentData[awa_index]
cog = currentData[cog_index] # Course over ground
hog = currentData[hog_index] # Height over ground
X = 16.64 # Degrees, Calculated
Dest = 23.41 # Meters, Distance from boat to buoy
angleToNorth = standardcalc.angleBetweenTwoCoords(GPSCoord, BuoyLoc)
reflectLong = GPSCoord.long * -1 # Used for calculation ONLY, because longitude decreases from left to right
if reflectLong > BuoyLoc.long and GPSCoord.lat > BuoyLoc.lat:
moveLong = abs(math.cos(180 - angleToNorth + X)) * -1 # - X movement
moveLat = abs(math.sin(180 - angleToNorth + X)) * - 1 # - Y movement
elif reflectLong < BuoyLoc.long and GPSCoord.lat > BuoyLoc.lat:
moveLong = abs(math.sin(angleToNorth -90 - X)) # + X Movement
moveLat = abs(math.cos(angleToNorth - 90 - X)) * -1 # - Y Movement
elif reflectLong < BuoyLoc.long and GPSCoord.lat < BuoyLoc.lat:
moveLong = abs(math.cos(angleToNorth - X)) # + X Movement
moveLat = abs(math.sin(angleToNorth - X)) # + Y Movement
else:
moveLong = abs(math.sin(angleToNorth - X)) * - 1 # - X Movement
moveLat = abs(math.cos(angleToNorth - X)) # + Y Movement
moveLong *= Dest
moveLat *= Dest
moveLong *= -1 # Convert back to actual coordinates
pointToPoint(datatypes.GPSCoordinate(moveLat, moveLong),1)
return 0
def setUp(self):
gVars.logger = sailbot_logger.Logger()
self.round_buoy = roundbuoy.RoundBuoy()
gVars.currentData = datatypes.ArduinoData(
0, 0, 0, 0, datatypes.GPSCoordinate(0, 0), 0, 0, 0, 0, 0)
self.buoyLoc = datatypes.GPSCoordinate(1, 1)
self.angleBetweenBuoyAndGPSCoord = standardcalc.angleBetweenTwoCoords(
gVars.currentData.gps_coord, self.buoyLoc)
def testFindLeftBuoyPoint(self):
leftBuoyPoint = roundbuoy.RoundBuoy.findLeftBuoyPoint(
self.round_buoy, self.buoyLoc)
self.assertTrue(
abs(
standardcalc.angleBetweenTwoCoords(self.buoyLoc, leftBuoyPoint)
- (self.angleBetweenBuoyAndGPSCoord -
roundbuoy.RoundBuoy.TargetAndBuoyAngle)) < 0.1)
def testFindLeftBuoyPoint(self):
leftBuoyPoint = roundbuoy.RoundBuoy.findLeftBuoyPoint(self.round_buoy, self.buoyLoc)
self.assertTrue(
abs(
standardcalc.angleBetweenTwoCoords(self.buoyLoc, leftBuoyPoint)
- (self.angleBetweenBuoyAndGPSCoord - roundbuoy.RoundBuoy.TargetAndBuoyAngle)
)
< 0.1
)
def updateData(self):
self.GPSCoord = gVars.currentData.gps_coord
self.distanceToWaypoint = standardcalc.distBetweenTwoCoords(self.GPSCoord, self.Dest)
self.AWA = gVars.currentData.awa
self.cog = gVars.currentData.cog
self.hog = gVars.currentData.hog
self.sog = gVars.currentData.sog * 100
self.angleBetweenCoords = standardcalc.angleBetweenTwoCoords(self.GPSCoord,self.Dest)
standardcalc.updateWeatherSetting(self.AWA,self.sog)
def testReadyToTackTrue(self):
AWA = 30
GPSCoord = datatypes.GPSCoordinate(49, -123)
Dest = datatypes.GPSCoordinate(49.1, -123) # 0 degrees, N
bearingToMark = standardcalc.angleBetweenTwoCoords(GPSCoord, Dest)
hog = 90 # E
self.assertTrue(self.tackengine.readyToTack(AWA, hog, bearingToMark))
hog = -90 # NW
self.assertTrue(self.tackengine.readyToTack(AWA, hog, bearingToMark))
def testReadyToTackTrue(self):
AWA =30
GPSCoord = datatypes.GPSCoordinate(49,-123)
Dest = datatypes.GPSCoordinate(49.1,-123) # 0 degrees, N
bearingToMark = standardcalc.angleBetweenTwoCoords(GPSCoord, Dest)
hog = 90 # E
self.assertTrue(self.tackengine.readyToTack(AWA, hog, bearingToMark))
hog = -90 # NW
self.assertTrue(self.tackengine.readyToTack(AWA, hog, bearingToMark))
def findLeftBuoyPoint(self, BuoyLoc):
angleOfCourse = standardcalc.angleBetweenTwoCoords(gVars.currentData.gps_coord, BuoyLoc)
angleFromEast = 90-angleOfCourse
LongitudalDistBetweenBuoyAndTarget = self.TargetToBuoyDist*math.cos(math.radians(angleFromEast+self.TargetAndBuoyAngle))
LatitudalDistBetweenBuoyAndTarget = self.TargetToBuoyDist*math.sin(math.radians(angleFromEast+self.TargetAndBuoyAngle))
leftPoint = standardcalc.GPSDistAway(BuoyLoc, LongitudalDistBetweenBuoyAndTarget, LatitudalDistBetweenBuoyAndTarget)
return leftPoint
def canLayMarkWithoutTack(self):
if standardcalc.isWPNoGoAWA(self.AWA, self.hog, self.Dest, self.sog,
self.GPSCoord):
return False
else:
windDirection = standardcalc.boundTo180(self.AWA + self.hog)
bearing = standardcalc.angleBetweenTwoCoords(
self.GPSCoord, self.Dest)
return not standardcalc.isAngleBetween(bearing, windDirection,
self.hog)
def updateData(self):
self.GPSCoord = gVars.currentData.gps_coord
self.distanceToWaypoint = standardcalc.distBetweenTwoCoords(
self.GPSCoord, self.Dest)
self.AWA = gVars.currentData.awa
self.cog = gVars.currentData.cog
self.hog = gVars.currentData.hog
self.sog = gVars.currentData.sog * 100
self.angleBetweenCoords = standardcalc.angleBetweenTwoCoords(
self.GPSCoord, self.Dest)
standardcalc.updateWeatherSetting(self.AWA, self.sog)
def calculateHeadingForExit(self):
if standardcalc.isWPNoGoAWA(gVars.currentData.awa, gVars.currentData.hog, self.wayPtCoords[self.currentWaypoint],gVars.currentData.sog,self.wayPtCoords[(self.currentWaypoint+2)%4]):
self.STEER_METHOD = self.AWA_METHOD
if gVars.currentData.awa<0:
tackAngleMultiplier = -1
else:
tackAngleMultiplier = 1
return tackAngleMultiplier*34
else:
self.STEER_METHOD = self.COMPASS_METHOD
return standardcalc.angleBetweenTwoCoords(self.wayPtCoords[(self.currentWaypoint+2)%4], self.wayPtCoords[self.currentWaypoint])
def run(self, BuoyLoc):
gVars.kill_flagRB = 0
gVars.logger.info("Rounding to " + self.rounding)
if(self.rounding=="port"):
point = self.findRightBuoyPoint(BuoyLoc)
else:
point = self.findLeftBuoyPoint(BuoyLoc)
gVars.logger.info("Sailing To Rounding Point: " + repr(point))
if(gVars.kill_flagRB == 0):
edgeBearing = standardcalc.angleBetweenTwoCoords(gVars.currentData.gps_coord, point)
chaseRaceTackEngine = chaseracetackengine.ChaseRaceTackEngine(self.rounding,edgeBearing)
self.pointtopoint.withTackEngine(chaseRaceTackEngine).run(point,6)
gVars.logger.info("Finished RoundBuoy")
def enterBeatLoop(self, port):
if port:
self.p2pLogger.printTacking("On port tack")
tackAngleMultiplier = -1
else:
self.p2pLogger.printTacking("On starboard tack")
tackAngleMultiplier = 1
while gVars.kill_flagPTP ==0 and not (self.arrivedAtPoint() or self.canLayMarkWithoutTack() ):
self.updateData()
bearingToMark = standardcalc.angleBetweenTwoCoords(self.GPSCoord, self.Dest)
if self.tackEngine.readyToTack(self.AWA, self.hog, bearingToMark) or self.boundaryHandler.hitBoundary():
self.sailor.tack(self.tackEngine.getTackDirection(self.AWA))
break
if self.isThereChangeToAWAorWeatherOrMode():
self.sailor.adjustSheetsAndSteerByApparentWind(tackAngleMultiplier, self.AWA)
time.sleep(.1)
def enterBeatLoop(self, port):
if port:
self.p2pLogger.printTacking("On port tack")
tackAngleMultiplier = -1
else:
self.p2pLogger.printTacking("On starboard tack")
tackAngleMultiplier = 1
while gVars.kill_flagPTP == 0 and not (self.arrivedAtPoint() or
self.canLayMarkWithoutTack()):
self.updateData()
bearingToMark = standardcalc.angleBetweenTwoCoords(
self.GPSCoord, self.Dest)
if self.tackEngine.readyToTack(
self.AWA, self.hog,
bearingToMark) or self.boundaryHandler.hitBoundary():
self.sailor.tack(self.tackEngine.getTackDirection(self.AWA))
break
if self.isThereChangeToAWAorWeatherOrMode():
self.sailor.adjustSheetsAndSteerByApparentWind(
tackAngleMultiplier, self.AWA)
time.sleep(.1)
def setNavigationBuoyPoint(buoyLocation, boatCoords, distFromBuoy):
interpoAngle = 90 - standardcalc.angleBetweenTwoCoords(buoyLocation, boatCoords)
xDelta = distFromBuoy*math.cos(interpoAngle)
yDelta = distFromBuoy*math.sin(interpoAngle)
return standardcalc.GPSDistAway(buoyLocation, xDelta, yDelta)
def pointToPointTWA(Dest, initialTack, ACCEPTANCE_DISTANCE):
sheetList = parsing.parse(path.join(path.dirname(__file__), 'apparentSheetSetting'))
gVars.kill_flagPTP = 0
end_flag = 0
arduino = gVars.arduino
TWA = 0
oldColumn = 0
tackDirection = 0
print "Started point to point"
gVars.logger.info("Started point to pointTWA")
while(end_flag == 0 and gVars.kill_flagPTP == 0):
while(gVars.currentData[aut_index] == False):
time.sleep(0.1)
currentData = gVars.currentData
GPSCoord = currentData[gps_index]
appWindAng = currentData[awa_index]
cog = currentData[cog_index]
hog = currentData[hog_index]
sog = currentData[sog_index] * 100
if(standardcalc.distBetweenTwoCoords(GPSCoord, Dest) > ACCEPTANCE_DISTANCE):
#This if statement determines the sailing method we are going to use based on apparent wind angle
if(sog < sVars.SPEED_AFFECTION_THRESHOLD):
newTWA = appWindAng
newTWA = abs(int(newTWA))
if(appWindAng < 0):
gVars.TrueWindAngle = -newTWA
else:
gVars.TrueWindAngle = newTWA
gVars.currentColumn = 0
#print ("TWA is: " + str(gVars.TrueWindAngle))
else:
newTWA = standardcalc.getTrueWindAngle(abs(int(appWindAng)),sog)
newTWA = abs(int(newTWA))
if(appWindAng < 0):
gVars.TrueWindAngle = -newTWA
else:
gVars.TrueWindAngle = newTWA
#print ("Hit else statement")
#print ("TWA is: " + str(gVars.TrueWindAngle))
if(standardcalc.isWPNoGo(appWindAng,hog,Dest,sog,GPSCoord)):
gVars.logger.info("Cannot reach point directly")
#Trying to determine whether 45 degrees clockwise or counter clockwise of TWA wrt North is closer to current heading
#This means we are trying to determine whether hog-TWA-45 or hog-TWA+45 (both using TWA wrt North) is closer to our current heading.
#Since those values give us TWA wrt to north, we need to subtract hog from them to get TWA wrt to our heading and figure out which one has a smaller value.
#To get it wrt to current heading, we use hog-TWA-45-hog and hog-TWA+45-hog. Both terms have hogs cancelling out.
#We are left with -TWA-45 and -TWA+45, which makes sense since the original TWA was always with respect to the boat.
#Since we are trying to figure out which one is closest to turn to, we use absolute values.
if((abs(-newTWA-45)<abs(-newTWA+45) and initialTack is None) or initialTack == 1):
initialTack = None
while(abs(hog-standardcalc.angleBetweenTwoCoords(GPSCoord, Dest))<80 and gVars.kill_flagPTP ==0):
gVars.logger.info("On starboard tack")
while(gVars.currentData[aut_index] == False):
time.sleep(0.1)
gVars.tacked_flag = 0
GPSCoord = currentData[gps_index]
appWindAng = currentData[awa_index]
cog = currentData[cog_index]
hog = currentData[hog_index]
sog = currentData[sog_index] * 100
if(sog > sVars.SOG_THRESHOLD):
newTWA = standardcalc.getTrueWindAngle(appWindAng, sog)
else:
newTWA = appWindAng
newTWA = abs(int(newTWA))
if( TWA != newTWA or oldColumn != gVars.currentColumn):
gVars.logger.info("Changing sheets and rudder")
arduino.adjust_sheets(sheetList[newTWA][gVars.currentColumn])
arduino.steer(AWA_METHOD,hog-newTWA-45)
TWA = newTWA
oldColumn = gVars.currentColumn
if(appWindAng > 0):
tackDirection = 1
else:
tackDirection = 0
if( len(gVars.boundaries) > 0 ):
for boundary in gVars.boundaries:
if(standardcalc.distBetweenTwoCoords(boundary, GPSCoord) <= boundary.radius):
arduino.tack(gVars.currentColumn,tackDirection)
gVars.logger.info("Tacking from boundary")
gVars.tacked_flag = 1
break
if(gVars.tacked_flag):
break
arduino.tack(gVars.currentColumn,tackDirection)
gVars.logger.info("Tacking from 80 degrees")
elif((abs(-newTWA-45)>=abs(-newTWA+45) and initialTack is None) or initialTack == 0):
initialTack = None
while(abs(hog-standardcalc.angleBetweenTwoCoords(GPSCoord, Dest))<80 and gVars.kill_flagPTP == 0):
gVars.logger.info("On port tack")
while(gVars.currentData[aut_index] == False):
time.sleep(0.1)
gVars.tacked_flag = 0
GPSCoord = currentData[gps_index]
appWindAng = currentData[awa_index]
cog = currentData[cog_index]
hog = currentData[hog_index]
sog = currentData[sog_index]*100
if(sog > sVars.SOG_THRESHOLD):
newTWA = standardcalc.getTrueWindAngle(appWindAng, sog)
else:
newTWA = appWindAng
newTWA = abs(int(newTWA))
#TWA = abs(int(TWA))
#print ("TWA is: " + str(newTWA))
if(TWA != newTWA or oldColumn != gVars.currentColumn):
gVars.logger.info("Changing sheets and rudder")
arduino.adjust_sheets(sheetList[int(abs(newTWA))][gVars.currentColumn])
arduino.steer(AWA_METHOD,hog-newTWA+45)
TWA = newTWA
oldColumn = gVars.currentColumn
if(appWindAng > 0):
tackDirection = 1
else:
tackDirection = 0
if( len(gVars.boundaries) > 0 ):
for boundary in gVars.boundaries:
if(standardcalc.distBetweenTwoCoords(boundary, GPSCoord) <= boundary.radius):
gVars.logger.info("Tacking from boundary")
arduino.tack(gVars.currentColumn,tackDirection)
gVars.tacked_flag = 1
break
if(gVars.tacked_flag):
break
arduino.tack(gVars.currentColumn,tackDirection)
gVars.logger.info("Tacking from 80 degrees")
elif(abs(hog-newTWA-standardcalc.angleBetweenTwoCoords(GPSCoord, Dest))>90):
gVars.logger.info("Sailing straight to point")
if(TWA != newTWA or oldColumn != gVars.currentColumn):
gVars.logger.info("Adjusting sheets and rudder")
arduino.adjust_sheets(sheetList[abs(int(newTWA))][gVars.currentColumn])
arduino.steer(COMPASS_METHOD,standardcalc.angleBetweenTwoCoords(GPSCoord,Dest))
TWA = newTWA
gVars.currentColumn
else:
end_flag = 1
print ("Finished Point to Point")
gVars.logger.info("Finished Point to Point")
return 0
def setUp(self):
gVars.logger = sailbot_logger.Logger()
self.round_buoy = roundbuoy.RoundBuoy()
gVars.currentData = datatypes.ArduinoData(0, 0, 0, 0, datatypes.GPSCoordinate(0, 0), 0, 0, 0, 0, 0)
self.buoyLoc = datatypes.GPSCoordinate(1, 1)
self.angleBetweenBuoyAndGPSCoord = standardcalc.angleBetweenTwoCoords(gVars.currentData.gps_coord, self.buoyLoc)
def roundBuoyPort(BuoyLoc, FinalBearing):
currentData = gVars.currentData
GPSCoord = currentData[gps_index]
# appWindAng = currentData[awa_index]
InitCog = currentData[cog_index] # Course over ground
InitHog = currentData[hog_index] # Heading over ground
X = 16.64 # Degrees, Calculated
Dest = 23.41 # Meters, Distance from boat to buoy
angleToNorth = standardcalc.angleBetweenTwoCoords(GPSCoord, BuoyLoc)
reflectLong = GPSCoord.long * -1 # Used for calculation ONLY, because longitude decreases from left to right
quadDir = None
if reflectLong > BuoyLoc.long and GPSCoord.lat > BuoyLoc.lat:
moveLong = abs(math.sin(180 - angleToNorth + X)) * -1 # - X movement
moveLat = abs(math.cos(180 - angleToNorth + X)) * - 1 # - Y movement
quadDir = 3;
elif reflectLong < BuoyLoc.long and GPSCoord.lat > BuoyLoc.lat:
moveLong = abs(math.cos(angleToNorth -90 - X)) # + X Movement
moveLat = abs(math.sin(angleToNorth - 90 - X)) * -1 # - Y Movement
quadDir = 4;
elif reflectLong < BuoyLoc.long and GPSCoord.lat < BuoyLoc.lat:
moveLong = abs(math.sin(angleToNorth - X)) # + X Movement
moveLat = abs(math.cos(angleToNorth - X)) # + Y Movement
quadDir = 1;
else:
moveLong = abs(math.sin(angleToNorth + X)) * - 1 # - X Movement
moveLat = abs(math.cos(angleToNorth + X)) # + Y Movement
quadDir = 2;
moveLong *= Dest
moveLat *= Dest
moveLong *= -1 # Convert back actual coordinates
destination = standardcalc.GPSDistAway(GPSCoord, moveLong, moveLat)
# 10 represents the degree of error around the destination point
# Calls point to point function until it reaches location past buoy
# Adding 10 does not increase the radius by 10 meters(ERROR!) - fixed
if (GPSCoord.long >= standardcalc.GPSDistAway(destination, 10, 0).long):# or GPSCoord.long <= standardcalc.GPSDistAway(destination, -10, 0).long) and (GPSCoord.lat >= standardcalc.GPSDistAway(destination, 0, 10).lat or GPSCoord.lat <= standardcalc.GPSDistAway(destination, 0, -10).lat):
pointToPoint(datatypes.GPSCoordinate(destination.lat, destination.long),1)
GPSCoord.long = gVars.currentData[gps_index].long
GPSCoord.lat = gVars.currentData[gps_index].lat
# Checks if the boat needs to round the buoy or just pass it
vect = datatypes.GPSCoordinate()
vect.lat = BuoyLoc.lat - currentData[gps_index].lat
vect.long = BuoyLoc.long - currentData[gps_index].long
# Checks if the boat as to round the buoy
buoyAngle = None
buoyAngle = standardcalc.vectorToDegrees(vect.lat, vect.long)
buoyAngle -= 90
buoyAngle = standardcalc.boundTo180(buoyAngle) #git later
# Incomplete, not static values, need to use trig to determine new gps locations
if FinalBearing < buoyAngle and FinalBearing > (buoyAngle - 90):
if reflectLong > BuoyLoc.long and GPSCoord.lat > BuoyLoc.lat:
moveLong2 = abs(math.cos(180 - angleToNorth + X)) * -1 # - X movement
moveLat2 = abs(math.sin(180 - angleToNorth + X)) * - 1 # - Y movement
elif reflectLong < BuoyLoc.long and GPSCoord.lat > BuoyLoc.lat:
moveLong2 = abs(math.sin(angleToNorth -90 - X)) # + X Movement
moveLat2 = abs(math.cos(angleToNorth - 90 - X)) * -1 # - Y Movement
elif reflectLong < BuoyLoc.long and GPSCoord.lat < BuoyLoc.lat:
moveLong2 = abs(math.cos(angleToNorth - X)) # + X Movement
moveLat2 = abs(math.sin(angleToNorth - X)) # + Y Movement
else:
moveLong2 = abs(math.sin(angleToNorth - X)) * - 1 # - X Movement
moveLat2 = abs(math.cos(angleToNorth - X)) # + Y Movement
destination = standardcalc.GPSDistAway(GPSCoord, moveLong2, moveLat2)
# 10 represents the degree of error around the destination point
# Calls point to point function until it reaches location past buoy
# Adding 10 does not increase the radius by 10 meters(ERROR!) - fixed
if (GPSCoord.long >= standardcalc.GPSDistAway(destination, 10, 0).long or GPSCoord.long <= standardcalc.GPSDistAway(destination, -10, 0).long) and (GPSCoord.lat >= standardcalc.GPSDistAway(destination, 0, 10).lat or GPSCoord.lat <= standardcalc.GPSDistAway(destination, 0, -10).lat):
pointToPoint(datatypes.GPSCoordinate(destination.lat, destination.long),1)
GPSCoord.long = gVars.currentData[gps_index].long
GPSCoord.lat = gVars.currentData[gps_index].lat
return 0
def run(Waypoint1,Waypoint2,Waypoint3):
currentData = gVars.currentData
GPSCoord = currentData[gps_index]
gVars.kill_flagNav = 0
num_nav_first = 0
num_nav_start_port = 0
num_nav_start_stbd = 0
wayList = list()
wayList.append(Waypoint1)
wayList.append(Waypoint2)
wayList.append(Waypoint3)
for waypoint in wayList:
if(waypoint.wtype == "nav_first"):
BuoyCoords = waypoint.coordinate
num_nav_first = num_nav_first + 1
elif(waypoint.wtype == "nav_start_port"):
PortStartInnerPoint = waypoint.coordinate
num_nav_start_port = num_nav_start_port + 1
elif(waypoint.wtype == "nav_start_stbd"):
StarboardStartInnerPoint = waypoint.coordinate
num_nav_start_stbd = num_nav_start_stbd + 1
if(num_nav_start_port > 1 or num_nav_start_stbd > 1 or num_nav_first > 1):
gVars.logger.error("Repeating or too many arguments")
interpolatedPoint = datatypes.GPSCoordinate((PortStartInnerPoint.latitude+StarboardStartInnerPoint.latitude)/2,(PortStartInnerPoint.longitude+StarboardStartInnerPoint.longitude)/2)
angleOfCourse = standardcalc.angleBetweenTwoCoords(interpolatedPoint, BuoyCoords)
boundAngle = math.atan(HORIZ_BOUNDARY_DISTANCE/30)*180/math.pi
bound_dist = math.sqrt(HORIZ_BOUNDARY_DISTANCE^2+30^2)
netAngleLeft = boundAngle - angleOfCourse
netAngleRight = boundAngle + angleOfCourse
leftBoundaryPoint = standardcalc.GPSDistAway(StarboardStartInnerPoint, bound_dist*math.sin(netAngleLeft), bound_dist*math.cos(netAngleLeft))
rightBoundaryPoint = standardcalc.GPSDistAway(PortStartInnerPoint, bound_dist*math.sin(netAngleRight), bound_dist*math.cos(netAngleRight))
buoySailPoint = setNavigationBuoyPoint(BuoyCoords, GPSCoord, 10)
if(gVars.kill_flagNav == 0):
coresailinglogic.pointToPoint(buoySailPoint)
if(gVars.kill_flagNav == 0):
coresailinglogic.roundBuoyPort(BuoyCoords,standardcalc.angleBetweenTwoCoords(BuoyCoords,GPSCoord))
if(gVars.kill_flagNav == 0):
thread.start_new_thread(coresailinglogic.pointToPoint, interpolatedPoint)
while(standardcalc.distBetweenTwoCoords(GPSCoord, interpolatedPoint)>sVars.ACCEPTANCE_DISTANCE_DEFAULT and gVars.kill_flagNav == 0):
GPSCoord = currentData[gps_index]
appWindAng = currentData[awa_index]
while(gVars.currentData[aut_index] == False):
time.sleep(0.1)
if(standardcalc.distBetweenTwoCoords(GPSCoord,leftBoundaryPoint) > bound_dist or standardcalc.distBetweenTwoCoords(GPSCoord,rightBoundaryPoint) > bound_dist):
if(appWindAng > 0):
tackDirection = 1
else:
tackDirection = 0
gVars.arduino.tack(gVars.currentColumn,tackDirection)
gVars.tacked_flag = 1
return 0
