def __init__(self):
self.__horizontalBoundary = None
self.__verticalBoundary = None
self.__horizontalCoordinate = None
self.__verticalCoordinate = None
self.__orientation = None
self.__roverCompass = compass.Compass(["N", "E", "S", "W"])
def calibrate_boat():
lsm303 = Adafruit_LSM303.LSM303()
start = time.time()
readings = []
print("rotate the compass through all orientations for 15 seconds...",
end='')
while time.time() - start < 15:
accel, mag = lsm303.read()
readings.append(mag)
print("done.")
array = np.array(readings)
c = compass.Compass(declination=13)
c.calibrate(array)
print(c)
c.save(boat.Boat.DEFAULT_CALIBRATION_FILE_PATH)
c = compass.load(boat.Boat.DEFAULT_CALIBRATION_FILE_PATH)
print(c)
def __init__(self):
self.connectArduino()
self.encoders = [0, 0, 0, 0]
self.lastEncoders = [0, 0, 0, 0]
self.isFirstRun = True
self.lastSentWheelSpeed = Object
self.lastSentWheelSpeed.frontLeftSpeed = 0
self.lastSentWheelSpeed.frontRightSpeed = 0
self.lastSentWheelSpeed.rearLeftSpeed = 0
self.lastSentWheelSpeed.rearRightSpeed = 0
self.lastSentIter = Object
self.lastSentIter.frontRight = -999
self.lastSentIter.frontLeft = -999
self.lastSentIter.rearRight = -999
self.lastSentIter.rearLeft = -999
self.currentIter = 0
self.reboot = False
self.servo = 0
self.compass = compass.Compass()
def __init__(self, master=None):
### for debug ###
self.heading_azimuth_dummy = 0
#################
# 数値初期化
self.distance = 0.0
self.target_azimuth = 0
self.heading_azimuth = 0
self.heading_latitude = 0.0 # 現在地緯度
self.heading_longitude = 0.0 # 経度
self.target_latitude = 0.0 # 目標緯度
self.target_longitude = 0.0 # 経度
# 諸々初期化
self.compass = compass.Compass()
# self.serial = sh.SerialHandler()
# Tkinter初期化
super().__init__(master)
self.master = master
self.master.title('GPS finder')
self.pack()
self.create_widgets()
self.master.after(500, self.update_widgets)
#!/usr/bin/python
import sys
from track import Track
import gps,compass
import config
c= config.Config()
track = Track(c.position_log_name)
gps.GPS("gps")
compass.Compass("compass",c.compass_deviation)
track.replay(float(c.position_log_rate))
import display
Image = display.Image
display = display.Display()
import button
button_a = button.Button(35)
button_b = button.Button(27)
import temperature
__adc = machine.ADC(machine.Pin(34, machine.Pin.IN))
__adc.atten(machine.ADC.ATTN_11DB)
temperature = temperature.Temperature(__adc).temperature
try:
from mpu9250 import MPU9250
from mpu6500 import MPU6500
__i2c = machine.I2C(scl=machine.Pin(22), sda=machine.Pin(21), freq=200000)
__dev = __i2c.scan()
# print("dev ", __dev)
if 104 in __dev:
print("1.4 version")
__sensor = MPU9250(__i2c, MPU6500(__i2c, 0x68))
if 105 in __dev:
print("1.2 version No compass")
__sensor = MPU9250(__i2c, MPU6500(__i2c, 0x69))
import accelerometer
accelerometer = accelerometer.Direction(__sensor)
import compass
compass = compass.Compass(__sensor)
except Exception as e:
print("MPU9250 Error", e)
def calibrate_compass():
arr = np.loadtxt("echoed.txt")
c = compass.Compass()
c.calibrate(arr)
c.save("good_boat.txt")
def turn_to_bearing(compass_tool, desired_heading):
correct = None
while not correct:
current_bearing = compass_tool.get_heading()
correct = current_bearing == desired_heading
turn = desired_heading - current_bearing
alternative_turn = desired_heading - current_bearing + 360
if abs(turn) > abs(alternative_turn):
best_turn = alternative_turn
else:
best_turn = turn
if best_turn > 0:
direction = "right"
elif best_turn < 0:
direction = "left"
else:
direction = "not"
print("Turn me %s!" % direction)
time.sleep(0.5)
if __name__ == "__main__":
my_compass = compass.Compass()
direction = 100
while direction != 0:
direction = my_compass.which_way_to_bearing(97)
print("Need to turn %d" % direction)
time.sleep(0.5)
#!/usr/bin/python
from persistant import *
import compass
from config import Config
c = Config()
compass = compass.Compass("compass", float(c.compass_deviation))
compass.update(float(c.compass_update_rate))
def __init__(self, compass_file=None):
self.compass = compass.Compass()
self.has_new_data = False
self.radio = MyRadio()
def rover(full_i2c):
# Re-direct our output to standard error, we need to ignore standard out
# to hide some nasty print statements from pygame
sys.stdout = sys.stderr
# Setup the PicoBorg Reverse
PBR = PicoBorgRev.PicoBorgRev()
# Uncomment and change the value if you have changed the board address
# PBR.i2cAddress = 0x44
PBR.Init()
if not PBR.foundChip:
boards = PicoBorgRev.ScanForPicoBorgReverse()
if len(boards) == 0:
print('No PicoBorg Reverse found, check you are attached :)')
else:
print('No PicoBorg Reverse at address %02X,'
'but we did find boards:' % (PBR.i2cAddress))
for board in boards:
print(' %02X (%d)' % (board, board))
print('If you need to change the I�C address change the setup'
'line so it is correct, e.g.')
print('PBR.i2cAddress = 0x%02X' % (boards[0]))
sys.exit()
# PBR.SetEpoIgnore(True) # Uncomment to disable EPO latch,
# # needed if you do not have a switch / jumper
# Ensure the communications failsafe has been enabled!
failsafe = False
for i in range(5):
PBR.SetCommsFailsafe(True)
failsafe = PBR.GetCommsFailsafe()
if failsafe:
break
if not failsafe:
print('Board %02X failed to report in failsafe mode!' %
(PBR.i2cAddress))
sys.exit()
PBR.ResetEpo()
# Settings for the joystick
axisUpDown = 1 # Joystick axis to read for up / down position
axisUpDownInverted = False # Set this to True if up and down appear to be swapped
axisLeftRight = 0 # Joystick axis to read for left / right position
axisLeftRightInverted = False # Set this to True if left and right appear to be swapped
buttonResetEpo = 4 # Joystick button number to perform an EPO reset (Start)
buttonSlow = 0 # Joystick button number for driving slowly whilst held (L2)
slowFactor = 0.5 # Speed to slow to when the drive slowly button is held, e.g. 0.5 would be half speed
buttonFastTurn = 2 # Joystick button number for turning fast (R2)
interval = 0.10 # Time between updates in seconds, smaller responds faster but uses more processor time
# buttonRight = 7
# buttonUp = 6
# buttonLeft = 5
# buttonDown = 4
pitch = 0.0
# pitch_delta = 0.02
direction = 0.0
# direction_delta = 0.02
if (full_i2c):
time.sleep(1)
UB = UltraBorg.UltraBorg()
UB.Init()
UB.SetServoPosition1(pitch)
UB.SetServoPosition2(direction)
time.sleep(1)
XLoBorg.Init()
time.sleep(1)
# Initialise display
lcd_i2c.lcd_init()
cp = compass.Compass()
doCalibration = -1
isCalibrated = 0
delayReadCount = 0
# Power settings
voltageIn = 12.0 # Total battery voltage to the PicoBorg Reverse
voltageOut = 12.0 * 0.95 # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power
# Setup the power limits
if voltageOut > voltageIn:
maxPower = 1.0
else:
maxPower = voltageOut / float(voltageIn)
# Setup pygame and wait for the joystick to become available
PBR.MotorsOff()
os.environ[
"SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
pygame.init()
# pygame.display.set_mode((1,1))
print('Waiting for joystick... (press CTRL+C to abort)')
while True:
try:
try:
pygame.joystick.init()
# Attempt to setup the joystick
if pygame.joystick.get_count() < 1:
# No joystick attached, toggle the LED
PBR.SetLed(not PBR.GetLed())
pygame.joystick.quit()
time.sleep(0.5)
else:
# We have a joystick, attempt to initialise it!
joystick = pygame.joystick.Joystick(0)
break
except pygame.error:
# Failed to connect to the joystick, toggle the LED
PBR.SetLed(not PBR.GetLed())
pygame.joystick.quit()
time.sleep(0.5)
except KeyboardInterrupt:
# CTRL+C exit, give up
print('\nUser aborted')
PBR.SetLed(True)
sys.exit()
print('Joystick found')
joystick.init()
PBR.SetLed(False)
try:
print('Press CTRL+C to quit')
driveLeft = 0.0
driveRight = 0.0
running = True
hadEvent = False
upDown = 0.0
leftRight = 0.0
# Loop indefinitely
while running:
# Get the latest events from the system
hadEvent = False
events = pygame.event.get()
# Handle each event individually
for event in events:
if event.type == pygame.QUIT:
# User exit
running = False
elif event.type == pygame.JOYBUTTONDOWN:
# A button on the joystick just got pushed down
hadEvent = True
elif event.type == pygame.JOYAXISMOTION:
# A joystick has been moved
hadEvent = True
if hadEvent:
# Read axis positions (-1 to +1)
if axisUpDownInverted:
upDown = -joystick.get_axis(axisUpDown)
else:
upDown = joystick.get_axis(axisUpDown)
if axisLeftRightInverted:
leftRight = -joystick.get_axis(axisLeftRight)
else:
leftRight = joystick.get_axis(axisLeftRight)
# Apply steering speeds
if not joystick.get_button(buttonFastTurn):
leftRight *= 0.5
# Determine the drive power levels
driveLeft = -upDown
driveRight = -upDown
if leftRight < -0.05:
# Turning left
driveLeft *= 1.0 + (2.0 * leftRight)
elif leftRight > 0.05:
# Turning right
driveRight *= 1.0 - (2.0 * leftRight)
# Check for button presses
if joystick.get_button(buttonResetEpo):
PBR.ResetEpo()
if joystick.get_button(buttonSlow):
driveLeft *= slowFactor
driveRight *= slowFactor
pitch = joystick.get_axis(4)
direction = -joystick.get_axis(3)
# if joystick.get_button(buttonUp):
# if pitch < 1.0:
# pitch += pitch_delta;
# if joystick.get_button(buttonDown):
# if pitch > -1.0:
# pitch -= pitch_delta;
# if joystick.get_button(buttonLeft):
# if direction > -1.0:
# direction -= direction_delta;
# if joystick.get_button(buttonRight):
# if direction < 1.0:
# direction += direction_delta;
if (full_i2c):
if joystick.get_button(1):
if (doCalibration != 1):
lcd_i2c.lcd_string("Calibrating...",
lcd_i2c.LCD_LINE_1)
lcd_i2c.lcd_string("Drive in circles",
lcd_i2c.LCD_LINE_2)
time.sleep(2)
lcd_i2c.lcd_byte(
0x01,
lcd_i2c.LCD_CMD) # 000001 Clear display
doCalibration = 1
if joystick.get_button(3):
if (doCalibration == 1):
lcd_i2c.lcd_string("Calibration",
lcd_i2c.LCD_LINE_1)
lcd_i2c.lcd_string("stopped",
lcd_i2c.LCD_LINE_2)
time.sleep(2)
lcd_i2c.lcd_byte(
0x01,
lcd_i2c.LCD_CMD) # 000001 Clear display
doCalibration = 0
cp.calibrate_compass()
print("number of samples: %d" % len(cp.rawX))
print("D1 origin: %d, scale %d" %
(cp.origin_D1, cp.scale_D1))
print("D2 origin: %d, scale %d" %
(cp.origin_D2, cp.scale_D2))
isCalibrated = 1
UB.SetServoPosition1(pitch)
UB.SetServoPosition2(direction)
# UB.SetServoPosition1(upDown)
# UB.SetServoPosition2(leftRight)
# Set the motors to the new speeds
PBR.SetMotor1(driveRight * maxPower)
PBR.SetMotor2(-driveLeft * maxPower)
print("joystick up/down: %.2f left/right %.2f" %
(upDown, leftRight))
print("direction:%.2f pitch: %.2f" % (direction, pitch))
# Change the LED to reflect the status of the EPO latch
PBR.SetLed(PBR.GetEpo())
# Wait for the interval period
time.sleep(interval)
if (doCalibration == 1):
mx, my, mz = XLoBorg.ReadCompassRaw()
cp.push_calibration_value(mx, my, mz)
# print("Raw data: %d %d %d" % (mx, my, mz))
# lcd_i2c.lcd_string("calibrating...",lcd_i2c.LCD_LINE_1)
if (isCalibrated == 1):
if (delayReadCount < 10):
delayReadCount += 1
else:
mx, my, mz = XLoBorg.ReadCompassRaw()
heading = cp.get_heading(mx, my, mz)
# print("Direction: %d" % heading)
lcd_i2c.lcd_string("Direction: %d" % heading,
lcd_i2c.LCD_LINE_1)
delayReadCount = 0
# Disable all drives
PBR.MotorsOff()
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
PBR.MotorsOff()
if (full_i2c):
UB.SetServoPosition1(0.0)
UB.SetServoPosition2(0.0)
direction_delta = 0.02
time.sleep(1)
UB = UltraBorg.UltraBorg()
UB.Init()
UB.SetServoPosition1(pitch)
UB.SetServoPosition2(direction)
time.sleep(1)
XLoBorg.Init()
time.sleep(1)
# Initialise display
lcd_i2c.lcd_init()
cp = compass.Compass()
doCalibration = -1
isCalibrated = 0
delayReadCount = 0
# Power settings
voltageIn = 12.0 # Total battery voltage to the PicoBorg Reverse
voltageOut = 12.0 * 0.95 # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power
# Setup the power limits
if voltageOut > voltageIn:
maxPower = 1.0
else:
maxPower = voltageOut / float(voltageIn)
# Setup pygame and wait for the joystick to become available
