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
