def getPaperPoints():
images = laserTogglePictures();
img = cv2.imdecode(images[1], 1);
cv2.imwrite("crnrs.png",img);
#pointList = DroneUtils.getPaperByCorner(img.tostring('c'), img.shape[1], img.shape[0]);
pointList = DroneUtils.paperContour(img.tostring('c'), img.shape[1], img.shape[0]);
return pointList;
def getCurDist():
images = toggleLaserPictures();
img1 = cv2.imdecode(images[0],1);
#img2 = cv2.imdecode(images[1], 1);
cv2.imwrite("ta.png", img1);
#cv2.imwrite("tb.png", img2);
dist = DroneUtils.getLaserDistOneImg(img1.tostring('c'), img1.shape[1], img1.shape[0],255.405, 922.538);
return dist;
def findBox():
camera = PiCamera()
camera.resolution = (2592,1944)
camera.brightness = 75
stream = io.BytesIO()
camera.capture(stream, format='png')
data = np.fromstring(stream.getvalue(), dtype=np.uint8)
image = cv2.imdecode(data,1)
stuff = DroneUtils.getAllBoxes(image.tostring('c'), image.shape[1], image.shape[0])
for box in stuff:
cv2.rectangle(image, (box[0], box[1]), (box[0]+box[2], box[1]+box[3]), (255,255,0),5)
cv2.imwrite('FindaBox.png', image)
camera.close()
cv2.destroyAllWindows()
return stuff
def getBoxCorners():
camera = PiCamera();
camera.resolution = (2592,1944)
stream = io.BytesIO()
camera.capture(stream,format='png')
data = np.fromstring(stream.getvalue(),dtype=np.uint8)
image = cv2.imdecode(data,1)
cv2.imshow("Image",image)
cv2.waitKey(10)
lst = DroneUtils.getBoxCorner(image.tostring('c'),image.shape[1],image.shape[0])
for i in lst:
for x in range(len(i)/2):
print(str(i[2*x]) + " " + str(i[2*x+1]))
cv2.imwrite("output.png",image)
camera.close()
cv2.destroyAllWindows()
print(lst)
def calibrate():
print("wat")
startDist = 10
ptsLst = []
img = getOnePic()
distance= getPaperDistWithDat(img)
laserPt = getLaserPosWithDat(img)
apndLst = [distance, laserPt[1]]
ptsLst.append(apndLst)
ser.write('mf0001');
time.sleep(2)
print("ALL OF MY WAT")
for i in xrange(3):
img = getOnePic()
distance = getPaperDistWithDat(img)
lasarPt = getLaserPosWithDat(img)
apnd = [distance, lasarPt[1]]
ser.write('mf0001')
ptsLst.append(apnd)
time.sleep(2)
print(ptsLst)
consts = DroneUtils.getLaserConstants(ptsLst)
print(consts[0])
print(consts[1])
#!usr/bin/python
import numpy as np
import time
import cv2
import DroneUtils
image = cv2.imread("17foot_BothBoxes.png");
print(DroneUtils.getBoxCorner(image.tostring('c'),image.shape[1],image.shape[0]));
#cv2.resize(image,(1000,1000))
#cv2.imshow("",image);
#cv2.waitKey(1)
cv2.destroyAllWindows()
#!usr/bin/python
import cv2
import numpy as np
import serial
import time
import io
import DroneUtils
ser = serial.Serial("/dev/ttyACM0")
img2 = cv2.imread("laser10b.png")
img = cv2.imread("laser10bwithout.png")
dist = DroneUtils.getLaserDist(img.tostring("c"), img2.tostring("c"), 1080, 1920)
print "%d" % dist
def sendCmd(str):
#---------------------------INIT CONTORLS---------------------------------------
#Arm the drone
if cmd == 'r':
# print("This line is commented out so the software devs don't accidently kill anyone, soooo you should fix that")
ser.write('ia0000');
#Disarm the drone
if cmd == 't':
ser.write('ik0000');
#Set alt
if cmd == 'h':
ser.write('is0000');
#Run test
if cmd == 'g':
# print("This line is commented out so the software devs don't accidently kill anyone, soooo you should fix that")
ser.write('it0000');
#---------------------------DRONE MOVEMENT CONTROLS------------------------------
#Move the drone up
if cmd == 'u':
ser.write('mu2012'); #THRMAX
#Move the drone down
if cmd == 'o':
ser.write('md1029'); #THRMIN
#Move the drone forward
if cmd == 'i':
ser.write('mf2300'); #PIT60
#Move the drone right
if cmd == 'm':
ser.write('mr1600'); #ROLL60
#Move the drone backwards
if cmd == 'k':
ser.write('mb1400'); #PIT40
#Move the drone left
if cmd == 'n':
ser.write('ml1400'); #ROLL40
#Turn the drone left
if cmd == 'j':
ser.write('tl1428'); #YAW40
#Turn the drone right (YAW60)
if cmd == 'l':
ser.write('tr1628'); #YAW60
#--------------------------SERVO CONTROLS----------------------------------------
#Move the horizontal servo to the left.. hard coded for 5 degrees currently
if cmd == 'a':
ser.write('sl0005');
#Move the vertical servo down.. hard coded for 5 degrees currently
if cmd == 's':
ser.write('sd0005');
#Move the horizontal servo to the right.. hard coded for 5 degrees currently
if cmd == 'd':
ser.write('sr0005');
#Move the vertical servo up.. harde coded for 5 degrees currently
if cmd == 'w':
ser.write('su0005');
#Reset the servos positions
if cmd == 'x':
ser.write('sx0000');
#-------------------------LASER CONTROLS-------------------------------------------
#Toggle the laser on/off
if cmd == 'f':
GPIO.output(7, 1);
# ser.write('l00000');
if cmd == 'p':
GPIO.output(7, 0);
# ser.write('100000');
#------------------------QUIT CONTROLS---------------------------------------------
#Get the distance to the laser (alt)
if cmd == 'z':
#Set Servos to 90:90
ser.write('sx0000');
#Move vertical servo down 35Deg
ser.write('sd0090');
#Set MID values for motors.
ser.write('it0000');
#wait for the drone to be above 5 feet.
d = getCurDist();
safety = 0;
#while (d < 4) or (safety > 4):
# d = getCurDist();
# safety += 1;
#Disarm the Drone
ser.write('ik0000');
print d
#Get the distance to the laser (far)
if cmd == 'v':
ser.write('sx0000');
d = getCurDist();
print d
if cmd == 'q':
getLaserPoint()
#Calibrate lzr
if cmd == 'c':
print "Enter dist to wall:"
wallDist = raw_input("")
print "Enter y-coord of lzr:"
lzrY = raw_input("")
pt = [float(wallDist), float(lzrY)]
list = []
list.append(pt)
DroneUtils.getLaserConstants(list)
#Quit the program
if cmd == 'quit':
ser.close();
sys.exit(0);
else :
return;
def getCurDist():
imgs = toggleLaserPictures();
img1 = cv2.imdecode(imgs[0], 1);
cv2.imwrite("RangePic.png", img1);
dist = DroneUtils.getLaserDistOneImg(img1.tostring('c'), img1.shape[1], img1.shape[0],241.978,1287);
return dist;
def getLaserPoint():
imgs = toggleLaserPictures()
img1 = cv2.imdecode(imgs[0],1)
cv2.imwrite("DroneLASAR.png",img1)
lst = DroneUtils.getLaserDistOneImgLoc(img1.tostring('c'), img1.shape[1], img1.shape[0])
print(lst[0],lst[1])
def getLaserPos():
images = toggleLaserPictures();
img1 = cv2.imdecode(images[0], 1);
cv2.imwrite('test.png',img1)
pos = DroneUtils.getLaserDistOneImgLoc(img1.tostring('c'), img1.shape[1], img1.shape[0]);
return pos;
def getLaserPosWithDat(image):
img1 = cv2.imdecode(image, 1);
pos = DroneUtils.getLaserDistOneImgLoc(img1.tostring('c'), img1.shape[1], img1.shape[0]);
return pos;
def getPaperDistWithDat(image):
img = cv2.imdecode(image,1)
dist = DroneUtils.getPaperDistContour(img.tostring('c'), img.shape[1], img.shape[0]);
return dist;
def getPaperDist():
images = laserOnOffPictures();
img = cv2.imdecode(images[0], 1);
#cv2.imwrite("t1a.png", img);
dist = DroneUtils.getPaperDistContour(img.tostring('c'), img.shape[1], img.shape[0]);
return dist;
def getCurDist():
images = toggleLaserPictures()
img1 = cv2.imdecode(images[0], 1)
dist = DroneUtils.getLaserDistOneImg(img1.tostring('c'), img1.shape[1], img1.shape[0], 255.405, 922.538)
return dist
