Exemplo n.º 1
0
 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
Exemplo n.º 2
0
 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)))
Exemplo n.º 3
0
 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)
Exemplo n.º 4
0
 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)
Exemplo n.º 6
0
 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
Exemplo n.º 9
0
 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
Exemplo n.º 11
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