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"})
Exemple #3
0
    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))
Exemple #4
0
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"})
Exemple #5
0
    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]
Exemple #6
0
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()