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