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])
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;
