def _updateAWA(self): if STATIC_AWA == None: # Sets the apparent wind angle boat_bearing = self.arduinoData.hog # Reverse direction for boat vector if (boat_bearing >= 0): boat_bearing -= 180 else: boat_bearing += 180 boat_speed = self.arduinoData.sog # Reverse direction for wind vector wind_bearing = self.trueWindAngle if (wind_bearing >= 0): wind_bearing -= 180 else: wind_bearing += 180 wind_speed = self.trueWindSpeed boat_x = boat_speed * math.cos(boat_bearing) boat_y = boat_speed * math.sin(boat_bearing) wind_x = wind_speed * math.cos(wind_bearing) wind_y = wind_speed * math.sin(wind_bearing) x = boat_x + wind_x y = boat_y + wind_y if self.previousx is None: self.previousx = x awa = math.atan(y/x) if (math.copysign(self.previousx, x) != self.previousx or self.flipflag): if (not self.flipflag): self.flipflag = True elif (math.copysign(self.previousx, x) != self.previousx): self.flipflag = False print(str(self.previousx) + ", " + str(x)) if(awa > 0): awa -= math.pi else: awa += math.pi awa = math.degrees(awa) awa -= self.arduinoData.hog awa = standardcalc.boundTo180(awa) self.previousx = x else: awa = standardcalc.boundTo180(standardcalc.boundTo360(STATIC_AWA)-standardcalc.boundTo180(self.arduinoData.hog)) self.arduinoData.awa = standardcalc.boundTo180(awa)
def interpretArr(self, ardArr): arduinoData = datatypes.ArduinoData() arduinoData.hog = standardcalc.boundTo180(ardArr[ARD_HOG]) arduinoData.cog = standardcalc.boundTo180(ardArr[ARD_COG]) arduinoData.sog = ardArr[ARD_SOG] arduinoData.awa = ardArr[ARD_AWAV] arduinoData.gps_coord = datatypes.GPSCoordinate(ardArr[ARD_LAT]/10000000, ardArr[ARD_LONG]/10000000) arduinoData.sheet_percent = ardArr[ARD_SHT] arduinoData.num_sat = ardArr[ARD_SAT] arduinoData.gps_accuracy = ardArr[ARD_ACC] arduinoData.auto = ardArr[ARD_AUT] arduinoData.rudder = ardArr[ARD_RUD] return arduinoData
def interpretArr(self, ardArr): arduinoData = datatypes.ArduinoData() arduinoData.hog = standardcalc.boundTo180(ardArr[ARD_HOG]) arduinoData.cog = standardcalc.boundTo180(ardArr[ARD_COG]) arduinoData.sog = ardArr[ARD_SOG] arduinoData.awa = ardArr[ARD_AWAV] arduinoData.gps_coord = datatypes.GPSCoordinate( ardArr[ARD_LAT] / 10000000, ardArr[ARD_LONG] / 10000000) arduinoData.sheet_percent = ardArr[ARD_SHT] arduinoData.num_sat = ardArr[ARD_SAT] arduinoData.gps_accuracy = ardArr[ARD_ACC] arduinoData.auto = ardArr[ARD_AUT] arduinoData.rudder = ardArr[ARD_RUD] return arduinoData
def gybe(self, x): tempspeed = self.arduinoData.sog self.arduinoData.sog = 0 self.arduinoData.hog = standardcalc.boundTo180(self.arduinoData.hog+180) gVars.logger.info("-------GYBE-----------") time.sleep(4) self.arduinoData.sog = tempspeed
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 _updateTrueWind(self): # Updates true wind angle if (ALLOW_WIND_REVERSAL): self.trueWindAngle += random.uniform(-.2, 0) else: self.trueWindAngle += random.uniform(-.1, .1) self.trueWindAngle = standardcalc.boundTo180(self.trueWindAngle)
def _updateCOG(self): # Sets the course over ground if (math.fabs(self.arduinoData.cog + self.currplusmin - self.arduinoData.hog) < .4): self.arduinoData.cog += round(random.uniform(-.1, .1), 2) elif (self.arduinoData.cog + self.currplusmin < self.arduinoData.hog): self.arduinoData.cog += round(random.uniform(0, 5), 2) elif (self.arduinoData.cog + self.currplusmin > self.arduinoData.hog): self.arduinoData.cog += round(random.uniform(-5, 0), 2) self.arduinoData.cog = standardcalc.boundTo180(self.arduinoData.cog)
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 tack(self, weather, tack): hog = self.arduinoData.hog # Format # Tack: Port=0 Stbd=1 if (tack == 1): hog += 70 else: hog -= 70 hog = standardcalc.boundTo180(hog) gVars.logger.info("--------TACK----------") self.arduinoData.hog = hog self.arduinoData.awa = hog+STATIC_AWA time.sleep(5)
def testGreaterThan180(self): self.assertEqual(standardcalc.boundTo180(self.num2), self.num2bounded)
def testGreaterThan360ToNegative(self): self.assertEqual(standardcalc.boundTo180(self.num5), self.num5bounded)
def testGreaterThan360ToPositive(self): self.assertEqual(standardcalc.boundTo180(self.num4), self.num4bounded)
def testLessThan180(self): self.assertEqual(standardcalc.boundTo180(self.num3), self.num3bounded)
def testNoChange(self): self.assertEqual(standardcalc.boundTo180(self.num1), self.num1bounded)
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 testLessThan360ToNegative(self): self.assertEqual(standardcalc.boundTo180(self.num6), self.num6bounded)
def testLessThan360ToPositive(self): self.assertEqual(standardcalc.boundTo180(self.num7), self.num7bounded)