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()
