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;