def getBoxDist(self, boxCoords, absoluteValue=True): boxDistList = [] #top, right, bottom, left TL2Boat = standardcalc.distBetweenTwoCoords(gVars.currentData.gps_coord, boxCoords[0]) #top left to boat TR2Boat = standardcalc.distBetweenTwoCoords(gVars.currentData.gps_coord, boxCoords[1]) #top right to boat BR2Boat = standardcalc.distBetweenTwoCoords(gVars.currentData.gps_coord, boxCoords[2]) #bottom right to boat BL2Boat = standardcalc.distBetweenTwoCoords(gVars.currentData.gps_coord, boxCoords[3]) #bottom left to boat TL2TR = standardcalc.distBetweenTwoCoords(boxCoords[0], boxCoords[1]) #top left to top right TR2BR = standardcalc.distBetweenTwoCoords(boxCoords[1], boxCoords[2]) #top right to bottom right BR2BL = standardcalc.distBetweenTwoCoords(boxCoords[2], boxCoords[3]) #bottom right to bottom left BL2TL = standardcalc.distBetweenTwoCoords(boxCoords[3], boxCoords[0]) #bottom left to top left topLeftAngle = standardcalc.findCosLawAngle(TL2TR, TL2Boat, TR2Boat) topRightAngle = standardcalc.findCosLawAngle(TR2BR, TR2Boat, BR2Boat) botRightAngle = standardcalc.findCosLawAngle(BR2BL, BR2Boat, BL2Boat) botLeftAngle = standardcalc.findCosLawAngle(BL2TL, BL2Boat, TL2Boat) topDist = TL2Boat * math.sin(topLeftAngle) rightDist = TR2Boat * math.sin(topRightAngle) bottomDist = BR2Boat * math.sin(botRightAngle) leftDist = BL2Boat * math.sin(botLeftAngle) if absoluteValue: boxDistList = [abs(topDist), abs(rightDist), abs(bottomDist), abs(leftDist)] else: boxDistList = [topDist, rightDist, bottomDist, leftDist] return boxDistList
def printNavigationLog(self): while (gVars.kill_flagNav == 0): if (time.time() - self.nav_log_timer > self.LOG_UPDATE_INTERVAL): self.nav_log_timer = time.time() gVars.logger.info("Distance: " + str( standardcalc.distBetweenTwoCoords( gVars.currentData.gps_coord, self.interpolatedPoint)))
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 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 testDist1(self): distance = standardcalc.distBetweenTwoCoords(self.point1, self.point2) self.assertEqual(round(distance/1000,1), 157.4)
def printNavigationLog(self): while (gVars.kill_flagNav == 0): if (time.time() - self.nav_log_timer>self.LOG_UPDATE_INTERVAL): self.nav_log_timer = time.time() gVars.logger.info("Distance: "+ str(standardcalc.distBetweenTwoCoords(gVars.currentData.gps_coord, self.interpolatedPoint)))
def getOuterBoundaries(self, boundaries): boundaryList = [] for boundary in boundaries: if(standardcalc.distBetweenTwoCoords(boundary.coordinate, gVars.currentData.gps_coord) > boundary.radius): boundaryList.append(boundary) return boundaryList
def checkOuterBoundaryInterception(self): for boundary in self.outerBoundaries: if(standardcalc.distBetweenTwoCoords(boundary.coordinate, gVars.currentData.gps_coord) <= boundary.radius): return True return False
def testDist1(self): distance = standardcalc.distBetweenTwoCoords(self.point1, self.point2) self.assertEqual(round(distance/1000,1), 157.4)
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 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