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)
# Set up option parsing to get connection string
import argparse
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_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
# Set up option parsing to get connection string
import argparse
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_T = PIDController(proportional = 0.15, derivative_time = 0, integral_time=0)
pid_R = PIDController(proportional = 0.15, derivative_time =0, integral_time=0)
pid_P = PIDController(proportional = 0.15, derivative_time = 0, integral_time=0)
pid_T.vmin, pid_T.vmax = -0.05, 0.05
pid_R.vmin, pid_R.vmax = -0.5, 0.5
pid_P.vmin, pid_T.vmax = -0.5, 0.5
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
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)
import cv2
import serial
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 = cv2.VideoCapture(0)
val=90
val_y=90
Center_x=0
Center_y=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
#-----------------------------------PID---y------------------------------------------
pid_y = PIDController(proportional = 0.015, derivative_time = 0, integral_time=0)
pid_y.vmin, pid_y.vmax = -10, 10
pid_y.setpoint = 0.0 #aTargetDifference(m)
TDifference_y = pid_y.setpoint
baseAngle_y = 90
pidout_y = 0
while 1:
