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