def element_click_cb(e, name): global main_alive print(name) if (main_alive): if (name == "Environment"): envPage = Environment() envPage.createPage() elif (name == "Hvac"): hvac = Hvac() hvac.createPage() elif (name == "Compass"): compassPage = Compass() compassPage.createPage() elif (name == "Sound"): ttfPage = SoundTTF() ttfPage.createPage() elif (name == "Music"): musicPage = Music() musicPage.createPage() elif (name == "Timer"): timePage = Timer() timePage.createPage() main_alive = False
def __init__(self, motors_object, proximity_manager_object): #Check oggetto classe Motors if motors_object is None: raise Exception('Motors object cannot be null') return #Check istanza classe ProximityManager if proximity_manager_object is None: raise Exception('ProximityManager object cannot be null') return #Reference istanza oggetto classe Motors self.motors_object = motors_object #Reference istanza oggetto classe ProximityManager self.proximity_manager_object = proximity_manager_object #Reference istanza oggetto classe Compass print('Starting Compass..') self.compass_object = Compass() Thread.__init__(self) self.status = self.RUNNING self.name = self.__class__.__name__ self.start()
def findValidWords(grid, rows, cols): valid_words = [] for i in range(len(grid)): for j in range(len(grid[0])): compass = Compass() sequence_matcher = Matcher(grid[i][j]) # for each index in character grid, obtain all the valid words # starting at that index in all 8 directions - left, right, top, down, # and all four diagonals. for TURN in range(8): x, y = i, j # obtain current change in rows(i) and change in columns(j) from # compass (compass automatically updates to the next direction) iChange, jChange = compass.get_curr_index_changes() # compute character sequence in current direction char_sequence = [] while isWithinRange(x, y, rows, cols): char_sequence.append(grid[x][y]) x += iChange y += jChange # compute correct matches correct_matches = sequence_matcher.process_sequence(char_sequence) valid_words.extend(correct_matches) return valid_words
def test_heads_in_right_direction(self): compass = Compass({'lat': 10, 'lng': 10}) curr_dist = compass.get_distance_from_coords(self.user.where_am_i()) self.user.follow_the_compass( compass.get_direction(self.user.where_am_i())) new_dist = compass.get_distance_from_coords(self.user.where_am_i()) self.assertLess(new_dist, curr_dist)
def __init__(self, scope, resources, calibration): #initialise pygame self.compass = Compass(calibration) #get a compass object self.scope = scope #Use a pyscope object so we can run the program from the console self.resources = resources #get the resources class instance self.contactsArray = Contacts() #get a contacts class instance self.text = None self.smallnumber_text = None self.dots = None self.display_scale = None self.closest_text = "" self.smallnumber = ""
def move(self): """ Moves robot one 'field' towards the facing direction. """ if self.is_active: new_x, new_y = Compass.get_new_position(self.position['x'], self.position['y'], self.direction) self._set_new_position(new_x, new_y)
def __init__(self, color, location_str, command_str): self.color = color x, y, heading = location_str.split(' ') self.x_coord = int(x) self.y_coord = int(y) self.compass = Compass(heading) self.commands = command_str
def __init__(self,gps=False,servo_port=SERVO_PORT): # devices self.compass = Compass(I2C(COMPASS_I2C_ADDRESS),I2C(ACCELEROMETER_I2C_ADDRESS)) self.rudder_servo = Servo(serial.Serial(servo_port),RUDDER_SERVO_CHANNEL,RUDDER_MIN_PULSE,RUDDER_MIN_ANGLE,RUDDER_MAX_PULSE,RUDDER_MAX_ANGLE) try: while True: current_bearing = self.compass.bearing difference = angle_between(TARGET_BEARING, current_bearing) # for definite rudder deflection at low angles for difference: # scaled_diff = max(5, abs(difference/3)) # rudder_angle = copysign(scaled_diff,difference) # for tolerance of small differences rudder_angle = difference/3 if abs(difference) > 5 else 0 self.rudder_servo.set_position(rudder_angle) print('Current bearing {:+.1f}, target {:+.1f}, rudder {:+.1f}'.format(current_bearing, TARGET_BEARING, rudder_angle)) # smooth response with a sleep time of less than 100ms ? time.sleep(0.1) except (KeyboardInterrupt, SystemExit): quit()
def __init__(self): pygame.init() self.settings = Settings() self.screen = pygame.display.set_mode(self.settings.screen_size) self.compass = Compass() self.place = Place(self.settings, self.screen, self.compass) self.form = Form(self.settings, self.screen, self.compass) self.functions = Functions(self.settings, self.screen, self.compass, self.place, self.form)
def draw_triangles(surf, directions, color, radius=8, offset=4): off = offset rx = ry = radius w, h = surf.get_size() for direction in directions: is_corner = direction in Compass.ordinals() vx, vy = (0+off if 'W' in direction.name else (w-1-off if 'E' in direction.name else w//2-1), 0+off if 'N' in direction.name else (h-1-off if 'S' in direction.name else h//2-1)) rx = ry = radius * 75//100 if not is_corner else radius dx, dy = Compass.xy(Compass.flip(direction)) dx, dy = int(dx), int(dy) if is_corner: vx += dx * rx vy += dy * ry Ax, Ay = None, None Bx, By = None, None if not dx: # north, south Ax = vx - rx Bx = vx + rx Ay = By = vy + dy * ry elif not dy: # east, west Ay = vy - ry By = vy + ry Ax = Bx = vx + dx * rx else: # diagonals Ax = vx + dx * rx By = vy + dy * ry Ay = vy Bx = vx tri = [(vx, vy), (Ax, Ay), (Bx, By)] pygame.draw.polygon(surf, color, tri)
def __init__(self, gps=False, servo_port=SERVO_PORT): # devices self._gps = gps self.windsensor = WindSensor(I2C(WINDSENSOR_I2C_ADDRESS)) self.compass = Compass(I2C(COMPASS_I2C_ADDRESS), I2C(ACCELEROMETER_I2C_ADDRESS)) self.red_led = GpioWriter(17, os) self.green_led = GpioWriter(18, os) # Navigation self.globe = Globe() self.timer = Timer() self.application_logger = self._rotating_logger(APPLICATION_NAME) self.position_logger = self._rotating_logger("position") self.exchange = Exchange(self.application_logger) self.timeshift = TimeShift(self.exchange, self.timer.time) self.event_source = EventSource(self.exchange, self.timer, self.application_logger, CONFIG['event source']) self.sensors = Sensors(self.gps, self.windsensor, self.compass, self.timer.time, self.exchange, self.position_logger, CONFIG['sensors']) self.gps_console_writer = GpsConsoleWriter(self.gps) self.rudder_servo = Servo(serial.Serial(servo_port), RUDDER_SERVO_CHANNEL, RUDDER_MIN_PULSE, RUDDER_MIN_ANGLE, RUDDER_MAX_PULSE, RUDDER_MAX_ANGLE) self.steerer = Steerer(self.rudder_servo, self.application_logger, CONFIG['steerer']) self.helm = Helm(self.exchange, self.sensors, self.steerer, self.application_logger, CONFIG['helm']) self.course_steerer = CourseSteerer(self.sensors, self.helm, self.timer, CONFIG['course steerer']) self.navigator = Navigator(self.sensors, self.globe, self.exchange, self.application_logger, CONFIG['navigator']) self.self_test = SelfTest(self.red_led, self.green_led, self.timer, self.rudder_servo, RUDDER_MIN_ANGLE, RUDDER_MAX_ANGLE) # Tracking self.tracking_logger = self._rotating_logger("track") self.tracking_sensors = Sensors(self.gps, self.windsensor, self.compass, self.timer.time, self.exchange, self.tracking_logger, CONFIG['sensors']) self.tracker = Tracker(self.tracking_logger, self.tracking_sensors, self.timer)
def init_modules(): right_eye = Eye(list(range(12)), (100, 0, 0)) left_eye = Eye(list(range(12, 24)), (100, 0, 0)) left_eye.eye_time = right_eye.eye_time # Pretty hacky but syncronizes the pulse # gills gills = Gills( [Gill(list(range(24, 69)), color=(0, 0, 20), intensity=1.0)], [Gill(list(range(71, 131)), color=(0, 0, 20), intensity=1.0) ]) # treat all as lefties for now... cannon = Cannon() return { 'gills': gills, 'left_eye': left_eye, 'right_eye': right_eye, 'cannon': cannon, 'compass': Compass() }
def setAngle(self): compass = Compass() sunpos = Sunpos() azimuth = sunpos.get_az_alt()[1] ultimaPosicion = self.getAngulobase() if azimuth <= 90 and azimuth >= 0: #Primer cuadrante (N-E) if ultimaPosicion >= 270 and ultimaPosicion <= 359: self.startingPos(compass) ultimaPosicion = 0 if ultimaPosicion < azimuth: self.moveNegAngles(compass, azimuth) else: self.movePosAngles(compass, azimuth) if azimuth > 90 and azimuth <= 180: #Segundo cuadrante (E-S) if ultimaPosicion > 0 and ultimaPosicion <= 90: self.startingPos(compass) ultimaPosicion = 0 azimuth = azimuth + 180 if ultimaPosicion < azimuth: self.moveNegAngles(compass, azimuth) else: self.movePosAngles(compass, azimuth) if azimuth > 180 and azimuth <= 270: #Tercer cuadrante (S-O) if ultimaPosicion >= 270 and ultimaPosicion <= 359: self.startingPos(compass) ultimaPosicion = 0 azimuth = azimuth - 180 if ultimaPosicion < azimuth: self.moveNegAngles(compass, azimuth) else: self.movePosAngles(compass, azimuth) if azimuth > 270 and azimuth <= 359: #Cuarto cuadrante (O-N) if ultimaPosicion > 0 and ultimaPosicion <= 90: self.startingPos(compass) ulitmaPosicion = 0 if ultimaPosicion < azimuth: self.moveNegAngles(compass, azimuth) else: self.movePosAngles(compass, azimuth) self.setAngulobase(azimuth) servo1.stop() GPIO.cleanup() return self.getAngulobase()
class Rover(object): def __init__(self, color, location_str, command_str): self.color = color x, y, heading = location_str.split(' ') self.x_coord = int(x) self.y_coord = int(y) self.compass = Compass(heading) self.commands = command_str def complete_mission(self): for c in self.commands: self.execute_command(c) def execute_command(self, command): if command == "L": self.turn_left() elif command == "R": self.turn_right() elif command == "M": self.go_forward() else: raise ValueError("incorrect command! - {0}".format(c)) def current_position(self): return "x: {0}, y: {1}, heading: {2}".format(self.x_coord, self.y_coord, self.compass.direction()) def go_forward(self): heading = self.compass.direction() if heading == Compass.NORTH: self.y_coord += 1 elif heading == Compass.EAST: self.x_coord += 1 elif heading == Compass.SOUTH: self.y_coord -= 1 elif heading == Compass.WEST: self.x_coord -= 1 else: raise ValueError("incorrect heading! - {0}".format(heading)) def turn_left(self): self.compass.rotate_left() def turn_right(self): self.compass.rotate_right()
class antenna_tracker(): def __init__(self, servo_COM, antenna_Lat, antenna_Lon, antenna_altitude): self.compass = Compass(antenna_Lat, antenna_Lon, antenna_altitude) print(self.compass) self.servo_COM = servo_COM def panBothServos(self): #Moves servos through range of motion tests #global s print "Starting serial communication with", servoCOM if servoAttached: for i in range(127, 0, -2): self.moveTiltServo(i) self.movePanServo(i) time.sleep(0.05) time.sleep(1) for i in range(0, 254, 2): self.moveTiltServo(i) self.movePanServo(i) time.sleep(0.05) time.sleep(1) print "Motion Test Finished" else: print "Error: Settings set to no Servo Connection" def moveToCenterPos( self ): #Send servos to their center pos (should be horizontal and straight ahead if zeroed) #global s print "Starting serial communication with", servoCOM if servoAttached: self.moveTiltServo(127) self.movePanServo(127) print "Move to Center Command Sent via", servoCOM else: print "Error: Settings set to no Servo Connection" def moveToTarget( self, bearing, elevation): #moves servos based on a bearing and elevation angle centerBear = 0 temp = 0 if ((bearing > 180) and (centerBear == 0)): centerBear = 360 elif (((centerBear - bearing) > 180) and (centerBear >= 270)): bearing = bearing + 360 elif (((centerBear - bearing) > 180) and (centerBear <= 180)): temp = centerBear centerBear = 360 + temp #print ("\tBearing: %.0f" %bearing) #print ("\tElevation Angle: %.0f"%elevation) #panTo = (((offsetDegrees-bearing+panOffset)*255)/panRange)+127.5 # With new digital servos, can use map method as described here: http://arduino.cc/en/reference/map panTo = ((bearing - (centerBear - 168)) * (servo_max - servo_min) / ((centerBear + 168) - (centerBear - 168)) + servo_min) + (255 * panOffset / 360) if panTo > 254: panTo = 254 if panTo < 0: panTo = 0 #print "\tServo Degrees:" if servoAttached: self.movePanServo(math.trunc(panTo)) #tiltTo = (255*(96-elevation))/90 #tiltTo = (255*(tiltRange-elevation+tiltOffset))/90 #If Error in Antenna Mount i.e. put antenna on backwards fix with changing 0-elevation to elevation (must change tilt stops too tiltTo = (((0 - elevation) - tilt_angle_min) * (servo_max - servo_min) / (tilt_angle_max - tilt_angle_min) + servo_min) + tiltOffset if tiltTo > 254: tiltTo = 254 if tiltTo < 0: tiltTo = 0 if servoAttached: self.moveTiltServo(math.trunc(tiltTo)) if (temp != 0): centerBear = temp #if servoAttached: # s.close() def moveTiltServo(self, position): s = serial.Serial(self.servo_COM, baudrate=servoBaud, timeout=servoTimeout) #move tilt if (position < 70): #80 degrees upper limit moveTilt = [moveCommand, tiltChannel, chr(70)] elif (position > 123): #5 degrees lower limit moveTilt = [moveCommand, tiltChannel, chr(123)] else: moveTilt = [moveCommand, tiltChannel, chr(position)] s.write(moveTilt) #print "\t\tTilt Pan: ", float(position) #RFD (for use with a second antenna tracker) # moveTilt = [moveCommand,rfd_tiltChannel,chr(position)] s.close() #print("Tilting") def movePanServo(self, position): s = serial.Serial(self.servo_COM, baudrate=servoBaud, timeout=servoTimeout) ''' if previousPan > position: position += 1 previousPan = position ''' #move Ubiquity movePan = [moveCommand, panChannel, chr(255 - position)] s.write(movePan) #move RFD # movePan = [moveCommand,rfd_panChannel,chr(255-position)] # s.write(movePan) #print "\t\tMove Pan: ", float(position) s.close() #print("Paning") def pointToTarget(self, latitude, longitude, altitude): distance = self.compass.get_distance(latitude, longitude) bearing = self.compass.get_bearing(latitude, longitude) angle = self.compass.get_elevation(latitude, longitude, altitude) feet_to_miles = (1 / 5280) print("Dist={0:.2f}, Bearing={1:.2f}, Angle={2:.2f}".format( distance * feet_to_miles, bearing, angle)) self.moveToTarget(bearing, angle)
def _set_new_direction(self, direction, rotation_angle=None): self.direction = Compass.get_new_direction(direction, rotation_angle) print 'Direction: {}.'.format(self.direction)
def run(): oled = Oled(12, 2, 15, 14, 13, 4) ssd = oled.ssd #home=14, left=13 right=11 back=7 key = Key(33, 25, 26, 32) key.start() keyvalue = 15 menu_index = 1 menu = Menu(ssd, jsonfile) def lowpower(ssd): ssd.poweroff() machine.deepsleep() wake1 = Pin(33, mode=Pin.IN) #level parameter can be: esp32.WAKEUP_ANY_HIGH or esp32.WAKEUP_ALL_LOW esp32.wake_on_ext0(pin=wake1, level=esp32.WAKEUP_ALL_LOW) time_cnt = 0 clock = Clock(ssd, key) alarm = Alarm(ssd, key, jsonfile) compass = Compass(ssd, key) weather = Weather(ssd, key, jsonfile) appstore = Appstore(ssd, key, jsonfile) sysset = Sysset(ssd, key, jsonfile) menu_dic = { 1: menu.clock, 2: menu.alarm, 3: menu.compass, 4: menu.weather, 5: menu.appstore, 6: menu.sysset } menu_event = { 1: clock.run, 2: alarm.run, 3: compass.run, 4: weather.run, 5: appstore.run, 6: sysset.run } while True: # time.sleep_ms(200) #time_cnt+=1 #if time_cnt>50: # lowpower(ssd) if key.read() == key.LEFT_PRESS: time_cnt = 0 menu_index = menu_index - 1 if menu_index <= 1: menu_index = 1 time.sleep_ms(200) elif key.read() == key.RIGHT_PRESS: time_cnt = 0 menu_index = menu_index + 1 if menu_index >= 6: menu_index = 6 time.sleep_ms(200) menu.disSensor(90, 0, 0.57) menu_dic[menu_index]() if key.read() == key.HOME_PRESS: time_cnt = 0 ssd.fill(0) menu_event[menu_index]() ssd.fill(0) time.sleep_ms(200)
def test_bearing(self): C = Compass(43.682213, -70.450696, 0) self.assertEqual(round(C.get_bearing(43.682194, -70.450769), 2), 250.21) C = Compass(0, 0, 0) self.assertEqual(round(C.get_bearing(90, 0), 2), 0) self.assertEqual(round(C.get_bearing(0, 90), 2), 90) self.assertEqual(round(C.get_bearing(-90, 0), 2), 180) self.assertEqual(round(C.get_bearing(0, -90), 2), 270) C = Compass(33.77487, -84.39904, 0) self.assertEqual(round(C.get_bearing(34.44768, -84.39367), 3), 0.377) #N - 74.633 km, 46.375 mi, self.assertEqual(round(C.get_bearing(34.28672, -83.75097), 3), 46.197) #NE - 82.495 km, 51.26 mi self.assertEqual(round(C.get_bearing(33.77001, -83.69604), 3), 90.281) #E - 65.121 km, 40.464 mi self.assertEqual(round(C.get_bearing(33.46581, -83.98718), 3), 131.909) #SE - 51.339 km, 31.901 mi self.assertEqual(round(C.get_bearing(33.4933, -84.43487), 3), 186.058) #S - 31.407 km, 19.515 mi self.assertEqual(round(C.get_bearing(33.54368, -84.62301), 3), 218.943) #SW - 33.001 km, 20.506 mi self.assertEqual(round(C.get_bearing(33.76829, -84.71982), 3), 268.676) #W - 29.723 km, 18.469 mi self.assertEqual(round(C.get_bearing(34.03842, -84.68068), 3), 318.508) #NW - 39.154 km, 24.329 mi
class TrackerGraphics: smallNumber = "" def __init__(self, scope, resources, calibration): #initialise pygame self.compass = Compass(calibration) #get a compass object self.scope = scope #Use a pyscope object so we can run the program from the console self.resources = resources #get the resources class instance self.contactsArray = Contacts() #get a contacts class instance self.text = None self.smallnumber_text = None self.dots = None self.display_scale = None self.closest_text = "" self.smallnumber = "" def update(self, wave_size, addcontact, calibration): self.compass.updatexy(calibration) if addcontact: self.contactsArray.addContact(self.compass.smbusAvailable) #Read the current direction of the tracker from the digital compass bearing = self.compass.getCompassBearing() #rotate the contacts in relation to the grid self.contactsArray.updateContactsInWorld(bearing) #Set the screen background backdrop = self.rot_center(self.resources.compass, bearing) background_colour = (0, 0, 0) self.scope.pySurface.fill(background_colour) self.scope.pySurface.blit(backdrop, (0, 22)) #render our contacts if (len(self.contactsArray.ContactArray)) > 0: for x in self.contactsArray.ContactArray: trackerScale = self.contactsArray.trackerScale opacityIndex = x.getOpacityIndex(wave_size, trackerScale) self.scope.pySurface.blit( self.resources.contactBack[opacityIndex], (x.worldX - 25, x.worldY - 25)) for x in self.contactsArray.ContactArray: trackerScale = self.contactsArray.trackerScale opacityIndex = x.getOpacityIndex(wave_size, trackerScale) self.scope.pySurface.blit( self.resources.contactFore[opacityIndex], (x.worldX - 25, x.worldY - 25)) #render the wave pulse with current wave size self.scope.pySurface.blit(self.resources.waves[wave_size], (0, 0)) #Convert the range to the closest blip to text. If no blip present display dashes if self.contactsArray.getClosestContactDistance() == 999: self.closest_text = "" self.smallnumber = "" else: if wave_size == 0: self.closest_text = str( self.contactsArray.getClosestContactDistance()) self.smallnumber = str(random.randint(10, 99)) range_prefix = "" if len(self.closest_text) == 1: range_prefix = '0' if len(self.closest_text) == 2: range_prefix = '' #Render the display self.text = self.resources.font.render(str(bearing), 1, (215, 0, 0)) self.text = self.resources.font.render( range_prefix + self.closest_text, 1, (215, 0, 0)) self.smallnumber_text = self.resources.smallfont.render( self.smallnumber, 1, (215, 0, 0)) self.dots = self.resources.smallfont.render("-", 1, (215, 0, 0)) self.display_scale = self.resources.displayScaleFont.render( 'm', 1, (215, 0, 0)) #render the info panel to screen self.scope.pySurface.blit(self.resources.info, (0, 182)) self.scope.pySurface.blit(self.text, (129, 182)) self.scope.pySurface.blit(self.smallnumber_text, (170, 182)) self.scope.pySurface.blit(self.dots, (162, 182)) self.scope.pySurface.blit(self.display_scale, (178, 195)) self.scope.screen.blit(self.scope.pySurface, (0, 0)) #update the display and show our images pygame.display.update() if wave_size == 15: self.contactsArray.moveContacts() return self.contactsArray #Define the image rotation function def rot_center(self, image, angle): orig_rect = image.get_rect() rot_image = pygame.transform.rotate(image, angle * -1) rot_rect = orig_rect.copy() rot_rect.center = rot_image.get_rect().center rot_image = rot_image.subsurface(rot_rect).copy() return rot_image
nextLocation = dict({}) currentLocation = dict({}) nextGpsLat = None nextGpsLon = None # ================ # Connecting Motor # ================ try: roverMotor = Motor(motorPath) motorConnected = True except serial.serialutil.SerialException: motorConnected = False gps = RoverGps() compass = Compass("/dev/ttyACM1") # ================ # Helper Functions # ================ def resetMotor(roverMotor): global motorConnected try: roverMotor.connectMotor(motorPath) motorConnected = True except serial.serialutil.SerialException: motorConnected = False def getSlope(currentLocation):
compass.enc._value = compass.pause_val await asyncio.sleep_ms(200) else: if compass.pause_val: compass.enc._value = compass.pause_val compass.pause_val = None compass.checkDirection() print(compass.getDirection()) await asyncio.sleep_ms(100) # Set up compass compass = Compass(enc=(12, 13), north=2, south=0, west=14, east=4, output1=16, output2=15) input_pause = Pin(5, Pin.IN) # Set up async loop = asyncio.get_event_loop() sleep(1) try: loop.run_until_complete(main(compass, input_pause)) except KeyboardInterrupt: print("Bye") except OSError:
def main(): try: Gps = Gps() compass = Compass() global nextLocationIndex global locations except Exception as e: print(str(e)) print("Error in creating the objects") return try: while 1: try: for _i in range(len(locations)): Reached = False while(not Reached): currentLocation = Gps.getGpsData() slope = getSlope(currentLocation) print(centerBot(compass, slope, Gps, 10)) Reached = isReached(Gps) if(Reached): sock.send("27".encode()) input("Reached "+str(nextLocationIndex) + " Press anything to continue") sleep(0.1) nextLocationIndex += 1 except Exception as e: print(str(e)) PORT = '/dev/ttyUSB0' BUFFER_SIZE = 3 ser = serial.Serial(PORT) command = -1 ignoreCommand = False x = -1 toAngle = 0 try: while True: try: data = ser.read(BUFFER_SIZE) currentAngle = compass.getCompassAngle() if(not ignoreCommand): x=parseCommand(data) print("\n") if(x==27): toAngle = currentAngle print("Stop") elif(x==87): toAngle = currentAngle print("Forward") elif(x==65): toAngle = (currentAngle - 90 + 360)%360 ignoreCommand = True print("Left") elif(x==68): toAngle = (currentAngle + 90 + 360)%360 ignoreCommand = True print("Right") else: print("Command Not Found ") sock.send(str(x).encode()) os.system(cmd) except Exception as e: print(e) ser.close() ser = serial.Serial(PORT) except Exception as e: print(e) finally: ser.close() except Exception as e: print(str(e)) finally: global sock sock.close()
def test_distance(self): C = Compass(33.77487, -84.39904, 0) feet_to_miles = (1 / 5280) self.assertEqual( round(C.get_distance(34.44768, -84.39367) * feet_to_miles, 1), 46.5) #N - 74.633 km, 46.375 mi, self.assertEqual( round(C.get_distance(34.28672, -83.75097) * feet_to_miles, 1), 51.3) #NE - 82.495 km, 51.26 mi self.assertEqual( round(C.get_distance(33.77001, -83.69604) * feet_to_miles, 1), 40.4) #E - 65.121 km, 40.464 mi self.assertEqual( round(C.get_distance(33.46581, -83.98718) * feet_to_miles, 1), 31.9) #SE - 51.339 km, 31.901 mi self.assertEqual( round(C.get_distance(33.4933, -84.43487) * feet_to_miles, 1), 19.6) #S - 31.407 km, 19.515 mi self.assertEqual( round(C.get_distance(33.54368, -84.62301) * feet_to_miles, 1), 20.5) #SW - 33.001 km, 20.506 mi self.assertEqual( round(C.get_distance(33.76829, -84.71982) * feet_to_miles, 1), 18.4) #W - 29.723 km, 18.469 mi self.assertEqual( round(C.get_distance(34.03842, -84.68068) * feet_to_miles, 1), 24.3) #NW - 39.154 km, 24.329 mi C = Compass(32.827103, -83.649268, 0) feet_to_miles = (1 / 5280) self.assertEqual(round(C.get_distance(32.826488, -83.645286), 1), 1241.2) #N - 74.633 km, 46.375 mi, self.assertEqual(round(C.get_distance(32.828877, -83.647539), 1), 836.5) #NE - 82.495 km, 51.26 mi self.assertEqual(round(C.get_distance(32.832558, -83.673762), 1), 7767.8) #E - 65.121 km, 40.464 mi self.assertEqual( round(C.get_distance(32.822634, -83.745582) * feet_to_miles, 1), 5.6) #SE - 51.339 km, 31.901 mi self.assertEqual( round(C.get_distance(32.580011, -85.067355) * feet_to_miles, 1), 84.2) #S - 31.407 km, 19.515 mi
def main(): # Local functions in main function global nextLocationIndex global locations global roverMotor botCentered = False locationAccuracy = 0.000005 print("Setting devices...") compass = Compass("/dev/ttyACM0") roverMotor = Motor(('192.168.43.94', 12345)) try: roverGps = RoverGps() except OSError: print("GPSD not started!") exit(-1) roverMotor.moveMotor('stop') print("All device set!") sleep(1) roverMotor.moveMotor('stop') # Set the bot to point at next location while not (170 < compass.getCompassAngle() and compass.getCompassAngle() < 180): print(compass.getCompassAngle()) # os.system("clear") botCentered = False # Rotate Rover Once # angle = compass.getCompassAngle()-100 # while compass.getCompassAngle()>angle or compass.getCompassAngle()<angle: # print("Calibration!") # roverMotor.moveMotor('left') roverMotor.moveMotor("stop") os.system("clear") print("Rover centered!") roverMotor.moveMotor('stop') sleep(2) # roverMotor.resetAllMotors() print("Locations:", locations) # input('Press anything to continue!') sleep(2) # ========= # Main Loop # ========= while nextLocationIndex < len(locations): # roverMotor.resetAllMotors() try: currentLocation = roverGps.getGpsData() # print(roverGps) except ValueError: print("GPS not set") continue if currentLocation[0] == None: print("GPS no location") continue if abs(currentLocation[0] - locations[nextLocationIndex][0]) < locationAccuracy and abs( currentLocation[1] - locations[nextLocationIndex][1]) < locationAccuracy: roverMotor.moveMotor("stop") nextLocationIndex += 1 if nextLocationIndex >= len(locations): break print(locations) print("Location Reached!", currentLocation) print("Coninue to next location") # input() sleep(2) # Center bot to north botCentered = False while not botCentered: # os.system("clear") if centerBot(compass, 0, roverGps, roverMotor, 20): print() botCentered = True botCentered = False print("Continue to location", locations[nextLocationIndex]) # input() continue slope = getSlope(currentLocation) # Move the rover to this slope while not botCentered: # os.system("clear") slope = getSlope(currentLocation) if centerBot(compass, slope, roverGps, roverMotor, 10): botCentered = True # sleep(0.5) if not centerBot(compass, slope, roverGps, roverMotor, 10): print() botCentered = False # # Move bot forward # # os.system("clear") try: print("Move Forward", roverGps.getGpsData(), locations[nextLocationIndex], slope, compass.getCompassAngle()) # roverMotor.moveMotor('forward') roverMotor.moveMotor('forward') except ValueError: print("Compass Value error") roverMotor.moveMotor('stop') print("Finished!")
# !/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Mon Jan 22 13:16:23 2018 @author: vijayasai """ from compass import Compass class Directions(object): def __init__(self): pass def if __name__ == "__main__": comp = Compass() data = [ (13.086878, 80.108567), (13.086704, 80.108540), (13.086552, 80.108583), (13.086477, 80.108546), (13.086412, 80.108807) ] for i in range(len(data)-1): comp.GeoData(data[i], data[i+1]) comp.solve() print comp.output
return ", ".join(flags) def startup_tasks(): """System commands to run on service start""" print("main: running startup tasks") subprocess.run("/usr/bin/tvservice -o", shell=True, capture_output=True) # disable HDMI os.nice(-5) gps = GPS() compass = Compass() threads["compass"] = threading.Thread(target=compass.track_ahrs, args=(), daemon=True) threads["compass"].start() display = Display(gps, compass) # Blocks until the GPS is ready display.start() os.nice(15) ac = Aircraft(gps) threads["aircraft"] = threading.Thread(target=ac.track_aircraft, args=(),
def setUp(self): self.i2c_compass = Mock() self.i2c_accel = Mock() self.compass = Compass(self.i2c_compass,self.i2c_accel)
def main(): roverGps="dummy" global nextLocationIndex global locations botCentered = False locationAccuracy=0.00001 # locationAccuracy=2 initializedCurrentLocation=False print("Setting devices...") compass = Compass("/dev/ttyACM0") # roverMotor = Motor("/dev/ttyACM0") # roverGps = RoverGps() # roverMotor.resetAllMotors() print("All device set!") #sleep(10) # Set the bot to point at north while not botCentered: print("Centering Rover!",end=": ") if centerBot(compass, 0, roverGps, 10): botCentered=True # sleep(0.1) # os.system("clear") # botCentered=False os.system("clear") print("Rover centered!") # roverMotor.resetAllMotors() sleep(2) setScaledLocations() print(locations) sleep(2) while nextLocationIndex < len(locations): # while True: # roverMotor.resetAllMotors() try: currentLocation = readGPS() # currentLocation = roverGps.getGpsData() # print(roverGps) print(readGPS()) # print(roverGps.getGpsData()) except ValueError: print("GPS not set") continue if currentLocation[0]==None: print("GPs no locayion") continue if abs(currentLocation[0]-locations[nextLocationIndex][0])<locationAccuracy and abs(currentLocation[1]-locations[nextLocationIndex][1])<locationAccuracy: nextLocationIndex+=1 if nextLocationIndex>=len(locations): break print(locations) print("Location Reached!", currentLocation) print("Press any key to continue") input() # sleep(2) # Center bot to north botCentered=False while not botCentered: # os.system("clear") print("Centering Rover!",end=": ") if centerBot(compass, 0, roverGps, 20): print() botCentered=True botCentered=False print("Press anything to continue to location", locations[nextLocationIndex]) input() continue slope = getSlope(currentLocation) # Move the rover to this slope while not botCentered: # os.system("clear") slope = getSlope(currentLocation) try: print("Centering Rover ", end=': ') except ValueError: print("Value Error") if centerBot(compass, slope, roverGps, 15): botCentered=True # sleep(0.5) if not centerBot(compass, slope, roverGps, 15): print() botCentered=False # Move bot forward # os.system("clear") try: print("Move Forward", readGPS(), slope, compass.getCompassAngle(), abs(slope)-abs(compass.getCompassAngle())) #roverMotor.forwardMotor() except ValueError: print("Compass Value error") print("Finished!")
class RouteManager(Thread): #Stati Thread STOPPED = 0 RUNNING = 1 status = STOPPED #Reference oggetto classo Motors motors_object = None #Reference classe oggetto Compass compass_object = None #Direzione obiettivo goal_direction_degrees = 300 #Tolleranza magnetometro compass_tolerance = 5 #Step decrementale potenza motori motors_deceleration_step = 15 normal_mode_status = 'ENABLED' bug_mode_status = 'DISABLED' directions = {0: 'LEFT', 1: 'RIGHT'} u_turn = False u_turn_forward_steps = 0 before_turn_steps = 0 max_before_turn_steps = 2 main_locked_direction = None max_bug_mode_critical_exceptions = 3 bug_mode_critical_exceptions_count = 0 bug_mode_last_move = None map_file_manager = None def getGoalDirectionDegrees(self): return self.goal_direction_degrees def getCompassTolerance(self): return self.compass_tolerance def setGoalDirectionDegrees(self, value): self.goal_direction_degrees = value def setCompassTolerance(self, value): self.compass_tolerance = value def __init__(self, motors_object, proximity_manager_object): #Check oggetto classe Motors if motors_object is None: raise Exception('Motors object cannot be null') return #Check istanza classe ProximityManager if proximity_manager_object is None: raise Exception('ProximityManager object cannot be null') return #Reference istanza oggetto classe Motors self.motors_object = motors_object #Reference istanza oggetto classe ProximityManager self.proximity_manager_object = proximity_manager_object #Reference istanza oggetto classe Compass print('Starting Compass..') self.compass_object = Compass() Thread.__init__(self) self.status = self.RUNNING self.name = self.__class__.__name__ self.start() def run(self): #print('Rotating to goal..') #self.rotationToDegrees( self.goal_direction_degrees ) while self.status != self.STOPPED: time.sleep(0.05) if self.normal_mode_status == 'ENABLED': print('Starting normalMode..') self.normalMode() elif self.bug_mode_status == 'ENABLED': print('Starting bugMode..') self.bugMode(self.goal_direction_degrees) def normalMode(self): try: #Caso di stop del thread if self.status == self.STOPPED or self.normal_mode_status == 'DISABLED': print( '$$$$$ NORMAL MODE: Normal Mode disabled, resuming to normal mode.. $$$$$' ) return self.proximity_manager_object.retrieveProximityData() if self.proximity_manager_object.getAvailability().get( 'FRONT') is False: print('$$$$$ NORMAL MODE: Enabling bugMode.. $$$$$') self.normal_mode_status = 'DISABLED' self.bug_mode_status = 'ENABLED' if self.map_file_manager is None: print('Initialing MapFileManager..') self.map_file_manager = MapFileManager() self.motors_object.restoreMotorActualPowerToDefault() return #Check stato motori a FORWARD if self.motors_object.getMotorsStatus( ) == self.motors_object.STOPPED: self.motors_object.forward() degrees = self.compass_object.getDegress() motor_left_actual_power = self.motors_object.getMotorLeftActualPower( ) motor_right_actual_power = self.motors_object.getMotorRightActualPower( ) #print("Degrees: " + str( degrees )) print("$$$$$ NORMAL MODE: Motor left actual power: " + str(motor_left_actual_power) + ' $$$$$') print("$$$$$ NORMAL MODE: Motor right actual power: " + str(motor_right_actual_power) + ' $$$$$') #Caso in cui si รจ a sinistra dell'obiettivo if degrees + self.compass_tolerance < self.goal_direction_degrees: print("$$$$$ NORMAL MODE: Goal on the right $$$$$") if motor_right_actual_power - self.motors_deceleration_step >= 140: motor_right_actual_power = motor_right_actual_power - self.motors_deceleration_step self.motors_object.updateMotorPower( 'RIGHT', motor_right_actual_power) elif degrees - self.compass_tolerance > self.goal_direction_degrees: print("$$$$$ NORMAL MODE: Goal on the left $$$$$") if motor_left_actual_power - self.motors_deceleration_step >= 140: motor_left_actual_power = motor_left_actual_power - self.motors_deceleration_step self.motors_object.updateMotorPower( 'LEFT', motor_left_actual_power) else: #Caso in cui obiettivo in posizione frontale print("$$$$$ NORMAL MODE: Goal on the front $$$$$") self.motors_object.forward(True) except proximityMeasurementErrorException, e: print('$$$$$ NORMAL MODE: proximityMeasurementErrorException: ' + str(e) + ' $$$$$') except proximityGetDistanceException, e: print('$$$$$ NORMAL MODE: proximityGetDistanceException: ' + str(e) + ' $$$$$')
def main(): # Local functions in main function global nextLocationIndex global locations global roverMotor botCentered = False locationAccuracy = 0.00001 print("Setting devices...") compass = Compass("/dev/ttyACM0") # roverMotor = Motor("/dev/ttyACM2") roverGps = RoverGps() # roverMotor.resetAllMotors() print("All device set!") sleep(2) # Set the bot to point at next location while not botCentered: if centerBot(compass, 0, roverGps, roverMotor, 10): botCentered = True # sleep(0.1) # os.system("clear") # botCentered=False # roverMotor.moveMotor("stop") os.system("clear") print("Rover centered!") # roverMotor.resetAllMotors() sleep(2) print("Locations:", locations) input('Press anything to continue!') # ========= # Main Loop # ========= while nextLocationIndex < len(locations): # roverMotor.resetAllMotors() try: currentLocation = roverGps.getGpsData() # print(roverGps) except ValueError: print("GPS not set") continue if currentLocation[0] == None: print("GPS no location") continue if abs(currentLocation[0] - locations[nextLocationIndex][0]) < locationAccuracy and abs( currentLocation[1] - locations[nextLocationIndex][1]) < locationAccuracy: # roverMotor.moveMotor("stop") nextLocationIndex += 1 if nextLocationIndex >= len(locations): break print(locations) print("Location Reached!", currentLocation) print("Press any key to continue") input() # sleep(2) # Center bot to north botCentered = False while not botCentered: # os.system("clear") if centerBot(compass, 0, roverGps, roverMotor, 20): print() botCentered = True botCentered = False print("Press anything to continue to location", locations[nextLocationIndex]) input() continue slope = getSlope(currentLocation) # Move the rover to this slope while not botCentered: # os.system("clear") slope = getSlope(currentLocation) if centerBot(compass, slope, roverGps, roverMotor, 15): botCentered = True # sleep(0.5) if not centerBot(compass, slope, roverGps, roverMotor, 15): print() botCentered = False # # Move bot forward # # os.system("clear") try: print("Move Forward", roverGps.getGpsData(), locations[nextLocationIndex], slope, compass.getCompassAngle()) # roverMotor.moveMotor('forward') except ValueError: print("Compass Value error") print("Finished!")
#!/usr/bin/env python3 # # This file is part of paradar (https://github.com/blinken/paradar). # # Copyright (C) 2020 Patrick Coleman # # This program is free software: you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by # the Free Software Foundation, either version 3 of the License, or # (at your option) any later version. # # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # # You should have received a copy of the GNU General Public License # along with this program. If not, see <https://www.gnu.org/licenses/>. from compass import Compass compass = Compass() compass.calibrate()
def __init__(self, servo_COM, antenna_Lat, antenna_Lon, antenna_altitude): self.compass = Compass(antenna_Lat, antenna_Lon, antenna_altitude) print(self.compass) self.servo_COM = servo_COM