def __init__(self): ''' direction: if direction is 1 then the robot drives in the direction of its sensor head ''' self.mode = 1 #mapping mode self.firstCell = True direction = 1 self.left = not direction self.right = direction self.front = 2 setPoint = 14.9 cmMaxPid = 35 cmMaxWallChecker = 26 cmMin = 5 self.Lock = threading.Event() self.Lock.clear() #locks for tcp communication self.server = ZeroconfTcpServer() self.server.addHandler("maze", self.sendMaze) self.server.addHandler("path", self.receivePath) self.server.addHandler("currentPosition", self.sendCurrentPosition) self.server.initThreads() self.server.start() try: os.remove("/home/pi/robot_sem4/robot.log") except OSError: pass logger = logging.getLogger('robot') logger.setLevel(logging.INFO) fh = logging.FileHandler('robot.log') fh.setLevel(logging.INFO) formatter = logging.Formatter('%(asctime)s/%(name)s/%(message)s') fh.setFormatter(formatter) logger.addHandler(fh) inited = False while not inited: try: 'sensors' self.ir_sensors = IR_Sensors_Controller(0x20) #self.ir_sensors.setConfigurationRegister(0x00,0x7F) 'motors' self.dual_motors = DualMotorController(0x64, 0x61) self.dual_motors.hardStop() self.dual_motors.getFullStatus1() self.dual_motors.setOtpParam() self.dual_motors.setMotorParams(self.left, self.right, 2, 2) self.dual_motors.resetPosition() #self.dual_motors.runInit() time.sleep(2) 'pid and direction' self.pid = Pid(self.left, self.right, self.ir_sensors, self.dual_motors, cmMin, cmMaxPid, setPoint) 'wallchecker' self.wallChecker = WallsChecker(self.left, self.right, self.front, cmMin, cmMaxWallChecker, setPoint) 'turnThread' self.turnThread = TurnThread(self.ir_sensors, self.wallChecker, self.dual_motors, self.left, self.right) 'StepCounter' self.stepCounter = StepCounter() 'Mapping' self.mapping = Mapping() 'load gainfactors' gainfactors = self.pid.getGainFactors() self.pGain = gainfactors[0] self.dGain = gainfactors[1] self.iGain = gainfactors[2] 'stateMachineThread' self.stateThread = threading.Thread(target=self.doMapping) self.stateThread.daemon = True self.stateThread.start() inited = True except IOError as e: print("error in doPid: " + str(e))