parser = argparse.ArgumentParser( description='Control Copter and send commands in GUIDED mode ') parser.add_argument( '--connect', help= "Vehicle connection target string. If not specified, SITL automatically started and used." ) args = parser.parse_args() connection_string = args.connect sitl = None #PID pid = PIDController(proportional=0.15, derivative_time=0, integral_time=0) pid.vmin, pid.vmax = -0.05, 0.05 pid.setpoint = 1, 5 #aTargetAltitude(m) TAltitude = pid.setpoint baseVelocity = 0.5 pidout = 0 # Start SITL if no connection string specified if not connection_string: import dronekit_sitl sitl = dronekit_sitl.start_default() connection_string = sitl.connection_string() # Connect to the Vehicle print('Connecting to vehicle on: %s' % connection_string) vehicle = connect(connection_string, wait_ready=True)
parser.add_argument('--connect', help="Vehicle connection target string. If not specified, SITL automatically started and used.") args = parser.parse_args() connection_string = args.connect sitl = None #PID pid_T = PIDController(proportional = 0.15, derivative_time = 10, integral_time=0) pid_R = PIDController(proportional = 0.15, derivative_time = 10, integral_time=0) pid_P = PIDController(proportional = 0.15, derivative_time = 10, integral_time=0) pid_T.vmin, pid_T.vmax = -0.05, 0.05 pid_R.vmin, pid_R.vmax = -1, 1 pid_P.vmin, pid_T.vmax = -1, 1 pid_T.setpoint = 1.0 #aTargetAltitude(m) TAltitude = pid_T.setpoint pid_R.setpoint=0 #distance between centre of line and centre of frame TRoll=pid_R.setpoint pid_P.setpoint=0 #distance between centre of line and centre of frame TPitch=pid_P.setpoint baseThrust = 0.55 baseRoll = 0 basePitch = 0 pidout_T = 0 pidout_R = 0 pidout_P = 0 # Start SITL if no connection string specified if not connection_string:
import imutils from PID import PIDController #ser = serial.Serial('/dev/ttyACM0',9600) # multiple cascades: https://github.com/Itseez/opencv/tree/master/data/haarcascades #https://github.com/Itseez/opencv/blob/master/data/haarcascades/haarcascade_frontalface_default.xml face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml') #cap = WebcamVideoStream(src=0).start() cap = cv2.VideoCapture(0) val=90 Center_x=0 #-----------------------------------PID------------------------------------------ pid = PIDController(proportional = 0.015, derivative_time = 0, integral_time=0) pid.vmin, pid.vmax = -10, 10 pid.setpoint = 0.0 #aTargetDifference(m) TDifference = pid.setpoint baseAngle = 90 pidout = 0 i=0 while 1: i+=1 center_x=[] center_y=[] ret,img = cap.read() img = cv2.resize(img, (120, 140)) if i%1 ==0: gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) faces = face_cascade.detectMultiScale(gray, 1.3, 5) CENTER=img.shape[0]/2
parser = argparse.ArgumentParser( description='Control Copter and send commands in GUIDED mode ') parser.add_argument( '--connect', help= "Vehicle connection target string. If not specified, SITL automatically started and used." ) args = parser.parse_args() connection_string = args.connect sitl = None #PID pid = PIDController(proportional=0.15, derivative_time=0, integral_time=0) pid.vmin, pid.vmax = -0.05, 0.05 pid.setpoint = 2.0 #aTargetAltitude(m) TAltitude = pid.setpoint baseVelocity = 0.5 pidout = 0 # Start SITL if no connection string specified if not connection_string: import dronekit_sitl sitl = dronekit_sitl.start_default() connection_string = sitl.connection_string() # Connect to the Vehicle print('Connecting to vehicle on: %s' % connection_string) vehicle = connect(connection_string, wait_ready=True)
# greenLower = (20, 139, 21) # greenUpper = (35, 255, 255) # greenLower = (20, 117, 166) # greenUpper = (99, 189, 255) # greenLower = (29, 88, 161) # greenUpper = (47, 255, 255) greenLower = (27, 76, 98) greenUpper = (87, 255, 255) camera = cv2.VideoCapture(2) camera.set(cv2.CAP_PROP_FPS, 60) pid = PIDController(proportional=25.0, derivative_time=1, integral_time=100) pid.vmin, pid.vmax = -10000, 10000 pid.setpoint = 600 baseThrust = 43000 rollTrim = 1 pitchTrim = 1.5 yawTrim = 0 cflib.crtp.init_drivers(True) r = RadioDriver() # cf_urls = r.scan_interface(None) # print(cf_urls) # uri = cf_urls[0][0] cf = Crazyflie() cf.open_link("radio://0/90/250K") cf.commander.send_setpoint(0, 0, 0, 0) pidout = 0