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
