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 test file")
f = open("testFile.txt", "r")
f1 = f.readlines()
for x in f1:
logger.debug(x)
com.send(x)
f.close()
#file sent, inform the receiver
logger.debug("File sent")
com.send("EndOfFile")
# Program end
logger.debug("Finished program.")
sys.exit(0)
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)
class Application:
def __init__(self, solar_panel, serial_port):
self.solar_panel = solar_panel
self.serial_port = serial_port
self.communication = Communication()
def start(self):
print("Application: Start")
button_one = Pin('D0', Pin.IN, Pin.PULL_UP)
while True:
self.button_press_handler(button_one,
self.on_publish_state_button_press)
self.serial_message_handler(self.serial_port,
self.on_client_message_received)
def on_publish_state_button_press(self):
print("SW2 button has been pressed")
self.solar_panel.publish_current_state()
time.sleep_ms(500)
def on_client_message_received(self, message):
self.communication.send(message)
@staticmethod
def button_press_handler(io_pin, press_handler):
if io_pin.value() == 0:
press_handler()
@staticmethod
def serial_message_handler(serial_port, serial_message_handler):
if serial_port.any() > 0:
print('Reading Serial Port')
message = serial_port.read(serial_port.any())
serial_message_handler(message)
def main():
global agent
try:
agent = Optimalagent(Parameters1)
kim = Communication()
kim.run()
kim.runner1()
kim.runner2()
time.sleep(3)
print(kim.state)
state_set = [kim.state]
action_1 = -2.1
action_2 = -2.1
while True:
t1 = time.time()
kim.translate()
agent.state = kim.state
print(agent.state)
msg1 = {"force_x": action_1, "force_y": 0, "force_z": 0}
msg2 = {"force_x": action_2, "force_y": 0, "force_z": 0}
#msg1 = {"force_x": 0, "force_y": 0, "force_z": 0}
#msg2 = {"force_x": 0, "force_y": 0, "force_z": 0}
msg1_json = json.dumps(msg1)
msg2_json = json.dumps(msg2) # pallavi
kim.send(msg1_json)
kim.my_sender(msg2_json) # pallavi
t2 = time.time()
agent.data_append(kim.state, action_1, action_2, t2)
if t2 - t1 - Parameters1.T < 0:
time.sleep(Parameters1.T - t2 + t1)
except KeyboardInterrupt:
np.savetxt("rot_sysid.csv", agent.state_set, delimiter=",")
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 = 23.67
y1 = 0.1823
x2 = 121.213
y2 = 123.123
x3 = 321.321
y3 = 657.765
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")
# 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()
class Robot():
def __init__(self,
lancer_exp=True,
MatCode=False,
db="Points",
defaultPoint="Point0",
setTimer=True):
# Initialisation variables
self.db = filedb.fileDB(db=db)
self.__lastpoint = Point.get_db_point(defaultPoint, self.db)
self.__com = Communication('/dev/ttyACM0')
self.__Oparam = Param()
self.__Oparam.config()
self.__Oparam.calib()
self.__side = Switch.cote()
if not self.__side:
self.__lastpoint.mirror()
self.__move = Move(self.__Oparam.odrv0)
self.__MatCode = MatCode
self.__traj = Trajectoire(param=self.__Oparam,
move=self.__move,
initial_point=self.__lastpoint,
Solo=self.__MatCode)
self.__com.waitEndMove(Communication.MSG["Initialisation"])
if setTimer:
self.__lidar = RPLidar(
'/dev/ttyUSB0') #self.__lidar = Lidar('/dev/ttyUSB0')
self.__timer = RIR_timer(
self.__com, (self.__Oparam, self.__move), self.__lidar,
lancer_exp) # Test: placé avant __init_physical
self.__lidar.start_motor()
self.set_ready()
def set_ready(self):
Switch.tirette()
self.__timer.start_timer()
if self.__MatCode:
self.__traj.solo_launcher() #Mat's code
def move_to(self, point_name, revert=False):
self.__lastpoint = Point.get_db_point(point_name, self.db)
if not self.__side:
self.__lastpoint.mirror()
self.__traj.process(self.__lastpoint, revert)
def action(self, action_name, dist_deploiement=100):
if action_name == "Transport" or action_name == "Palet_Floor_In" or action_name == "Palet_Floor_Out":
self.__com.waitEndMove(Communication.MSG[action_name])
elif action_name == "Arret":
self.__com.send(Communication.MSG[action_name])
elif action_name == "Palet_Wall_In" or action_name == "Palet_Wall_Out":
self.__com.send(Communication.MSG[action_name])
while not self.__com.Avancer:
self.__com.read()
temp_point = self.__lastpoint
if action_name == "Palet_Wall_In":
temp_point.x += dist_deploiement
else:
temp_point.x -= dist_deploiement
self.__traj.process(temp_point)
self.__com.send(Communication.MSG["Action_Finished"])
while not self.__com.Reculer:
self.__com.read()
self.__traj.process(self.__lastpoint)
self.com.send(Communication.MSG["Action_Finished"])
while not self.__com.readyNext:
self.__com.read()
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)
#declares three LGR objects in case they're needed. Default altitude is also declared here; for now it's 30m
alt = 3;
ISU1Location = LocationGlobalRelative(GPSCoordinates[0], GPSCoordinates[1], alt)
ISU2Location = LocationGlobalRelative(GPSCoordinates[2], GPSCoordinates[3], alt)
GNDLocation = LocationGlobalRelative(GPSCoordinates[4], GPSCoordinates[5], alt)
print("assigns locations")
#takes off and sets the homelocation. alt of it is 0, and is what the LGR's base theirs off of
ISU1Log = open("/home/pi/Documents/ISU1Log.txt", "a+")
ISU1Log.truncate(0)
homeLoc = takeoffSequence(alt, UAV)
travel(GPSCoordinates[0],GPSCoordinates[1], alt, UAV)
UAV.mode = VehicleMode("LOITER")
com.send("Requesting ISU1 data")
ISUOne = com.receive()
noneCounter = 0
searchFlag = 0;
timeout = 5
timeFlag = time.time()
foundFlag = 0
breakFlag = True;
while breakFlag:
com.send("Requesting ISU1 data")
ISUOne = com.receive()
while ISUOne != "EndOfFile":
noneCounter -= -1
if noneCounter >= 10:
print(noneCounter)
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()
if GPIO.input(pir): # If PIR pin goes high, motion is detected
