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)