#!/usr/bin/env python # -*- coding: utf-8 -*- from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import cv2 #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0 pos_bound_err = 300 yaw_bound_err = 0.5 yaw = 0
#!/usr/bin/env python # -*- coding: utf-8 -*- from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import Adafruit_BBIO.ADC as ADC import logging ############################### #Setup of vidro and controller# ############################### vidro = Vidro(False, 1) flight_ready = vidro.connect_vicon() vidro.connect_mavlink() controller = PositionController(vidro) start_time = time.time() #Load gains controller.update_gains() #################### # Controller Values# #################### take_off_alt = 1000 # Pos Commands
if line > 22 or line < 0: return #Print to screen using curses if col == 0: screen.addstr(line, 0, string) if col == 1: screen.addstr(line, 40, string) screen.refresh() logging.basicConfig(filename='flight_test.log', level=logging.DEBUG) #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) #Initalization of curses screen = curses.initscr() screen.clear() screen.refresh() plot.ion() switch = False timer = time.time() while (vidro.current_rc_channels[4] > 1600) and (flight_ready == True):
#image_data = [cx ,cy, area, min_con_x, max_con_x, min_con_y, max_con_y, out_of_bounds, num_objects] image_data = [cx,cy,area_max,num_objects] return image_data def get_camera_frame(): global frame global new_frame _, frame = cap.read() frame = cv2.flip(frame,-1,-1) new_frame = True ############################### #Setup of vidro and controller# ############################### vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() # Load gains controller.update_gains() #################### # Controller Values# #################### take_off_alt = 1000 # Pos Commands x_com = 0 y_com = 0 alt_com = 0
# -*- coding: utf-8 -*-
from vidro import Vidro, ViconStreamer
from position_controller import PositionController
import sys, math, time
import socket, struct, threading
import numpy as np
import cv2
import logging
#Start of a log
logging.basicConfig(filename='bft_log.log', level=logging.INFO)
logging.info('Starting')
#Setup of vidro and controller
vidro = Vidro(False, 1)
flight_ready = vidro.connect()
controller = PositionController(vidro)
# Start Time for Logging
global start_time
start_time = time.time()
cycle = 0
controller.update_gains()
sequence = 0
seq0_cnt = 0
seq1_cnt = 0
seq2_cnt = 0
seq3_cnt = 0
cy = 0 cv2.circle(frame,(cx,cy),5,(255,0,0),-1) #print 'Num Objs: ',repr(num_objects),' Cx: ',repr(cx),' Cy: ',repr(cy),' Area: ',repr(area_max) #image_data = [cx ,cy, area, min_con_x, max_con_x, min_con_y, max_con_y, out_of_bounds, num_objects] image_data = [cx,cy,area_max,num_objects] return image_data def get_camera_frame(): global frame global new_frame _, frame = cap.read() frame = cv2.flip(frame,-1,-1) new_frame = True #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0
""" Simple test for connecting to the vidro class. Some things like time may not be working """ from vidro import Vidro, ViconStreamer import sys, math, time import socket, struct, threading import curses import utm import matplotlib.pyplot as plot vidro = Vidro(True, 115200,"127.0.0.1:14551") vidro.connect() cycles_message = 0 cycles_change = 0 previous_time = 0 previous_rc = 0 while vidro.current_rc_channels[4] > 1500: while vidro.current_rc_channels[2] < 1800: print str(vidro.current_rc_channels[2]) + " " + str(vidro.rc_msg_time) + " " + str(cycles_message) + " " + str(cycles_change) vidro.set_rc_throttle(vidro.current_rc_channels[2]+10) vidro.get_mavlink() time.sleep(.01) if vidro.rc_msg_time < previous_time: cycles_message = 0 if vidro.current_rc_channels[2] != previous_rc: cycles_change = 0 cycles_change += 1
#Check for bad inputs
if col > 1 or col < 0:
return
if line > 22 or line < 0:
return
#Print to screen using curses
if col == 0:
screen.addstr(line, 0, string)
if col == 1:
screen.addstr(line, 40, string)
screen.refresh()
vidro = Vidro(False, 1)
vidro.connect()
controller = PositionController(vidro)
screen = curses.initscr()
screen.clear()
screen.refresh()
while_check = 1
while True:
while_check=1
curses_print("Roll: " + str(vidro.current_rc_channels[0]),0,0)
curses_print("Pitch: " + str(vidro.current_rc_channels[1]),1,0)
curses_print("Throttle: " + str(vidro.current_rc_channels[2]),2,0)
curses_print("Yaw: " + str(vidro.current_rc_channels[3]),3,0)
curses_print("Quad Arm: " + str(vidro.current_rc_channels[4]),4,0)
#!/usr/bin/env python # -*- coding: utf-8 -*- from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import cv2 #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0 yaw = 0
#!/usr/bin/env python # -*- coding: utf-8 -*- from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import cv2 #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0 pos_bound_err = 100 yaw_bound_err = 0.2 yaw = 0
#image_data = [cx ,cy, area, min_con_x, max_con_x, min_con_y, max_con_y, out_of_bounds, num_objects] image_data = [cx,cy,area_max,num_objects] return image_data def get_camera_frame(): global frame global new_frame _, frame = cap.read() frame = cv2.flip(frame,-1,-1) new_frame = True ############################### #Setup of vidro and controller# ############################### vidro = Vidro(False, 1) #flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() # Load gains controller.update_gains() #################### # Controller Values# #################### take_off_alt = 1000 # Pos Commands x_com = 0 y_com = 0 alt_com = 0
