def test_motor_control_joysick(self): # Testing joystick control port = '/dev/tty.usbmodem1433401' baudrate = 9600 timeout = 3 arduino = Communication(port, baudrate, timeout) joystick = Joystick() key_q = False t = time.time() while not key_q: key_q = joystick.getJS('R2') data_1 = joystick.getJS('axis2') data_2 = joystick.getJS('axis3') * 45 data_3 = '100' data = ','.join([str(data_1), str(data_2), data_3]) if data: t = time.time() arduino.send(data) if time.time() - t > 20: key_q = True while not arduino.buffer and data: arduino.receive() if arduino.buffer: print(arduino.buffer) arduino.set_buffer(None)
def main(): # Setup logging logger = logging.getLogger('control') logger.setLevel(logging.DEBUG) filehandler = logging.FileHandler('main.log') filehandler.setLevel(logging.DEBUG) console = logging.StreamHandler() console.setLevel(logging.DEBUG) formatter = logging.Formatter( '%(asctime)s - %(name)s - %(levelname)s - %(message)s') filehandler.setFormatter(formatter) console.setFormatter(formatter) logger.addHandler(filehandler) logger.addHandler(console) # Connect to xBee com = Communication(COM_CONNECTION_STRING, 0.1) logger.debug("Connected to wireless communication transceiver") #Wait to receive file logger.debug("Waiting to receive file") f = open("receivedData.txt", "w") waypoints = com.receive() while waypoints != "EndOfFile": #valid data if waypoints != None: #valid data logger.debug(waypoints) #write to file f.write(waypoints) waypoints = com.receive() #time.sleep(1) f.close() # Program end logger.debug("Finished program.") sys.exit(0)
class Capture(): selected_color = (0, 0, 0) width = 0 height = 0 detection_size = 50 detection_delta = 50 etalonnage_top_left = 0 etalonnage_bottom_right = 0 cap, com = (None,)*2 upper_bound, lower_bound = (None,)*2 def __init__(self): self.com = Communication() self.com.start() self.cap = cv2.VideoCapture(1) _, frame = self.cap.read() self.height, self.width, _ = frame.shape self.etalonnage_top_left = (self.width/2 - self.detection_size, self.height / 2 - self.detection_size) self.etalonnage_bottom_right = (self.width/2 + self.detection_size, self.height / 2 + self.detection_size) ### Etalonnage def etalonnage(self): print("--- ETALONNAGE ---") while True: _, frame = self.cap.read() cv2.rectangle(frame, self.etalonnage_top_left, self.etalonnage_bottom_right, (255, 0, 0), 5) # @TODO @IMPROVEMENT Moyenner sur plein d'images detect_blue, detect_green, detect_red, count_pix = (0,)*4 for i in range(self.etalonnage_top_left[0]+1, self.etalonnage_bottom_right[0]): for j in range(self.etalonnage_top_left[1]+1, self.etalonnage_bottom_right[1]): detect_blue += frame[j][i][0] detect_green += frame[j][i][1] detect_red += frame[j][i][2] count_pix += 1 self.selected_color = ( \ detect_blue / count_pix, \ detect_green / count_pix, \ detect_red / count_pix, \ ) self.com.send("ETA "+str(self.selected_color[2])+" "+str(self.selected_color[1])+" "+str(self.selected_color[0])+"\n") if not self.com.receive().empty(): self.calculate_bounds() return self.com.receive().get() def calculate_bounds(self): # define range of blue color in BGR format self.lower_bound = np.array([ \ max(self.selected_color[0] - self.detection_delta, 0), \ max(self.selected_color[1] - self.detection_delta, 0), \ max(self.selected_color[2] - self.detection_delta, 0) \ ], dtype=np.uint8) self.upper_bound = np.array([ \ min(self.selected_color[0] + self.detection_delta, 255), \ min(self.selected_color[1] + self.detection_delta, 255), \ min(self.selected_color[2] + self.detection_delta, 255) \ ], dtype=np.uint8) #### Capture def capture(self): print("--- CAPTURE ---") old_white_pixel_count = 0 old_centroid = (0, 0) while True: _, frame = self.cap.read() mask = cv2.inRange(frame, self.lower_bound, self.upper_bound) white_pixel_count = cv2.countNonZero(mask) moments = cv2.moments(mask, False) try: centroid = (moments['m10']/moments['m00'], moments['m01']/moments['m00']) except: centroid = old_centroid movement_size = white_pixel_count - old_white_pixel_count movement_position = (centroid[0] - old_centroid[0], centroid[1] - old_centroid[1]) data = "CAP "+ \ str(self.width)+" "+ \ str(self.height)+" "+ \ str(white_pixel_count)+" "+ \ str(movement_size)+" "+ \ str(centroid[0])+" "+ \ str(centroid[1])+" "+ \ str(movement_position[0])+" "+ \ str(movement_position[1])+"\n" self.com.send(data) ######### AFTER old_white_pixel_count = white_pixel_count old_centroid = centroid if not self.com.receive().empty(): return self.com.receive().get() def run(self): while True: self.etalonnage() self.capture() def terminate(self): self.com.terminate()
def main(): # Setup logging logger = logging.getLogger('control') logger.setLevel(logging.DEBUG) filehandler = logging.FileHandler('main.log') filehandler.setLevel(logging.DEBUG) console = logging.StreamHandler() console.setLevel(logging.DEBUG) formatter = logging.Formatter( '%(asctime)s - %(name)s - %(levelname)s - %(message)s') filehandler.setFormatter(formatter) console.setFormatter(formatter) logger.addHandler(filehandler) logger.addHandler(console) # Connect to xBee com = Communication(COM_CONNECTION_STRING, 0.1) logger.debug("Connected to wireless communication tansceiver") #Send file logger.debug("Sending coordinates") x1 = 33.175217 y1 = -87.605560 x2 = 33.17541 y2 = -87.6061 x3 = 33.175217 y3 = -87.605560 com.send(x1) com.send(y1) com.send(x2) com.send(y2) com.send(x3) com.send(y3) #file sent, inform the receiver logger.debug("Coordinates sent") com.send("EndOfFile") n_channels = 1 sample_width = 2 framerate = 44100 n_frames = 204800 comp_type = "NONE" comp_name = "not compressed" params = (n_channels, sample_width, framerate, n_frames, comp_type, comp_name) #Wait to receive 1st text file logger.debug("Waiting to receive file") f = open("firstTextFile.txt", "w") waypoints = com.receive() while waypoints != "EndOfFile": #valid data if waypoints != None: #valid data logger.debug(waypoints) #write to file f.write(waypoints) waypoints = com.receive() f.close() f = wave.open("firstAudioFile.wav", "w") f.setparams(params) #Wait to receive 1st audio file logger.debug("Waiting to receive file") audioData = b"" y = 0 z = 0 x = com.read() #iterate over file size #100 frames bytesSent = 0 lastBytesSent = 0 byteNumber = 0 fileSize = n_frames * sample_width * n_channels timeFlag = time.time() while byteNumber < fileSize: #valid data if x != b"": timeFlag = time.time() byteNumber = byteNumber + 1 #valid data bytesSent = bytesSent + 1 if bytesSent >= lastBytesSent + 1024: lastBytesSent = lastBytesSent + 1024 logger.debug(bytesSent) x = com.read() if (timeFlag + 5 < time.time()): logger.debug("Timeout") break f.writeframesraw(audioData) f.close() #Wait to receive 1st text file logger.debug("Waiting to receive file") f = open("secondTextFile.txt", "w") waypoints = com.receive() while waypoints != "EndOfFile": #valid data if waypoints != None: #valid data logger.debug(waypoints) #write to file f.write(waypoints) waypoints = com.receive() f.close() f = wave.open("secondAudioFile.wav", "w") f.setparams(params) #Wait to receive 1st audio file logger.debug("Waiting to receive file") audioData = b"" y = 0 z = 0 x = com.read() #iterate over file size #100 frames bytesSent = 0 lastBytesSent = 0 byteNumber = 0 fileSize = n_frames * sample_width * n_channels timeFlag = time.time() while byteNumber < fileSize: #valid data if x != b"": timeFlag = time.time() byteNumber = byteNumber + 1 #valid data bytesSent = bytesSent + 1 if bytesSent >= lastBytesSent + 1024: lastBytesSent = lastBytesSent + 1024 logger.debug(bytesSent) x = com.read() if (timeFlag + 5 < time.time()): logger.debug("Timeout") break f.writeframesraw(audioData) f.close() # Program end logger.debug("Finished program.") sys.exit(0)
#file.write("running") #file.close() time.sleep(30) print("Connecting to UAV.") UAV = connect(portInformation, wait_ready=True, baud=57600) print(" Autopilot Firmware version: %s" % UAV.version) # wait for radio transmission from ground station UI, store transmitted values into this array # 0-1 are ISU 1 lat/long, 2-3 ISU 2 lat/long, 4-5 gnd station lat/long # these default coordinates are locations around Palmer Park's drone field, and are default locations the drone can go to if communications fails # SAFETY NOTE: DON'T MESS UP YOUR COORDINATES. EVEN AN ERROR IN THE TENTH'S PLACE CAN SEND THE DRONE MILES AWAY GPSCoordinates = [33.175217, -87.605560, 33.175587, -87.605886, 33.175217, -87.605560]; print("receiving waypoints") waypoints = com.receive() coordinateFile = open("GPSCoords.txt", "w+") while waypoints != "EndOfFile": if waypoints is not None: coordinateFile.write(str(waypoints)) coordinateFile.write(" ") waypoints = com.receive() coordinateFile.close(); coords = open("GPSCoords.txt", "r") iteration = 0; for line in coords: for word in line.split(): GPSCoordinates[iteration] = float(word) iteration -= -1
#file sent f.close() #infinitely looping function that checks the motion detector, logs a temperature value if it's been 5 minutes, and checks #the radio for a ping from the drone iteration = 0 logTemperature(logFile) logFile.close() while True: logFile = open("ISULogs.txt", "a+") #radio check here, joe radioCheck = com.receive() if radioCheck == "Requesting ISU1 data": #each box should probably have a different name (ISU1, ISU2) com.send("ISU1 Ready") #ping response print("received ping") time.sleep(1) com.send("SendingTemperatureFile") audioName = 'test' + str(iteration) + '.wav' sendTextFile("/home/pi/ISULogs.txt") #find name of file com.send("EndOfFile") #com.send("SendingAudioFile") sendAudioFile("/home/pi/" + audioName) #still writing this #com.send("EndOfFile") if time.time() - logTime >= 300: logTemperature(logFile) logTime = time.time()