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()
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 = {}
class GCS:
def __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.MSP_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 quit(self):
self.motors.stop()
self.root.destroy()
def hide(self, widget):
widget.place(x=-9999)
def getChannels(self):
return self.rxchannels
def getRollPitchYaw(self):
# configure button to show connected
self._enable_buttons()
self.button_connect['text'] = 'Disconnect'
self._enable_button(self.button_connect)
return self.roll_pitch_yaw
def checkArmed(self):
if self.armed:
self._show_armed(self.root)
self._show_armed(self.pane1)
self._show_armed(self.pane2)
self._disable_button(self.button_motors)
else:
self._show_disarmed(self.root)
self._show_disarmed(self.pane1)
self._show_disarmed(self.pane2)
def scheduleTask(self, delay_msec, task):
self.root.after(delay_msec, task)
def _add_pane(self):
pane = tk.PanedWindow(self.frame, bg=BACKGROUND_COLOR)
pane.pack(fill=tk.BOTH, expand=1)
return pane
def _add_button(self, label, parent, callback, disabled=True):
button = tk.Button(parent, text=label, command=callback)
button.pack(side=tk.LEFT)
button.config(state='disabled' if disabled else 'normal')
return button
# Callback for IMU button
def _imu_callback(self):
self._clear()
self.motors.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.parser.set_ATTITUDE_RADIANS_Handler(self._handle_attitude)
self._send_attitude_request()
self.imu.start()
def _start(self):
self.parser.set_ATTITUDE_RADIANS_Handler(self._handle_attitude)
self._send_attitude_request()
self.imu.start()
self.parser.set_RC_NORMAL_Handler(self._handle_rc)
self.gotimu = False
self.hide(self.error_label)
self.scheduleTask(BOARD_REPLY_DELAY_MSEC, self._checkimu)
def _checkimu(self):
if not self.gotimu:
self.imu.stop()
self.error_label.place(x=50, y=50)
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
# Sends Attitude request to FC
def _send_attitude_request(self):
self.comms.send_request(self.attitude_request)
# Sends RC request to FC
def _send_rc_request(self):
self.comms.send_request(self.rc_request)
# Callback for Motors button
def _motors_button_callback(self):
self._clear()
self.imu.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.motors.start()
def _clear(self):
self.canvas.delete(tk.ALL)
# Callback for Receiver button
def _receiver_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.messages.stop()
#self.maps.stop()
self._send_rc_request()
self.receiver.start()
# Callback for Messages button
def _messages_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.maps.stop()
self.receiver.stop()
self.messages.start()
def _getting_messages(self):
return self.button_connect['text'] == 'Disconnect'
# Callback for Maps button
def _maps_button_callback(self):
self._clear()
if self._getting_messages():
self.receiver.stop()
self.messages.stop()
self.imu.stop()
self.motors.stop()
#self.maps.start()
# Callback for Connect / Disconnect button
def _connect_callback(self):
if self.connected:
self.imu.stop()
#self.maps.stop()
self.motors.stop()
#self.messages.stop()
self.receiver.stop()
if not self.comms is None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self.hide(self.error_label)
self._show_splash()
else:
#self.maps.stop()
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
# Gets available ports
def _getports(self):
allports = comports()
ports = []
for port in allports:
portname = port[0]
if 'usbmodem' in portname or 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
if not portname in ['COM1', 'COM2']:
ports.append(portname)
return ports
# Checks for changes in port status (hot-plugging USB cables)
def _connection_task(self):
ports = self._getports()
if ports != self.ports:
if self.portsmenu is None:
self.portsmenu = tk.OptionMenu(self.pane1, self.portsvar,
*ports)
else:
for port in ports:
self.portsmenu['menu'].add_command(label=port)
self.portsmenu.pack(side=tk.LEFT)
# Disconnected
if ports == []:
self.portsmenu['menu'].delete(0, 'end')
self.portsvar.set('')
self._disable_button(self.button_connect)
self._disable_buttons()
# Connected
else:
self.portsvar.set(ports[0]) # default value
self._enable_button(self.button_connect)
self.ports = ports
self._schedule_connection_task()
# Mutually recursive with above
def _schedule_connection_task(self):
self.root.after(USB_UPDATE_MSEC, self._connection_task)
def _disable_buttons(self):
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
#self._disable_button(self.button_messages)
def _enable_buttons(self):
self._enable_button(self.button_imu)
self._enable_button(self.button_motors)
self._enable_button(self.button_receiver)
#self._enable_button(self.button_messages)
def _enable_button(self, button):
button['state'] = 'normal'
def _disable_button(self, button):
button['state'] = 'disabled'
def sendMotorMessage(self, index, percent):
values = [0] * 4
values[index - 1] = percent / 100.
self.comms.send_message(msppg.serialize_SET_MOTOR_NORMAL, values)
def _show_splash(self):
self.splash = self.canvas.create_image(SPLASH_LOCATION,
image=self.splashimage)
def _hide_splash(self):
self.canvas.delete(self.splash)
def _show_armed(self, widget):
widget.configure(bg='red')
def _show_disarmed(self, widget):
widget.configure(bg=BACKGROUND_COLOR)
if self._getting_messages():
self._enable_button(self.button_motors)
def _handle_calibrate_response(self):
self.imu.showCalibrated()
def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):
# Only handle parms from firmware on a fresh connection
if self.newconnect:
self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)
self.newconnect = False
def _handle_attitude(self, x, y, z):
self.roll_pitch_yaw = x, -y, z
self.gotimu = True
#self.messages.setCurrentMessage('Roll/Pitch/Yaw: %+3.3f %+3.3f %+3.3f' % self.roll_pitch_yaw)
# As soon as we handle the callback from one request, send another request, if IMU dialog is running
if self.imu.running:
self._send_attitude_request()
def _handle_rc(self, c1, c2, c3, c4, c5, c6):
# Display throttle as [0,1], other channels as [-1,+1]
self.rxchannels = c1 / 2. + .5, c2, c3, c4, c5, c6
# As soon as we handle the callback from one request, send another request, if receiver dialog is running
if self.receiver.running:
self._send_rc_request()
#self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))
def _handle_arm_status(self, armed):
self.armed = armed
#self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))
def _handle_battery_status(self, volts, amps):
return
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
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")
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 = args.get('speed', 0)
if set_speed is None:
set_speed = 0
set_speed = float(set_speed)
speed = 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()
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
gate_passed = False
while True:
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 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
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]
motor_fr = sentValues[1]
motor_bl = sentValues[2] + set_speed
motor_br = sentValues[3] + set_speed
motor_t = sentValues[4]
# 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:
print("Stopping remaining threads...")
cv.stop()
imu.stop()
mcu.stop()
ss_track.append(2)
if params.ss3_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)
# Now append a new rotation
pi_2 = math.pi / 2
x_rotations.append(-accelerometer['z'] * pi_2)
z_rotations.append(accelerometer['x'] * pi_2)
# Calculate rotation matrix
return createRotationMatrix(
# this averaging isn't correct in the first <smoothing> frames, but who cares
sum(x_rotations) / smoothing,
math.radians(y_rotation),
sum(z_rotations) / smoothing
)
# Initialise hardware
ugfx.init()
ugfx.clear(ugfx.BLACK)
imu=IMU()
buttons.init()
# Enable tear detection for vsync
ugfx.enable_tear()
tear = pyb.Pin("TEAR", pyb.Pin.IN)
ugfx.set_tear_line(1)
# Set up static rendering matrices
camera_transform = createCameraMatrix(0, 0, -5.0)
proj = createProjectionMatrix(45.0, 100.0, 0.1)
camera_projection = proj * camera_transform
# Get the list of available objects, and load the first one
obj_vertices = []
obj_faces = []
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())
class SerialThread(BaseThread):
ser = serial.Serial()
A = [.0] * 3
G = [.0] * 3
M = [.0] * 3
E_dmp = [.3] * 3
E = [.0] * 3
LA = [.0] * 3
## Constructor
#
# Enables the basic thread functions and logs
# @param self The object pointer
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()
## Open the defined point
#
# Configures the serial port, address and speed, and verifies
# correct initialization and status
# @param self The object pointer
def openPort(self, port='/dev/ttyUSB0', baud=115200):
try:
# configure port
self.ser.port = port
self.ser.baudrate = baud
self.ser.stopbits = serial.STOPBITS_ONE
self.ser.bytesize = serial.EIGHTBITS
self.ser.rtscts = 1
self.ser.timeout = 0.5
# open port
if self.ser.isOpen():
log.w("Port is already open, closing it")
self.ser.close()
self.ser.open()
# clear input
self.ser.flushInput()
# verify that port is open
if self.ser.isOpen():
log.d("Port " + port + " opened at " + str(baud))
except serial.SerialException as se:
log.e("Unable to open port %s" % port)
except:
raise
## The thread process itself
#
# Defines the "loop" method of the thread
# + Obtain self.data from the defined self.serial port
# + Separate in CSV format
# + Assuming the input from a MPU-9150, scales self.data
# + Manage incorrect CSV format (by size)
# + Manage incomplete self.data transfer
# + Notify main thread (Qt4) with a signal
# @param self The objetc pointer
def run(self):
while not self.exiting and self.ser.isOpen():
try:
# check if there is self.data avaliable
if self.ser.inWaiting() > 0:
raw = self.ser.readline().strip()
log.v(raw)
try:
# convert received data to a float list
data = map(float, raw.split(','))
# if got correct data size, scale it
if len(data) == self.csvSize:
for i in range(0, self.csvSize):
data[i] *= self.csvScale[i]
# accelerometer
for i in self.range_accel:
self.A[i - self.range_accel[0]] = data[i]
# gyroscope
for i in self.range_gyro:
self.G[i - self.range_gyro[0]] = data[i]
# magnetometer
for i in self.range_mag:
self.M[i - self.range_mag[0]] = data[i]
# euler if its avaliable
if self.E_dmp[0] != -1:
for i in self.range_euler:
self.E_dmp[i -
self.range_euler[0]] = data[i]
# do sensor fusion
self.fusion.update9DOF(self.G, self.A, self.M)
self.LA = self.fusion.getLinearAcceleration()
self.E = self.fusion.getYawPitchRoll(True)
# notify new data is ready
self.emit(QtCore.SIGNAL('newData()'))
else:
log.d('Incorrect data format')
self.ser.flushInput()
pass
except:
self.ser.flushInput()
log.v("Skipping incomplete transfer")
pass
except:
self.closePort()
log.e("Thread Stopped by unknow error")
return
log.d("Thread Stopped Normally")
return
## Closes the self.serial port and waits for the thread to stop
#
# @param self The objet pointer#
def closePort(self):
log.i("Waiting Thread to stop...")
while self.isRunning():
self.exiting = True
self.exit()
self.ser.close()
log.d("Port closed")
def getAcceleration(self):
return self.A
def getLinearAccel(self):
return self.LA
def getGyroscope(self):
return self.G
def getMagnetometer(self):
return self.M
def getEulerDMP(self):
return self.E_dmp
def getEuler(self):
return self.E
from imu import IMU
i = IMU('/dev/ttyACM3')
print("Connected to IMU")
print("Identifier %s" % i.get_identifier())
class GCS:
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
def quit(self):
self.motors.stop()
self.root.destroy()
def hide(self, widget):
widget.place(x=-9999)
def getChannels(self):
return self.rxchannels
def getRollPitchYaw(self):
# configure button to show connected
self._enable_buttons()
self.button_connect['text'] = 'Disconnect'
self._enable_button(self.button_connect)
return self.roll_pitch_yaw
def checkArmed(self):
if self.armed:
self._show_armed(self.root)
self._show_armed(self.pane1)
self._show_armed(self.pane2)
self._disable_button(self.button_motors)
else:
self._show_disarmed(self.root)
self._show_disarmed(self.pane1)
self._show_disarmed(self.pane2)
def scheduleTask(self, delay_msec, task):
self.root.after(delay_msec, task)
def _add_pane(self):
pane = PanedWindow(self.frame, bg=BACKGROUND_COLOR)
pane.pack(fill=BOTH, expand=1)
return pane
def _add_button(self, label, parent, callback, disabled=True):
button = Button(parent, text=label, command=callback)
button.pack(side=LEFT)
button.config(state='disabled' if disabled else 'normal')
return button
# Callback for IMU button
def _imu_callback(self):
self._clear()
self.motors.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.parser.set_ATTITUDE_Handler(self._handle_attitude)
self._send_attitude_request()
self.imu.start()
def _start(self):
self.parser.set_ATTITUDE_Handler(self._handle_attitude)
self._send_attitude_request()
self.imu.start()
self.parser.set_RC_Handler(self._handle_rc)
# Sends Attitude request to FC
def _send_attitude_request(self):
self.comms.send_request(self.attitude_request)
# Sends RC request to FC
def _send_rc_request(self):
self.comms.send_request(self.rc_request)
# Callback for Motors button
def _motors_button_callback(self):
self._clear()
self.imu.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.motors.start()
def _clear(self):
self.canvas.delete(ALL)
# Callback for Receiver button
def _receiver_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.messages.stop()
#self.maps.stop()
self._send_rc_request()
self.receiver.start()
# Callback for Messages button
def _messages_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.maps.stop()
self.receiver.stop()
self.messages.start()
def _getting_messages(self):
return self.button_connect['text'] == 'Disconnect'
# Callback for Maps button
def _maps_button_callback(self):
self._clear()
if self._getting_messages():
self.receiver.stop()
self.messages.stop()
self.imu.stop()
self.motors.stop()
#self.maps.start()
# Callback for Connect / Disconnect button
def _connect_callback(self):
if self.connected:
self.imu.stop()
#self.maps.stop()
self.motors.stop()
#self.messages.stop()
self.receiver.stop()
if not self.comms is None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self._show_splash()
else:
#self.maps.stop()
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
# Gets available ports
def _getports(self):
allports = comports()
ports = []
for port in allports:
portname = port[0]
if 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
ports.append(portname)
return ports
# Checks for changes in port status (hot-plugging USB cables)
def _connection_task(self):
ports = self._getports()
if ports != self.ports:
if self.portsmenu is None:
self.portsmenu = OptionMenu(self.pane1, self.portsvar, *ports)
else:
for port in ports:
self.portsmenu['menu'].add_command(label=port)
self.portsmenu.pack(side=LEFT)
if ports == []:
self._disable_button(self.button_connect)
self._disable_buttons()
else:
self.portsvar.set(ports[0]) # default value
self._enable_button(self.button_connect)
self.ports = ports
self._schedule_connection_task()
# Mutually recursive with above
def _schedule_connection_task(self):
self.root.after(USB_UPDATE_MSEC, self._connection_task)
def _disable_buttons(self):
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
#self._disable_button(self.button_messages)
def _enable_buttons(self):
self._enable_button(self.button_imu)
self._enable_button(self.button_motors)
self._enable_button(self.button_receiver)
#self._enable_button(self.button_messages)
def _enable_button(self, button):
button['state'] = 'normal'
def _disable_button(self, button):
button['state'] = 'disabled'
def sendMotorMessage(self, index, percent):
values = [PWM_MIN] * 4
values[index - 1] += int(percent / 100. * (PWM_MAX - PWM_MIN))
self.comms.send_message(serialize_SET_MOTOR, values)
def _show_splash(self):
self.splash = self.canvas.create_image((400, 250),
image=self.splashimage)
def _hide_splash(self):
self.canvas.delete(self.splash)
def _show_armed(self, widget):
widget.configure(bg='red')
def _show_disarmed(self, widget):
widget.configure(bg=BACKGROUND_COLOR)
if self._getting_messages():
self._enable_button(self.button_motors)
def _handle_calibrate_response(self):
self.imu.showCalibrated()
def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):
# Only handle parms from firmware on a fresh connection
if self.newconnect:
self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)
self.newconnect = False
def _handle_attitude(self, x, y, z):
self.roll_pitch_yaw = x / 10., -y / 10., z
#self.messages.setCurrentMessage('Roll/Pitch/Yaw: %+3.3f %+3.3f %+3.3f' % self.roll_pitch_yaw)
# As soon as we handle the callback from one request, send another request, if IMU dialog is running
if self.imu.running:
self._send_attitude_request()
def _handle_rc(self, c1, c2, c3, c4, c5, c6, c7, c8):
self.rxchannels = c1, c2, c3, c4, c5
# As soon as we handle the callback from one request, send another request, if receiver dialog is running
if self.receiver.running:
self._send_rc_request()
#self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))
def _handle_arm_status(self, armed):
self.armed = armed
#self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))
def _handle_battery_status(self, volts, amps):
return
class SerialThread(BaseThread):
ser = serial.Serial()
A = [.0]*3
G = [.0]*3
M = [.0]*3
E_dmp = [.3]*3
E = [.0]*3
LA = [.0]*3
## Constructor
#
# Enables the basic thread functions and logs
# @param self The object pointer
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()
## Open the defined point
#
# Configures the serial port, address and speed, and verifies
# correct initialization and status
# @param self The object pointer
def openPort(self, port='/dev/ttyUSB0', baud=115200):
try:
# configure port
self.ser.port = port
self.ser.baudrate = baud
self.ser.stopbits = serial.STOPBITS_ONE
self.ser.bytesize = serial.EIGHTBITS
self.ser.rtscts = 1
self.ser.timeout = 0.5
# open port
if self.ser.isOpen():
log.w("Port is already open, closing it")
self.ser.close()
self.ser.open()
# clear input
self.ser.flushInput()
# verify that port is open
if self.ser.isOpen():
log.d("Port "+port+" opened at "+str(baud))
except serial.SerialException as se:
log.e("Unable to open port %s" % port)
except:
raise
## The thread process itself
#
# Defines the "loop" method of the thread
# + Obtain self.data from the defined self.serial port
# + Separate in CSV format
# + Assuming the input from a MPU-9150, scales self.data
# + Manage incorrect CSV format (by size)
# + Manage incomplete self.data transfer
# + Notify main thread (Qt4) with a signal
# @param self The objetc pointer
def run(self):
while not self.exiting and self.ser.isOpen():
try:
# check if there is self.data avaliable
if self.ser.inWaiting() > 0:
raw = self.ser.readline().strip()
log.v(raw)
try:
# convert received data to a float list
data = map(float, raw.split(','))
# if got correct data size, scale it
if len(data) == self.csvSize:
for i in range(0, self.csvSize):
data[i] *= self.csvScale[i]
# accelerometer
for i in self.range_accel:
self.A[i-self.range_accel[0]] = data[i]
# gyroscope
for i in self.range_gyro:
self.G[i-self.range_gyro[0]] = data[i]
# magnetometer
for i in self.range_mag:
self.M[i-self.range_mag[0]] = data[i]
# euler if its avaliable
if self.E_dmp[0] != -1:
for i in self.range_euler:
self.E_dmp[i-self.range_euler[0]] = data[i]
# do sensor fusion
self.fusion.update9DOF(self.G, self.A, self.M)
self.LA = self.fusion.getLinearAcceleration()
self.E = self.fusion.getYawPitchRoll(True)
# notify new data is ready
self.emit(QtCore.SIGNAL('newData()'))
else:
log.d('Incorrect data format')
self.ser.flushInput()
pass
except:
self.ser.flushInput()
log.v("Skipping incomplete transfer")
pass
except:
self.closePort()
log.e("Thread Stopped by unknow error")
return
log.d("Thread Stopped Normally")
return
## Closes the self.serial port and waits for the thread to stop
#
# @param self The objet pointer#
def closePort(self):
log.i("Waiting Thread to stop...")
while self.isRunning():
self.exiting = True
self.exit()
self.ser.close()
log.d("Port closed")
def getAcceleration(self):
return self.A
def getLinearAccel(self):
return self.LA
def getGyroscope(self):
return self.G
def getMagnetometer(self):
return self.M
def getEulerDMP(self):
return self.E_dmp
def getEuler(self):
return self.E
elif (l > 20):
ugfx.backlight(70)
else:
ugfx.backlight(30)
# 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)
from imu import IMU
i = IMU('/dev/ttyACM4')
print("Connected to IMU")
print("Identifier %s" % i.get_identifier())
#!/usr/bin/env python from imu import IMU import sys myImu = IMU() chr = sys.stdin.read(1)
#!/usr/bin/python
from imu import IMU
from bmp085 import BMP085
import smbus
import math
import serial
port = serial.Serial("/dev/ttyAMA0", baudrate=115200, timeout=3.0)
if __name__ == "__main__":
bus = smbus.SMBus(1)
imu_controller = IMU(bus, 0x69, 0x53, 0x1E, 0x68, "IMU")
counter = 0
# The main loop of IMU
while True:
imu_data = imu_controller.read_all()
# if not (counter%200):
# barometer = BMP085(bus, 0x77, "-barometer")
# pressure = barometer.read_pressure()
# temperature = barometer.read_temperature()
# port.write(str(counter) + "," + str(imu_data) + "," + str(pressure) + "," + str(temperature) + "\r\n")
port.write(str(counter) + "," + str(imu_data) + "\r\n")
counter += 1
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_RADIANS_Request()
self.rc_request = serialize_RC_NORMAL_Request()
# No messages yet
self.roll_pitch_yaw = 0,0,0
self.rxchannels = 0,0,0,0,0,0,0,0
# A hack to support display in IMU dialog
self.active_axis = 0
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()))
from pid import PID
from time import sleep
""" Limits the thrust passed to the motors
in the range (-100,100)
"""
def limitThrust(thrust, upperLimit = 100, lowerLimit = 0):
if thrust > upperLimit:
thrust = upperLimit
elif thrust < lowerLimit:
thrust = lowerLimit
return thrust
#instantiate IMU
#TODO see how to import C interface to python
imu=IMU()
#MyKalman=KalmanFilter(....)
#instantiate motors and put them together in a list
motor1=Motor(1)
motor2=Motor(2)
motor3=Motor(3)
motor4=Motor(4)
motors=[motor1,motor2,motor3,motor4]
# instantiate PID controllers and init them
rollPID=PID(0.9, 0.2, 0.3) # Kp, Kd, Ki
pitchPID=PID(0.9, 0.2, 0.3)
yawPID=PID(0.06, 0.02, 0.01)
#zPID=PID(.....)
#xposPID=PID(.....)
# -*- 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:
justification=ugfx.Label.LEFT,
style=styVal,
parent=c)
c.show()
# Unused, broadcast and multicasts are blocked on the wifi
#s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
#s.bind(('', 14612))
#s.setblocking(True)
#while True:
#data, addr = s.recvfrom(1024)
#curOut.text(data)
imu = IMU()
connectto = usocket.getaddrinfo('monitor1.emf.camp', 80)
oldorientation = ugfx.orientation()
looped = 0
def updateStats():
# Download the JSON file
s = usocket.socket()
try:
s.connect(connectto[0][4])
s.send("GET /api/\r\n")
### author: Laurie James
### description: Goes in a bin. Plays r2d2 noises when the bin shuts. Useful to probably no one but me.
### license: MIT
### reboot-before-run: True
### Appname: R2D2-bin
from http_client import get
import pyb
from imu import IMU
import wifi
TILT_THRESHOLD = -0.4
TILT_PLAY = -0.1
imu = IMU()
host = 'http://192.168.0.12:8001'
if not wifi.nic().is_connected():
wifi.connect(timeout=20)
triggered = False
while(True):
y = imu.get_acceleration()['y']
if(int(y) < TILT_THRESHOLD):
triggered = True
elif(y > TILT_PLAY and triggered):
try:
print('foobar')
get(host, timeout=10).raise_for_status()
from imu import IMU
import pyb
imu = IMU()
while True:
print(imu.get_acceleration())
pyb.delay(1000)
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()
#!/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("-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()
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:
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()
pidR = pidRoll(1, 0.2, 0) # 1, 0 , 0
pidP = pidPitch(0.6, 0, 0) # 5 ,0.1 ,8
pidD = pidDepth(1, 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)
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):
# 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.MSP_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 __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
Ki = float(sys.argv[3])
else:
Kp = 0.9
Kd = 0.2
Ki = 0
############################
#instantiate the BT controller
bt = BT_Controller(z_d)
bt.run()
#instantiate IMU
#TODO see how to import C interface to python
imu = IMU()
#MyKalman=KalmanFilter(....)
# dynamical model instance
dyn_model = Dynamical_Model()
#instantiate motors and put them together in a list
motor1 = Motor(1)
motor2 = Motor(2)
motor3 = Motor(3)
motor4 = Motor(4)
motors = [motor1, motor2, motor3, motor4]
# instantiate PID controllers
rollPID = PID(Kp, Kd, Ki) # Kp, Kd, Ki
pitchPID = PID(Kp, Kd, Ki)
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()
class GCS:
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_RADIANS_Request()
self.rc_request = serialize_RC_NORMAL_Request()
# No messages yet
self.roll_pitch_yaw = 0,0,0
self.rxchannels = 0,0,0,0,0,0,0,0
# A hack to support display in IMU dialog
self.active_axis = 0
def quit(self):
self.motors.stop()
self.root.destroy()
def hide(self, widget):
widget.place(x=-9999)
def getChannels(self):
return self.rxchannels
def getRollPitchYaw(self):
# configure button to show connected
self._enable_buttons()
self.button_connect['text'] = 'Disconnect'
self._enable_button(self.button_connect)
return self.roll_pitch_yaw
def checkArmed(self):
if self.armed:
self._show_armed(self.root)
self._show_armed(self.pane1)
self._show_armed(self.pane2)
self._disable_button(self.button_motors)
else:
self._show_disarmed(self.root)
self._show_disarmed(self.pane1)
self._show_disarmed(self.pane2)
def scheduleTask(self, delay_msec, task):
self.root.after(delay_msec, task)
def _add_pane(self):
pane = PanedWindow(self.frame, bg=BACKGROUND_COLOR)
pane.pack(fill=BOTH, expand=1)
return pane
def _add_button(self, label, parent, callback, disabled=True):
button = Button(parent, text=label, command=callback)
button.pack(side=LEFT)
button.config(state = 'disabled' if disabled else 'normal')
return button
# Callback for IMU button
def _imu_callback(self):
self._clear()
self.motors.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.parser.set_ATTITUDE_RADIANS_Handler(self._handle_attitude)
self._send_attitude_request()
self.imu.start()
def _start(self):
self.parser.set_ATTITUDE_RADIANS_Handler(self._handle_attitude)
self._send_attitude_request()
self.imu.start()
self.parser.set_RC_NORMAL_Handler(self._handle_rc)
# Sends Attitude request to FC
def _send_attitude_request(self):
self.comms.send_request(self.attitude_request)
# Sends RC request to FC
def _send_rc_request(self):
self.comms.send_request(self.rc_request)
# Callback for Motors button
def _motors_button_callback(self):
self._clear()
self.imu.stop()
self.receiver.stop()
#self.messages.stop()
#self.maps.stop()
self.motors.start()
def _clear(self):
self.canvas.delete(ALL)
# Callback for Receiver button
def _receiver_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.messages.stop()
#self.maps.stop()
self._send_rc_request()
self.receiver.start()
# Callback for Messages button
def _messages_button_callback(self):
self._clear()
self.imu.stop()
self.motors.stop()
#self.maps.stop()
self.receiver.stop()
self.messages.start()
def _getting_messages(self):
return self.button_connect['text'] == 'Disconnect'
# Callback for Maps button
def _maps_button_callback(self):
self._clear()
if self._getting_messages():
self.receiver.stop()
self.messages.stop()
self.imu.stop()
self.motors.stop()
#self.maps.start()
# Callback for Connect / Disconnect button
def _connect_callback(self):
if self.connected:
self.imu.stop()
#self.maps.stop()
self.motors.stop()
#self.messages.stop()
self.receiver.stop()
if not self.comms is None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self._show_splash()
else:
#self.maps.stop()
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
# Gets available ports
def _getports(self):
allports = comports()
ports = []
for port in allports:
portname = port[0]
if 'ttyACM' in portname or 'ttyUSB' in portname or 'COM' in portname:
ports.append(portname)
return ports
# Checks for changes in port status (hot-plugging USB cables)
def _connection_task(self):
ports = self._getports()
if ports != self.ports:
if self.portsmenu is None:
self.portsmenu = OptionMenu(self.pane1, self.portsvar, *ports)
else:
for port in ports:
self.portsmenu['menu'].add_command(label=port)
self.portsmenu.pack(side=LEFT)
if ports == []:
self._disable_button(self.button_connect)
self._disable_buttons()
else:
self.portsvar.set(ports[0]) # default value
self._enable_button(self.button_connect)
self.ports = ports
self._schedule_connection_task()
# Mutually recursive with above
def _schedule_connection_task(self):
self.root.after(USB_UPDATE_MSEC, self._connection_task)
def _disable_buttons(self):
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
#self._disable_button(self.button_messages)
def _enable_buttons(self):
self._enable_button(self.button_imu)
self._enable_button(self.button_motors)
self._enable_button(self.button_receiver)
#self._enable_button(self.button_messages)
def _enable_button(self, button):
button['state'] = 'normal'
def _disable_button(self, button):
button['state'] = 'disabled'
def sendMotorMessage(self, index, percent):
values = [0]*4
values[index-1] = percent / 100.
self.comms.send_message(serialize_SET_MOTOR_NORMAL, values)
def _show_splash(self):
self.splash = self.canvas.create_image((400,250), image=self.splashimage)
def _hide_splash(self):
self.canvas.delete(self.splash)
def _show_armed(self, widget):
widget.configure(bg='red')
def _show_disarmed(self, widget):
widget.configure(bg=BACKGROUND_COLOR)
if self._getting_messages():
self._enable_button(self.button_motors)
def _handle_calibrate_response(self):
self.imu.showCalibrated()
def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):
# Only handle parms from firmware on a fresh connection
if self.newconnect:
self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)
self.newconnect = False
def _handle_attitude(self, x, y, z):
print(x, y, z)
self.roll_pitch_yaw = x, -y, z
#self.messages.setCurrentMessage('Roll/Pitch/Yaw: %+3.3f %+3.3f %+3.3f' % self.roll_pitch_yaw)
# As soon as we handle the callback from one request, send another request, if IMU dialog is running
if self.imu.running:
self._send_attitude_request()
def _handle_rc(self, c1, c2, c3, c4, c5, c6, c7, c8):
# Display throttle as [0,1], other channels as [-1,+1]
self.rxchannels = c1/2.+.5, c2, c3, c4, c5
# As soon as we handle the callback from one request, send another request, if receiver dialog is running
if self.receiver.running:
self._send_rc_request()
#self.messages.setCurrentMessage('Receiver: %04d %04d %04d %04d %04d' % (c1, c2, c3, c4, c5))
def _handle_arm_status(self, armed):
self.armed = armed
#self.messages.setCurrentMessage('ArmStatus: %s' % ('ARMED' if armed else 'Disarmed'))
def _handle_battery_status(self, volts, amps):
return
class GCS(MspParser):
def __init__(self):
MspParser.__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)
# 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' +
' * IMU is not responding.\n\n')
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 dialogs; hide them immediately
self.motors_quadxmw = self._add_motor_dialog(MotorsQuadXMW)
self.motors_coaxial = self._add_motor_dialog(MotorsCoaxial)
# Create receiver dialog
self.receiver = Receiver(self)
# Create IMU dialog
self.imu = IMU(self)
self._schedule_connection_task()
# Create a splash image
self.splashimage = tk.PhotoImage(file=resource_path('splash.gif'))
self._show_splash()
# Set up parser's request strings
self.attitude_request = MspParser.serialize_ATTITUDE_Request()
self.rc_request = MspParser.serialize_RC_Request()
# No messages yet
self.roll_pitch_yaw = [0] * 3
self.rxchannels = [0] * 6
def quit(self):
self.motors_quadxmw.stop()
self.motors_coaxial.stop()
self.root.destroy()
def hide(self, widget):
widget.place(x=-9999)
def getChannels(self):
return self.rxchannels
def getRollPitchYaw(self):
# configure button to show connected
self._enable_buttons()
self.button_connect['text'] = 'Disconnect'
self._enable_button(self.button_connect)
return self.roll_pitch_yaw
def checkArmed(self):
if self.armed:
self._show_armed(self.root)
self._show_armed(self.pane1)
self._show_armed(self.pane2)
self._disable_button(self.button_motors)
else:
self._show_disarmed(self.root)
self._show_disarmed(self.pane1)
self._show_disarmed(self.pane2)
def scheduleTask(self, delay_msec, task):
self.root.after(delay_msec, task)
def handle_RC(self, c1, c2, c3, c4, c5, c6):
def norm(x):
MIN = 987
MAX = 2011
return (x - MIN) / (MAX - MIN)
def scale(x):
return 2 * norm(x) - 1
# Scale throttle from [-1,+1] to [0,1]
self.rxchannels = norm(c1), scale(c2), scale(c3), scale(c4), scale(
c5), scale(c6)
# As soon as we handle the callback from one request, send another
# request, if receiver dialog is running
if self.receiver.running:
self._send_rc_request()
def handle_ATTITUDE(self, angx, angy, heading):
self.roll_pitch_yaw = rad(angx / 10), -rad(angy / 10), rad(heading)
self.gotimu = True
# As soon as we handle the callback from one request, send another
# request, if receiver dialog is running
if self.imu.running:
self._send_attitude_request()
def _add_pane(self):
pane = tk.PanedWindow(self.frame, bg=BACKGROUND_COLOR)
pane.pack(fill=tk.BOTH, expand=1)
return pane
def _add_button(self, label, parent, callback, disabled=True):
button = tk.Button(parent, text=label, command=callback)
button.pack(side=tk.LEFT)
button.config(state=('disabled' if disabled else 'normal'))
return button
def _add_motor_dialog(self, dialog):
d = dialog(self)
d.stop()
return d
# unsigned int => signed float conversion
def _float(self, intval):
return intval / 1000 - 2
# Callback for IMU button
def _imu_callback(self):
self._clear()
self.motors_quadxmw.stop()
self.motors_coaxial.stop()
self.receiver.stop()
self._send_attitude_request()
self.imu.start()
def _start(self):
self._send_attitude_request()
self.imu.start()
self.gotimu = False
self.hide(self.error_label)
self.scheduleTask(CONNECTION_DELAY_MSEC, self._checkimu)
def _checkimu(self):
if not self.gotimu:
self.imu.stop()
self.error_label.place(x=50, y=50)
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
# Sends state request to FC
def _send_attitude_request(self):
self.comms.send_request(self.attitude_request)
# Sends RC request to FC
def _send_rc_request(self):
self.comms.send_request(self.rc_request)
# Callback for Motors button
def _motors_button_callback(self):
self._clear()
# self.comms.send_request(self.actuator_type_request)
self.imu.stop()
self.receiver.stop()
self.motors_quadxmw.start()
def _clear(self):
self.canvas.delete(tk.ALL)
# Callback for Receiver button
def _receiver_button_callback(self):
self._clear()
self.imu.stop()
self.motors_quadxmw.stop()
self.motors_coaxial.stop()
self._send_rc_request()
self.receiver.start()
# Callback for Connect / Disconnect button
def _connect_callback(self):
if self.connected:
self.imu.stop()
self.motors_quadxmw.stop()
self.motors_coaxial.stop()
self.receiver.stop()
if self.comms is not None:
self.comms.stop()
self._clear()
self._disable_buttons()
self.button_connect['text'] = 'Connect'
self.hide(self.error_label)
self._show_splash()
else:
self.comms = Comms(self)
self.comms.start()
self.button_connect['text'] = 'Connecting ...'
self._disable_button(self.button_connect)
self._hide_splash()
self.scheduleTask(CONNECTION_DELAY_MSEC, self._start)
self.connected = not self.connected
# Gets available ports
def _getports(self):
allports = comports()
ports = []
for port in allports:
portname = port[0]
if any(name in portname
for name in ('usbmodem', 'ttyACM', 'ttyUSB', 'COM')):
if portname not in ('COM1', 'COM2'):
ports.append(portname)
return ports
# Checks for changes in port status (hot-plugging USB cables)
def _connection_task(self):
ports = self._getports()
if ports != self.ports:
if self.portsmenu is None:
self.portsmenu = tk.OptionMenu(self.pane1, self.portsvar,
*ports)
else:
for port in ports:
self.portsmenu['menu'].add_command(label=port)
self.portsmenu.pack(side=tk.LEFT)
# Disconnected
if ports == []:
self.portsmenu['menu'].delete(0, 'end')
self.portsvar.set('')
self._disable_button(self.button_connect)
self._disable_buttons()
# Connected
else:
self.portsvar.set(ports[0]) # default value
self._enable_button(self.button_connect)
self.ports = ports
self._schedule_connection_task()
# Mutually recursive with above
def _schedule_connection_task(self):
self.root.after(USB_UPDATE_MSEC, self._connection_task)
def _disable_buttons(self):
self._disable_button(self.button_imu)
self._disable_button(self.button_motors)
self._disable_button(self.button_receiver)
def _enable_buttons(self):
self._enable_button(self.button_imu)
self._enable_button(self.button_motors)
self._enable_button(self.button_receiver)
def _enable_button(self, button):
button['state'] = 'normal'
def _disable_button(self, button):
button['state'] = 'disabled'
def sendMotorMessage(self, motors):
if self.comms is not None: # Guard at startup
self.comms.send_message(
MspParser.serialize_SET_MOTOR,
(motors[0], motors[1], motors[2], motors[3]))
def _show_splash(self):
self.splash = self.canvas.create_image(SPLASH_LOCATION,
image=self.splashimage)
def _hide_splash(self):
self.canvas.delete(self.splash)
def _show_armed(self, widget):
widget.configure(bg='red')
def _show_disarmed(self, widget):
widget.configure(bg=BACKGROUND_COLOR)
def _handle_calibrate_response(self):
self.imu.showCalibrated()
def _handle_params_response(self, pitchroll_kp_percent, yaw_kp_percent):
# Only handle parms from firmware on a fresh connection
if self.newconnect:
self.imu.setParams(pitchroll_kp_percent, yaw_kp_percent)
self.newconnect = False
def _handle_arm_status(self, armed):
self.armed = armed
def _handle_battery_status(self, volts, amps):
return
from imu import IMU
import pyb
imu = IMU()
while True:
print(imu.get_acceleration())
pyb.delay(1000);
class Bike():
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
# record parameters at the top of the log
def balance(self):
x, y, self.tilt = self.imu.get_acceleration()
self.goal = int(self.tilt_constant * self.tilt)
self.response = 'Goal: ' + str(self.goal)
self.try_step(self.goal - self.pos)
time.sleep(0.005)
def balance_initial(self):
t = time.time() - self.t0
x, y, self.tilt = self.imu.get_acceleration()
if tilt < -self.threshold:
self.response = 'leaning left'
self.try_step(self.steps)
elif tilt > self.threshold:
self.response = 'leaning right'
self.try_step(-self.steps)
elif self.pos > 0:
self.response = 'recentering right'
self.try_step(-self.steps)
elif self.pos < 0:
self.response = 'recentering left'
self.try_step(self.steps)
if self.log:
data = [
t,
x,
y,
self.pos,
self.tilt,
self.response,
]
with open(self.log, 'a') as f:
f.write(",".join(data) + "\n")
def try_step(self, steps):
if abs(self.pos + steps) <= self.max_steps:
self.pos += steps
self.mtr.step(steps)
def updateFPS(self):
self.fps.update(verbose=True)
def cleanup(self):
self.mtr.cleanup()
from imu import IMU
import sys, time
wantsToQuit = False
if len(sys.argv) == 1:
print("No devices defined")
sys.exit(255)
while not wantsToQuit:
i = IMU(sys.argv[1])
print("Connected to IMU")
print("Waiting for the IMU to wake up")
#time.sleep(5)
print("Calibrating the accelerometer")
if (i.calibrate_accelerometer()):
print(" - successful")
print("Calibrating the magnometer")
if (i.calibrate_magnometer()):
print(" - successful")
name = input("Enter a new name: ").rstrip()
i.rename(name)
print("The IMU is now %s" % i.get_identifier())
i.close()
def loop():
system('clear')
IMU.printData()
return
elif (l > 20):
ugfx.backlight(70)
else:
ugfx.backlight(30)
# 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)
import time
from imu import IMU
from pid import PID
from radio import Radio
from servos import Servos
# r = Radio('/dev/ttyUSB0')
# while True:
# if r.bytes_available() > 16:
# data = r.read()
# print(data.decode())
# r.close()
if __name__ == '__main__':
imu = IMU()
servos = Servos()
pid_roll = PID(600., 0., 0., control_range=[-250, 250])
pid_pitch = PID(-600., 0., 0., control_range=[-250, 250])
pid_yaw = PID(10., 0., 0)
pids = [pid_roll, pid_pitch, pid_yaw]
rpy = np.zeros(3)
sp_rpy = np.zeros(3)
cmd = np.array([1000, 1000, 1500, 1500])
throttle = 1100
tt = t = time.time()
running = True
