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))
class RobotNavigator():
'''
used to tune the pid gain factors using keyboard input
press q to save
tune wheel + -
pGain left a z
pGain right s x
iGain left d c
iGain right f v
dGain left g b
dGain right h n
'''
stepsPrCell=6000
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))
def printGains(self):
print("gains="+str(self.pid.getGainFactors()))
def doPathing(self):
print "running Paathing thread"
mode=1
first=True
while not self.Lock.is_set():
#print "no lock"
self.Lock.wait(0.01)
try:
self.dual_motors.setMotorParams(self.left, self.right, 1, 1)
sample=self.ir_sensors.multiChannelReadCm(sensorChannels,1)
walls=self.wallChecker.checkWalls(sample)
#print "has sampled"
if mode:
self.dual_motors.setPosition(32767, 32767)
if walls==[1,1,0] and not first and self.dual_motors.isBusy():
if mode:
self.dual_motors.setPosition(32767, 32767)
sample=self.ir_sensors.multiChannelReadCm(sensorChannels,1)
walls=self.wallChecker.checkWalls(sample)
self.pid.doPid(sample)
self.Lock.wait(0.001)
else:
#print "making choice"
choice=self.mapping.getChoice()
print choice
if choice==[0,0]:
#print "out of mode 2 clearet lock"
self.Lock.set()
else:
if not first:
self.turnThread.checkForTurn(-1)
self.turnThread.checkForTurn(choice[1])
self.pid.reset()
mode=1
if choice[0]!=0:
steps=choice[0]-self.stepsPrCell/2
self.dual_motors.setMotorParams(self.left, self.right, 1, 1)
self.dual_motors.setPosition(steps,steps)
mode=0
first=False
except IOError as e:
print("error in doPid: "+str(e))
print "closing Paathing thread"
def doMapping(self):
print "running mapping thread"
while not self.Lock.is_set():
self.Lock.wait(0.01)
try:
#print "start sampling section"
sample=self.ir_sensors.multiChannelReadCm(sensorChannels,1)
walls=self.wallChecker.checkWalls(sample)
self.dual_motors.setMotorParams(self.left, self.right, 1, 1)
#print "end of sampling section"
#print walls
if(walls==[1, 1, 0]):
self.stepCounter(self.dual_motors.setPosition(32767, 32767))
self.pid.doPid(sample)
else:
steps=self.stepCounter.getSteps()
if self.firstCell:
steps-=self.stepsPrCell
self.firstCell=False
print steps
self.turnThread.checkForTurn(-1)
sample=self.ir_sensors.multiChannelReadCm(sensorChannels,1)
walls=self.wallChecker.checkWalls(sample)
choice = self.mapping.getChoice(steps,walls)
self.turnThread.checkForTurn(choice)
print self.mapping.getMaze()
if not choice:
print "mapped Ok waiting for instructions\n heres the maze:"
print "lock cleared in mode 1"
self.Lock.set()
self.pid.reset()
if walls==[1,1,1]:
self.stepCounter.resetSteps(-800)
self.turnThread.checkForTurn(-1)
self.stepCounter.resetSteps()
self.dual_motors.resetPosition()
except IOError as e:
print("error in doPid: "+str(e))
print "closing mapping thread"
def stop(self):
self.Lock.set()
self.stateThread.join()
self.dual_motors.softStop()
self.server.stop()
def sendMaze(self,params=0):
print "in sendMaze"
if self.stateThread.is_alive():
return json.dumps({'status':"error",'cause':"robot is busy"})
else:
maze=self.mapping.getMaze()
print "sendMaze got maze"+str(maze)
currentPos=self.mapping.getCurrentPosition()
print "sendMaze got current position"+str(currentPos)
mazeDict=maze.getDict()
print "sendMaze got dict:"+str(mazeDict)
returner={'status':"success",'currentpos':currentPos,'maze':mazeDict}
print "returner"+str(returner)
return json.dumps(returner)
def sendCurrentPosition(self,params=0):
if self.stateThread.is_alive():
return json.dumps({'status':"error",'cause':"robot is busy"})
else:
currentPos=self.mapping.getCurrentPosition()
returner= {'status':"success",'currentPosition':currentPos}
self.Lock.clear()
return json.dumps(returner)
def receivePath(self,params=0):
if self.stateThread.is_alive():
return json.dumps({'status':"error",'cause':"robot is busy"})
else:
self.mapping.receiveStack(params)
self.Lock.clear()
self.stateThread=threading.Thread(target=self.doPathing)
self.stateThread.daemon = True
self.stateThread.start()
print "receive path success"
return json.dumps({'status':"success"})
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))
class RobotNavigator():
'''
used to tune the pid gain factors using keyboard input
press q to save
tune wheel + -
pGain left a z
pGain right s x
iGain left d c
iGain right f v
dGain left g b
dGain right h n
'''
stepsPrCell = 6000
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))
def printGains(self):
print("gains=" + str(self.pid.getGainFactors()))
def doPathing(self):
print "running Paathing thread"
mode = 1
first = True
while not self.Lock.is_set():
#print "no lock"
self.Lock.wait(0.01)
try:
self.dual_motors.setMotorParams(self.left, self.right, 1, 1)
sample = self.ir_sensors.multiChannelReadCm(sensorChannels, 1)
walls = self.wallChecker.checkWalls(sample)
#print "has sampled"
if mode:
self.dual_motors.setPosition(32767, 32767)
if walls == [1, 1, 0
] and not first and self.dual_motors.isBusy():
if mode:
self.dual_motors.setPosition(32767, 32767)
sample = self.ir_sensors.multiChannelReadCm(
sensorChannels, 1)
walls = self.wallChecker.checkWalls(sample)
self.pid.doPid(sample)
self.Lock.wait(0.001)
else:
#print "making choice"
choice = self.mapping.getChoice()
print choice
if choice == [0, 0]:
#print "out of mode 2 clearet lock"
self.Lock.set()
else:
if not first:
self.turnThread.checkForTurn(-1)
self.turnThread.checkForTurn(choice[1])
self.pid.reset()
mode = 1
if choice[0] != 0:
steps = choice[0] - self.stepsPrCell / 2
self.dual_motors.setMotorParams(
self.left, self.right, 1, 1)
self.dual_motors.setPosition(steps, steps)
mode = 0
first = False
except IOError as e:
print("error in doPid: " + str(e))
print "closing Paathing thread"
def doMapping(self):
print "running mapping thread"
while not self.Lock.is_set():
self.Lock.wait(0.01)
try:
#print "start sampling section"
sample = self.ir_sensors.multiChannelReadCm(sensorChannels, 1)
walls = self.wallChecker.checkWalls(sample)
self.dual_motors.setMotorParams(self.left, self.right, 1, 1)
#print "end of sampling section"
#print walls
if (walls == [1, 1, 0]):
self.stepCounter(self.dual_motors.setPosition(
32767, 32767))
self.pid.doPid(sample)
else:
steps = self.stepCounter.getSteps()
if self.firstCell:
steps -= self.stepsPrCell
self.firstCell = False
print steps
self.turnThread.checkForTurn(-1)
sample = self.ir_sensors.multiChannelReadCm(
sensorChannels, 1)
walls = self.wallChecker.checkWalls(sample)
choice = self.mapping.getChoice(steps, walls)
self.turnThread.checkForTurn(choice)
print self.mapping.getMaze()
if not choice:
print "mapped Ok waiting for instructions\n heres the maze:"
print "lock cleared in mode 1"
self.Lock.set()
self.pid.reset()
if walls == [1, 1, 1]:
self.stepCounter.resetSteps(-800)
self.turnThread.checkForTurn(-1)
self.stepCounter.resetSteps()
self.dual_motors.resetPosition()
except IOError as e:
print("error in doPid: " + str(e))
print "closing mapping thread"
def stop(self):
self.Lock.set()
self.stateThread.join()
self.dual_motors.softStop()
self.server.stop()
def sendMaze(self, params=0):
print "in sendMaze"
if self.stateThread.is_alive():
return json.dumps({'status': "error", 'cause': "robot is busy"})
else:
maze = self.mapping.getMaze()
print "sendMaze got maze" + str(maze)
currentPos = self.mapping.getCurrentPosition()
print "sendMaze got current position" + str(currentPos)
mazeDict = maze.getDict()
print "sendMaze got dict:" + str(mazeDict)
returner = {
'status': "success",
'currentpos': currentPos,
'maze': mazeDict
}
print "returner" + str(returner)
return json.dumps(returner)
def sendCurrentPosition(self, params=0):
if self.stateThread.is_alive():
return json.dumps({'status': "error", 'cause': "robot is busy"})
else:
currentPos = self.mapping.getCurrentPosition()
returner = {'status': "success", 'currentPosition': currentPos}
self.Lock.clear()
return json.dumps(returner)
def receivePath(self, params=0):
if self.stateThread.is_alive():
return json.dumps({'status': "error", 'cause': "robot is busy"})
else:
self.mapping.receiveStack(params)
self.Lock.clear()
self.stateThread = threading.Thread(target=self.doPathing)
self.stateThread.daemon = True
self.stateThread.start()
print "receive path success"
return json.dumps({'status': "success"})
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
self.tuneFactor=0.1
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)
'sensors'
self.ir_sensors = IR_Sensors_Controller(0x20)
#self.ir_sensors.setConfigurationRegister(0x00,0x7F)
'motors'
self.dual_motors=DualMotorController(0x60,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)
'wallchecker'
self.wallChecker=WallsChecker(self.pid.getMinMaxSetpoint(),self.left,self.right,self.front)
'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]
class PidTuner():
'''
used to tune the pid gain factors using keyboard input
press q to save
tune wheel + -
pGain left a z
pGain right s x
iGain left d c
iGain right f v
dGain left g b
dGain right h n
'''
stepsPrCell=6000
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
self.tuneFactor=0.1
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)
'sensors'
self.ir_sensors = IR_Sensors_Controller(0x20)
#self.ir_sensors.setConfigurationRegister(0x00,0x7F)
'motors'
self.dual_motors=DualMotorController(0x60,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)
'wallchecker'
self.wallChecker=WallsChecker(self.pid.getMinMaxSetpoint(),self.left,self.right,self.front)
'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]
def lpgadd(self):
self.pGain=[self.pGain[self.left]+self.tuneFactor,self.pGain[self.right]]
self.pid.pTune(self.pGain)
def rpgadd(self):
self.pGain=[self.pGain[self.left],self.pGain[self.right]+self.tuneFactor]
self.pid.pTune(self.pGain)
def lpgsub(self):
self.pGain=[self.pGain[self.left]-self.tuneFactor,self.pGain[self.right]]
self.pid.pTune(self.pGain)
def rpgsub(self):
self.pGain=[self.pGain[self.left],self.pGain[self.right]-self.tuneFactor]
self.pid.pTune(self.pGain)
def ldgadd(self):
self.dGain=[self.dGain[self.left]+self.tuneFactor,self.dGain[self.right]]
self.pid.dTune(self.dGain)
def rdgadd(self):
self.dGain=[self.dGain[self.left],self.dGain[self.right]+self.tuneFactor]
self.pid.dTune(self.dGain)
def ldgsub(self):
self.dGain=[self.dGain[self.left]-self.tuneFactor,self.dGain[self.right]]
self.pid.dTune(self.dGain)
def rdgsub(self):
self.dGain=[self.dGain[self.left],self.dGain[self.right]-self.tuneFactor]
self.pid.dTune(self.dGain)
def ligadd(self):
self.iGain=[self.iGain[self.left]+self.tuneFactor,self.iGain[self.right]]
self.pid.iTune(self.iGain)
def rigadd(self):
self.iGain=[self.iGain[self.left],self.iGain[self.right]+self.tuneFactor]
self.pid.iTune(self.iGain)
def ligsub(self):
self.iGain=[self.iGain[self.left]-self.tuneFactor,self.iGain[self.right]]
self.pid.iTune(self.iGain)
def rigsub(self):
self.iGain=[self.iGain[self.left],self.iGain[self.right]-self.tuneFactor]
self.pid.iTune(self.iGain)
def printGains(self):
print("gains="+str(self.pid.getGainFactors()))
def save(self):
return self.pid.pickleGainFactors()
def doPid(self):
try:
self.dual_motors.setMotorParams(self.left, self.right, 1, 1)
self.dual_motors.setAccelerations(self.left, self.right, 3)
'start sampling section'
sample=self.ir_sensors.multiChannelReadCm(sensorChannels,1)
walls=self.wallChecker.checkWalls(sample)
'end of sampling section'
if self.mode:#mapping mode
if(walls==[1, 1, 0]):
self.pid.doPid(sample)
self.stepCounter(self.dual_motors.setPosition(32767, 32767))
else:
steps=self.stepCounter.getSteps()
if self.firstCell:
steps-=self.stepsPrCell
self.firstCell=False
print steps
# if walls==[1,1,1]:
# choice = self.mapping.getChoice(steps,walls)
# self.turnThread.checkForTurn(choice)
# #pass
# else:
self.turnThread.checkForTurn(-1)
sample=self.ir_sensors.multiChannelReadCm(sensorChannels,1)
walls=self.wallChecker.checkWalls(sample)
choice = self.mapping.getChoice(steps,walls)
self.turnThread.checkForTurn(choice)
print sample
#print "choice=%d and turningSuccess=%d"%(choice,lol)
if choice==0:
self.mode=0
print "mapped Ok waiting for instructions\n heres the maze:"
print self.mapping.getMaze()
self.pid.reset()
if walls==[1,1,1]:
self.stepCounter.resetSteps(-800)
print self.stepCounter.getSteps()
self.dual_motors.resetPosition()
print self.mapping.getMaze()
elif self.mode==2:#goTo mode
choice=self.mapping.getChoice()
self.stepCounter.resetSteps()
if not self.turnThread.checkForTurn(choice[1]):
self.stepCounter(self.dual_motors.setPosition(choice[0], choice[0]))
self.dual_motors.setAccelerations(self.left, self.right, 5)
self.pid.doPid(sample)
else:
self.pid.reset()
except IOError as e:
print("error in doPid: "+str(e))
def stop(self):
self.dual_motors.softStop()