def __init__(self, device='MPU-9150'):
BaseThread.__init__(self)
log.d("Init serialThread")
# configure and retrieve IMU scales
self.imu = IMU(device)
self.csvScale = self.imu.getScaleVector()
self.csvSize = len(self.csvScale)
self.range_accel = self.imu.getRangeAccelerometer()
self.range_gyro = self.imu.getRangeGyroscope()
self.range_mag = self.imu.getRangeMagnetometer()
self.range_euler = self.imu.getRangeEuler()
# star sensor fusion
self.fusion = SensorFusion()
async def handler(websocket: websockets.server.WebSocketServer,
path: str) -> None:
imu = IMU()
while True:
request = await websocket.recv()
request = json.loads(request)
data = imu.sample()
await websocket.send(
json.dumps({
"time": int(round(time.time() * 1000)),
"calibration": imu.check_calibration(),
"orientation": data,
"motors": [0, 0, 0, 0]
}))
def __init__(self, log=False):
if str(log) == "True":
dir_path = os.getcwd() + "/"
file_name = time.strftime("%Y.%m.%d.%H.%M.%S") + ".log"
log = dir_path + file_name
self.log = log
self.mtr = BigBoy()
self.imu = IMU()
self.fps = FPS(1.0)
self.t0 = time.time()
self.steps = 5
self.max_steps = 60
self.pos = 0
self.threshold = 1.5
self.tilt_constant = -25.0
def main():
# read arguments
ap = argparse.ArgumentParser()
ap.add_argument("-p", "--port",
help="path for serial port")
args = vars(ap.parse_args())
# inits IMU
port = args.get('port', None)
if port is None:
port = '/dev/ttyUSB_IMU'
imu = IMU(port)
# start subprocess
imu.start()
try:
while True:
print("yaw = ", imu.get_yaw())
print("yaw2 = ", imu.get_yaw2())
print("pitch = ", imu.get_pitch())
print("roll = ", imu.get_roll())
print("angx = ", imu.get_angx())
print("angy = ", imu.get_angy())
print("angz = ", imu.get_angz())
print("accx = ", imu.get_accx())
print("accy = ", imu.get_accy())
print("accz = ", imu.get_accz())
print("magx = ", imu.get_magx())
print("magy = ", imu.get_magy())
print("magz = ", imu.get_magz())
print()
time.sleep(1)
except KeyboardInterrupt:
pass
finally:
print("Stopping remaining threads...")
imu.stop()
def main():
imu = IMU()
frametime = 1.0 / 30.0
print("Ready.")
while True:
try:
s = time.time()
imu.poll()
print(imu.getLinearAccel())
time.sleep(max(0, frametime - (time.time() - s)))
except Exception as e:
print(e)
break
print("-- Program at End --")
def main():
"""Main loop"""
global AB_ID, MIN_SEC_FADE, MAX_SEC_FADE, log, wait
# Launch intro image - to make it clear that the launch is on its way. Also: Feel free to update the image. I
introImage = Img("ImgOffline/polaIntro.jpeg")
fadeIn(introImage)
screenIsDark = True
shakes = 0
# Setup image library and API connection
api = APIconnection()
setup = Settings(api)
imageList = ImageLib(api)
log.connect(api)
log.logevent("Init", AB_ID)
currentImage = imageList.getNextImage()
# Fade out intro image - to make it clear that the Pola is ready to go
fadeOut(IntroImage)
while True:
# If no image is displayed - wait for shake
if screenIsDark == True:
imu = IMU()
if imu.checkShake() == True:
log.logevent("Shake", AB_ID)
fadeIn(currentImage)
screenIsDark = False
shakes = 0
"""After the image fade-in is complete, we do most of our expensive api calls.
This way, Pola will immediately be ready to show the next image after fadeout,
and users will never have to wonder if their shakes are being registered,
or if it is working on some other process"""
# Immediately fade out offline-notifications
# (we do not want these to be visible for 6 hours if the wifi returns in 6 minutes)
if currentImage.offline:
fadeoutTime = 0
else:
fadeoutTime = random.randint(MIN_SEC_FADE, MAX_SEC_FADE)
print("Fadeout Time: ", fadeoutTime)
fadeoutCounter = 0
nextImage = imageList.getNextImage()
#Update prototype settings (A/B etc)
settingUpdate = setup.update()
if settingUpdate[0]:
AB_ID = settingUpdate[1]
MIN_SEC_FADE = hoursToMs(settingUpdate[2])
MAX_SEC_FADE = hoursToMs(settingUpdate[3])
SENSITIVITY = settingUpdate[4]
FADEIN_SPEED = settingUpdate[5]
FADEOUT_SPEED = settingUpdate[6]
# If an image is displayed, check if it is time to stop displaying it.
else:
if fadeoutCounter == fadeoutTime:
fadeOut(currentImage)
# Update the current image to the one we loaded wile displaying the current image
currentImage.delete()
currentImage = nextImage
screenIsDark = True
fadeoutTime = math.inf
fadeoutCounter = 0
else:
#update fadeout timer
fadeoutCounter += 1
checkEvents()
pygame.time.delay(wait)
pygame.display.update()
ss_track.append(2)
if params.ss1_track:
ss_track.append(3)
# ss_eshim_x = [-1.763, -1.547, -1.578] #Specify electronic shims (x-dir) for sun sensors
# ss_eshim_y = [-2.290, -2.377, -2.215] #Specify electronic shims (y-dir) for sun sensors
ss_eshim_x = [params.ss1_eshim_x, params.ss2_eshim_x, params.ss3_eshim_x
] #Specify electronic shims (x-dir) for sun sensors
ss_eshim_y = [params.ss1_eshim_y, params.ss2_eshim_y, params.ss3_eshim_y
] #Specify electronic shims (y-dir) for sun sensors
print('eshims_x', ss_eshim_x)
print('eshims_y', ss_eshim_y)
#Establish communication with IMU
imu = IMU(com_port=params.imu_com_port, baudrate=params.imu_baud_rate)
#Establish communication with PTU
ptu_cmd_delay = params.ptu_cmd_delay #0.010
ptu = PTU(com_port=params.ptu_com_port,
baudrate=params.ptu_baud_rate,
cmd_delay=params.ptu_cmd_delay)
#Set latitude, longitude and altitude to Blacksburg, VA for sun pointing
ptu.lat, ptu.lon, ptu.alt = params.ptu_lat, params.ptu_lon, params.ptu_alt #'37.205144','-80.417560', 634
ptu.utc_off = params.ptu_utc_off #4 #Set UTC time offset of EST
#Find the Sun and the moon from your location
# lat,lon,alt='37.205144','-80.417560',634 #Blacksburg
# utc_datetime = datetime.now() #Use current time (can also set to custom datetime= '2018/5/7 16:04:56')
ptu.ephem_point(ep, imu=imu, target='sun', init=False, ptu_cmd=False)
ptu.ephem_point(ep, imu=imu, target='moon', init=False, ptu_cmd=False)
#!/usr/bin/env python from imu import IMU import sys myImu = IMU() chr = sys.stdin.read(1)
# -*- coding: utf-8 -*-
"""
Created on Thu May 31 10:57:21 2018
@author: Bailey group PC
"""
from imu import IMU
import time
import pandas as pd
from datetime import datetime
import os
#Create serial connection to IMU
imu = IMU(com_port='COM7', baudrate=115200) #921600
#Set recording parameters
hz = 20
delay = 1.0 / hz
record_time = 5
save_dir = 'C:/git_repos/GLO/Tutorials/tracking/'
#Initiate pandas dataframe to store IMU data in
data = pd.DataFrame(columns=[
'accel_x', 'accel_y', 'accel_z', 'angr_x', 'angr_y', 'angr_z', 'mag_x',
'mag_y', 'mag_z', 'elapsed'
])
t_start = time.time()
print('Recording VN100 IMU data for', record_time, 'seconds')
while (time.time() - t_start) < record_time:
import time
from remote import AndroidRemote
from motors import Motors
from imu import IMU
from pid import PID
remote_ctl = AndroidRemote()
drone_motors = Motors()
drone_imu = IMU()
pid = PID()
print('start_server:: Ready!')
drone_motors.start()
print('start_motors:: Ok!')
# power = 0
power = {'UR': 30, 'UL': 30, 'BR': 30, 'BL': 30}
t1 = time.time()
try:
while True:
# Get data from android orientation
target_state = remote_ctl.get_data()
# Get data from local sensor
t2 = time.time()
cur_state = drone_imu.get_data(t2 - t1)
# Find difference between actual and target values.
error_state = {}
from imu import IMU
import time
imu = IMU("/dev/ttyUSB0")
integration1_result = 0
integration2_result = 0
def get_depth():
#fast integration
last_time = time.time()
acceleration = imu.get_accz()
acc_diff_time = time.time() -last_time
integration1 = acceleration*acc_diff_time
global integration1_result += integration1
last_time = time.time()
#2nd integration
vel_diff_time = acc_diff_time
integration2 = integration1_result *acc_diff_time
global integration2_result += integration2
depth = integration2_result
return depth
while True:
print("Depth:" +str( get_depth()))
def main():
# read arguments
ap = argparse.ArgumentParser()
ap.add_argument("-s", "--speed")
args = vars(ap.parse_args())
set_speed = args.get('speed', 0)
if set_speed is None:
set_speed = 0
set_speed = float(set_speed)
speed = 0
# inits MCU
mcu = MCU(2222)
# inits IMU
imu = IMU("/dev/ttyUSB_IMU")
# inits CV
cv = CVManager(
0, # choose the first web camera as the source
enable_imshow=True, # enable image show windows
server_port=3333)
cv.add_core("Depth", DepthCore(), True)
# start subprocess
imu.start()
mcu.start()
cv.start()
mcu.wait()
pidR = pidRoll(1, 0.2, 0) # 1, 0 , 0
pidP = pidPitch(0.6, 0, 0) # 5 ,0.1 ,8
pidD = pidDepth(0, 0, 0)
pidY = pidYaw(1, 0.4, 0)
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
try:
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
counter = 0
while True:
counter += 1
if not cv.get_result('Depth')[0] is None:
depth = 150 - cv.get_result('Depth')[0] * 5
else:
depth = 70
pinger = mcu.get_angle()
pitch = imu.get_pitch()
roll = imu.get_roll()
yaw = imu.get_yaw2()
pidR.getSetValues(roll)
pidP.getSetValues(pitch)
pidD.getSetValues(100 - depth)
pidY.getSetValues(-yaw)
finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(),
pidY.start())
sentValues = []
for values in finalPidValues:
subValues = values
sentValues.append(subValues)
motor_fl = sentValues[0]
motor_fr = sentValues[1]
motor_bl = sentValues[2] + set_speed
motor_br = sentValues[3] + set_speed
motor_t = sentValues[4]
mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)
if counter % 5 == 0:
print('Depth:', depth)
print('Pinger:', pinger)
print('Pitch:', pitch)
print('Roll:', roll)
print('Yaw:', imu.get_yaw2())
print('Yaw_sent:', yaw)
print('Motors: %.2f %.2f %.2f %.2f %.2f' %
(motor_fl, motor_fr, motor_bl, motor_br, motor_t))
print()
time.sleep(0.1)
except KeyboardInterrupt:
pass
finally:
print("Stopping remaining threads...")
imu.stop()
mcu.stop()
def __init__(self):
msppg.Parser.__init__(self)
# No communications or arming yet
self.comms = None
self.armed = False
self.gotimu = False
# Do basic Tk initialization
self.root = tk.Tk()
self.root.configure(bg=BACKGROUND_COLOR)
self.root.resizable(False, False)
self.root.title('Hackflight Ground Control Station')
left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
self.root.geometry('%dx%d+%d+%d' %
(DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
self.frame = tk.Frame(self.root)
# Too much hassle on Windows
if 'nt' != os.name:
self.root.tk.call('wm', 'iconphoto', self.root._w,
tk.PhotoImage('icon.xbm'))
self.root.protocol('WM_DELETE_WINDOW', self.quit)
# Create panes for two rows of widgets
self.pane1 = self._add_pane()
self.pane2 = self._add_pane()
# Add a buttons
self.button_connect = self._add_button('Connect', self.pane1,
self._connect_callback)
self.button_imu = self._add_button('IMU', self.pane2,
self._imu_callback)
self.button_motors = self._add_button('Motors', self.pane2,
self._motors_button_callback)
self.button_receiver = self._add_button('Receiver', self.pane2,
self._receiver_button_callback)
#self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
#self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)
# Prepare for adding ports as they are detected by our timer task
self.portsvar = tk.StringVar(self.root)
self.portsmenu = None
self.connected = False
self.ports = []
# Finalize Tk stuff
self.frame.pack()
self.canvas = tk.Canvas(self.root,
width=DISPLAY_WIDTH,
height=DISPLAY_HEIGHT,
background='black')
self.canvas.pack()
# Set up a text label for reporting errors
errmsg = 'No response from board. Possible reasons:\n\n' + \
' * You connected to the wrong port.\n\n' + \
' * Firmware uses serial receiver\n' + \
' (DSMX, SBUS), but receiver is\n' + \
' not connected.'
self.error_label = tk.Label(self.canvas,
text=errmsg,
bg='black',
fg='red',
font=(None, 24),
justify=tk.LEFT)
self.hide(self.error_label)
# Add widgets for motor-testing dialog; hide them immediately
self.motors = Motors(self)
self.motors.stop()
# Create receiver dialog
self.receiver = Receiver(self)
# Create messages dialog
#self.messages = Messages(self)
# Create IMU dialog
self.imu = IMU(self)
self._schedule_connection_task()
# Create a maps dialog
#self.maps = Maps(self, yoffset=-30)
# Create a splash image
self.splashimage = tk.PhotoImage(file=resource_path('splash.gif'))
self._show_splash()
# Create a message parser
#self.parser = msppg.Parser()
# Set up parser's request strings
self.attitude_request = msppg.serialize_ATTITUDE_RADIANS_Request()
self.rc_request = msppg.serialize_RC_NORMAL_Request()
# No messages yet
self.roll_pitch_yaw = [0] * 3
self.rxchannels = [0] * 6
# A hack to support display in IMU dialog
self.active_axis = 0
def main():
# read arguments
ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video",
help="path to the (optional) video file")
ap.add_argument("-c", "--camera",
help="index of camera")
ap.add_argument("-o", "--output",
help="path to save the video")
ap.add_argument("-s", "--speed")
ap.add_argument("-t", "--time")
args = vars(ap.parse_args())
if args.get("video", False):
vs = args.get("video", False)
elif args.get("camera", False):
vs = int(args.get("camera", False))
else:
vs = 0
set_speed = float(args.get('speed', 0))
set_time = float(args.get('time', 0))
print('Speed', set_speed)
print('Time', set_time)
if set_speed is None:
set_speed = 0
set_speed = float(set_speed)
speed = 0
yaw = 0
# inits CV
cv = CVManager(vs, # choose the first web camera as the source
enable_imshow=True, # enable image show windows
server_port=3333, # start stream server at port 3333
delay=5,
outputfolder=args.get("output"))
cv.add_core("GateTracker", GateTrackerV3(), True)
# inits MCU
mcu = MCU(2222)
# inits IMU
imu = IMU("/dev/ttyUSB_IMU")
# start subprocess
cv.start()
imu.start()
mcu.start()
mcu.wait()
time.sleep(3)
start_time = time.time()
depth_speed = 0
pidR = pidRoll(1, 0.2, 0) # 5, 0.1 , 5
pidP = pidPitch(0.6, 0, 0)# 5 ,0.1 ,8
pidD = pidDepth(0, 0, 0)
pidY = pidYaw(1, 0.3, 0)
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
try:
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
counter = 0
last_cv_gate = 0
gate_passed = False
while time.time() - start_time < set_time:
counter += 1
gate, _, gate_size = cv.get_result("GateTracker")
depth = mcu.get_depth()
pinger = mcu.get_angle()
pitch = imu.get_pitch()
roll = imu.get_roll()
if True or gate_passed or gate is None: # just go straight
yaw = imu.get_yaw2(0)
else:
if gate != last_cv_gate:
imu.reset_yaw2(-gate * 0.1, 1)
last_cv_gate = gate
else:
yaw = imu.get_yaw2(1)
if gate_size > 350:
gate_passed = True
if abs(yaw) > 10:
speed = 0
else:
speed = set_speed
if abs((time.time() - start_time) % 5) < 1:
depth_speed = 0.45
else:
depth_speed = 0
pidR.getSetValues(roll)
pidP.getSetValues(pitch)
pidD.getSetValues(70-depth)
pidY.getSetValues(-yaw)
finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(), pidY.start())
sentValues = []
for values in finalPidValues:
subValues = values
sentValues.append(subValues)
motor_fl = sentValues[0] + depth_speed
motor_fr = sentValues[1] + depth_speed
motor_bl = sentValues[2] + set_speed
motor_br = sentValues[3] + set_speed
motor_t = sentValues[4] + depth_speed
# Put control codes here
mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)
if counter % 20 == 0:
print('Gate', gate)
print('GateSize', gate_size)
print('Passed?', gate_passed)
print('Depth:', depth)
print('Pinger:', pinger)
print('Pitch:', pitch)
print('Roll:', roll)
print('Yaw:', imu.get_yaw2())
print('Yaw_sent:', yaw)
print('Motors: %.2f %.2f %.2f %.2f %.2f'%(motor_fl, motor_fr, motor_bl, motor_br, motor_t))
print()
time.sleep(0.1)
except KeyboardInterrupt:
pass
finally:
mcu.set_motors(0, 0, 0, 0, 0)
print("Stopping remaining threads...")
cv.stop()
imu.stop()
mcu.stop()
def main():
# read arguments
ap = argparse.ArgumentParser()
ap.add_argument("-o", "--output")
ap.add_argument("-s", "--speed")
ap.add_argument("-t", "--time") # time to turn after passing
ap.add_argument("-ft",
"--forceturn") # time to force turn after launching, 30
ap.add_argument("-a", "--angle")
ap.add_argument("-d", "--depth") # 80
args = vars(ap.parse_args())
set_speed = float(args.get('speed', 0))
set_depth = float(args.get('depth', 0))
time_after_passing = float(args.get('time', 0))
time_force_turn = float(args.get('forceturn', 0))
angle_to_turn = float(args.get('angle', 0))
# inits CV
cv = CVManager(
0, # choose the first web camera as the source
enable_imshow=True, # enable image show windows
server_port=3333, # start stream server at port 3333
delay=5,
outputfolder=args.get("output"))
cv.add_core("GateTracker", GateTrackerV3(), False)
cv.add_core("Flare", Flare(), False)
# inits MCU
mcu = MCU(2222)
# inits IMU
imu = IMU("/dev/ttyUSB_IMU")
# start subprocess
cv.start()
imu.start()
mcu.start()
imu.reset_yaw2(-angle_to_turn, 2) # for state 3
mcu.wait()
cv.enable_core("GateTracker")
pidR = pidRoll(1, 0.2, 0) # 5, 0.1 , 5
pidP = pidPitch(0.6, 0, 0) # 5 ,0.1 ,8
pidD = pidDepth(1, 0, 0)
pidY = pidYaw(1, 0.3, 0)
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
try:
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
counter = 0
last_cv_gate = 0
yaw = 0
speed = 0
state = 0
timer_for_state0 = time.time()
timer_for_state1 = 0
timer_for_state2 = 0
# 0 -> go find the gate
# 1 -> continue after passing the gate
# 2 -> find flare
# 3 -> surfacing
while True:
counter += 1
if state == 0:
gate, _, gate_size = cv.get_result("GateTracker")
depth = mcu.get_depth()
pitch = imu.get_pitch()
roll = imu.get_roll()
speed = set_speed
if gate is None: # just go straight
yaw = imu.get_yaw2(0) # original heading
else:
if gate != last_cv_gate:
imu.reset_yaw2(-gate * 0.1, 1)
last_cv_gate = gate
else:
yaw = imu.get_yaw2(1) # heading with CV
if gate_size > 350:
state = 1
if time.time() - timer_for_state0 > time_force_turn:
state = 1
print('Gate', gate)
print('GateSize', gate_size)
# go straight
elif state == 1:
if timer_for_state1 == 0: # first time
cv.disable_core('GateTracker')
timer_for_state1 = time.time()
elif time.time() - timer_for_state1 > time_after_passing:
state = 2
cv.enable_core('Flare')
depth = mcu.get_depth()
pitch = imu.get_pitch()
roll = imu.get_roll()
yaw = imu.get_yaw2(0) # original heading
speed = set_speed
# go to the flare
elif state == 2:
gate, _, gate_size = cv.get_result("Flare")
depth = mcu.get_depth()
pitch = imu.get_pitch()
roll = imu.get_roll()
speed = set_speed
print(gate, gate_size)
if gate is None: # just go straight
yaw = imu.get_yaw2(2)
else:
if gate != last_cv_gate:
imu.reset_yaw2(-gate * 0.1, 1)
last_cv_gate = gate
else:
yaw = imu.get_yaw2(1)
if not gate_size is None:
if gate_size > 200:
timer_for_state2 = time.time()
if timer_for_state2 != 0 and time.time(
) - timer_for_state2 > 10:
state = 3
# surfacing
else:
depth = 500
pitch = imu.get_pitch()
roll = imu.get_roll()
yaw = 0
speed = 0
pidR.getSetValues(roll)
pidP.getSetValues(pitch)
pidD.getSetValues(set_depth - depth)
pidY.getSetValues(-yaw)
finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(),
pidY.start())
sentValues = []
for values in finalPidValues:
subValues = values
sentValues.append(subValues)
motor_fl = sentValues[0]
motor_fr = sentValues[1]
motor_bl = sentValues[2] + speed
motor_br = sentValues[3] + speed
motor_t = sentValues[4]
mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)
if counter % 20 == 0:
print('State:', state)
print('Depth:', depth)
print('Pitch:', pitch)
print('Roll:', roll)
print('Yaw:', imu.get_yaw2())
print('Yaw_sent:', yaw)
print('Motors: %.2f %.2f %.2f %.2f %.2f' %
(motor_fl, motor_fr, motor_bl, motor_br, motor_t))
print()
time.sleep(0.1)
except KeyboardInterrupt:
pass
finally:
print("Stopping remaining threads...")
cv.stop()
imu.stop()
mcu.stop()
def __init__(self):
# No communications or arming yet
self.comms = None
self.armed = False
# Do basic Tk initialization
self.root = Tk()
self.root.configure(bg=BACKGROUND_COLOR)
self.root.resizable(False, False)
self.root.title('Hackflight Ground Control Station')
left = (self.root.winfo_screenwidth() - DISPLAY_WIDTH) / 2
top = (self.root.winfo_screenheight() - DISPLAY_HEIGHT) / 2
self.root.geometry('%dx%d+%d+%d' % (DISPLAY_WIDTH, DISPLAY_HEIGHT, left, top))
self.frame = Frame(self.root)
self.root.wm_iconbitmap(bitmap = "@media/icon.xbm")
# Too much hassle on Windows
if 'nt' != os.name:
self.root.tk.call('wm', 'iconphoto', self.root._w, PhotoImage('icon.xbm'))
self.root.protocol('WM_DELETE_WINDOW', self.quit)
# Create panes for two rows of widgets
self.pane1 = self._add_pane()
self.pane2 = self._add_pane()
# Add a buttons
self.button_connect = self._add_button('Connect', self.pane1, self._connect_callback)
self.button_imu = self._add_button('IMU', self.pane2, self._imu_callback)
self.button_motors = self._add_button('Motors', self.pane2, self._motors_button_callback)
self.button_receiver = self._add_button('Receiver', self.pane2, self._receiver_button_callback)
#self.button_messages = self._add_button('Messages', self.pane2, self._messages_button_callback)
#self.button_maps = self._add_button('Maps', self.pane2, self._maps_button_callback, disabled=False)
# Prepare for adding ports as they are detected by our timer task
self.portsvar = StringVar(self.root)
self.portsmenu = None
self.connected = False
self.ports = []
# Finalize Tk stuff
self.frame.pack()
self.canvas = Canvas(self.root, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, background='black')
self.canvas.pack()
# Add widgets for motor-testing dialog; hide them immediately
self.motors = Motors(self)
self.motors.stop()
# Create receiver dialog
self.receiver = Receiver(self)
# Create messages dialog
#self.messages = Messages(self)
# Create IMU dialog
self.imu = IMU(self)
self._schedule_connection_task()
# Create a maps dialog
#self.maps = Maps(self, yoffset=-30)
# Create a splash image
self.splashimage = PhotoImage(file='media/splash.gif')
self._show_splash()
# Create a message parser
self.parser = MSP_Parser()
# Set up parser's request strings
self.attitude_request = serialize_ATTITUDE_Request()
self.rc_request = serialize_RC_Request()
# No messages yet
self.roll_pitch_yaw = 0,0,0
self.rxchannels = 0,0,0,0,0
# A hack to support display in IMU dialog
self.active_axis = 0
from gridmap import GridMap
from imu import IMU
from lidar import Lidar
if __name__ == "__main__":
lidar_data = Lidar("lidar")
imu_data = IMU("imu", "speed")
map = GridMap()
print(lidar_data)
print(imu_data)
print(map)
# lidarData.show_all()
from imu import IMU
i = IMU('/dev/ttyACM4')
print("Connected to IMU")
name = input("Enter a new name: ").rstrip()
i.rename(name)
print("The IMU is now %s" % i.get_identifier())
#!/usr/bin/env python
# dependencies
from flask import Flask, render_template, copy_current_request_context
from flask_socketio import SocketIO, emit
from flask_apscheduler import APScheduler
# local files
from imu import IMU
imu = IMU() # unique instance of IMU
class Config(object):
JOBS = [{
'id': 'run',
'func': imu.run,
'args': (),
'trigger': 'interval',
'seconds': imu.thread_update_delay
}]
SCHEDULER_API_ENABLED = True
SECRET_KEY = 'secret!'
# Set this variable to "threading", "eventlet" or "gevent" to test the
# different async modes, or leave it set to None for the application to choose
# the best option based on installed packages.
async_mode = None # should probably always be gevent
def main():
# read arguments
ap = argparse.ArgumentParser()
ap.add_argument("-s", "--speed")
args = vars(ap.parse_args())
set_speed = args.get('speed', 0)
if set_speed is None:
set_speed = 0
set_speed = float(set_speed)
speed = 0
# inits MCU
mcu = MCU(2222)
# inits IMU
imu = IMU("/dev/ttyUSB_IMU")
# start subprocess
imu.start()
mcu.start()
mcu.wait()
start_time = time.time()
depth_speed = 0
pidR = pidRoll(0.4, 0, 0) # 1, 0 , 0
pidP = pidPitch(0.6, 0, 0) # 5 ,0.1 ,8
pidD = pidDepth(0, 0, 0)
pidY = pidYaw(1, 0.4, 0)
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
try:
motor_fl, motor_fr, motor_bl, motor_br, motor_t = 0, 0, 0, 0, 0
counter = 0
while True:
counter += 1
depth = mcu.get_depth()
pinger = mcu.get_angle()
pitch = imu.get_pitch()
roll = imu.get_roll()
yaw = imu.get_yaw2()
pidR.getSetValues(roll)
pidP.getSetValues(pitch)
pidD.getSetValues(70 - depth)
pidY.getSetValues(-yaw)
finalPidValues = add_list(pidR.start(), pidP.start(), pidD.start(),
pidY.start())
sentValues = []
for values in finalPidValues:
subValues = values
sentValues.append(subValues)
if abs((time.time() - start_time) % 5) < 1:
depth_speed = 0.4
else:
depth_speed = 0
motor_fl = sentValues[0] + depth_speed
motor_fr = sentValues[1] + depth_speed
motor_bl = sentValues[2] + set_speed
motor_br = sentValues[3] + set_speed
motor_t = sentValues[4] + depth_speed
mcu.set_motors(motor_fl, motor_fr, motor_bl, motor_br, motor_t)
if counter % 5 == 0:
print('Depth:', depth)
print('Pinger:', pinger)
print('Pitch:', pitch)
print('Roll:', roll)
print('Yaw:', imu.get_yaw2())
print('Yaw_sent:', yaw)
print('Motors: %.2f %.2f %.2f %.2f %.2f' %
(motor_fl, motor_fr, motor_bl, motor_br, motor_t))
print()
time.sleep(0.1)
except KeyboardInterrupt:
pass
finally:
print("Stopping remaining threads...")
imu.stop()
mcu.stop()
SS(inst_id=2,com_port='COM4',baudrate=115200),
SS(inst_id=3,com_port='COM7',baudrate=115200)]
#List of sun sensors to read data from
ss_read = [2,3]
#List of sun sensors to use for tracking
ss_track = [2,3]
# ss_eshim_x = [-1.763, -1.547, -1.578] #Specify electronic shims (x-dir) for sun sensors
# ss_eshim_y = [-2.290, -2.377, -2.215] #Specify electronic shims (y-dir) for sun sensors
ss_eshim_x = [0.0,0.0,0.0] #Specify electronic shims (x-dir) for sun sensors
ss_eshim_y = [0.0,0.0,0.0] #Specify electronic shims (y-dir) for sun sensors
#Establish communication with IMU
imu=IMU(com_port='COM5',baudrate=115200)
#Establish communication with PTU
ptu_cmd_delay=0.025
ptu = PTU(com_port='COM6',baudrate=9600,cmd_delay=ptu_cmd_delay)
#Set latitude, longitude and altitude to Blacksburg, VA for sun pointing
ptu.lat, ptu.lon, ptu.alt = '37.205144','-80.417560', 634
ptu.utc_off=4 #Set UTC time offset of EST
#Find the Sun and the moon from your location
lat,lon,alt='37.205144','-80.417560',634 #Blacksburg
utc_datetime = datetime.now() #Use current time (can also set to custom datetime= '2018/5/7 16:04:56')
ptu.ephem_point(ep,imu=imu,target='sun',init=False,ptu_cmd=False)
ptu.ephem_point(ep,imu=imu,target='moon',init=False,ptu_cmd=False)
#Define Default Modes
# Finds all locally installed apps that have an external.py
def get_external_hook_paths():
return [
"%s/external" % app.folder_path for app in get_local_apps()
if is_file("%s/external.py" % app.folder_path)
]
def low_power():
ugfx.backlight(0)
ugfx.power_mode(ugfx.POWER_OFF)
ugfx.init()
imu = IMU()
neo = pyb.Neopix(pyb.Pin("PB13"))
neo.display(0x04040404)
ledg = pyb.LED(2)
ival = imu.get_acceleration()
if ival['y'] < 0:
ugfx.orientation(0)
else:
ugfx.orientation(180)
buttons.init()
if not onboard.is_splash_hidden():
splashes = ["splash1.bmp"]
for s in splashes:
ugfx.display_image(0, 0, s)
delay = 2000
IMU positioned on top/center of PTU
Multiple runs are saved sending a step re
"""
from imu import IMU
from ptu_newmark import PTU
import numpy as np
import time
import pandas as pd
import matplotlib.pyplot as plt
import os
cwd = os.getcwd()
ptu = PTU(com_port='COM9', baudrate=9600)
time.sleep(2)
imu = IMU(com_port='COM5', baudrate=921600)
time.sleep(0.3)
#imu.change_baudrate(921600)
#imu.imu.disconnect()
#imu = IMU(com_port='COM7',baudrate=921600)
#time.sleep(0.5)
N = 1000
t0 = time.time()
data = {}
data['date'] = '20180710'
data['samples'] = N
cnt = 0
test_vel = [15, 100, 500, 2500, 15000, 80000]
for run in test_vel:
