def getNextValues(self, state, inp):
v = sonarDist.getDistanceRight(inp.sonars)
print 'Dist from robot center to wall on right', v
return (state, v)
def getNextValues(self, state, inp):
#Here are some global variables/settings
mySpd = 0.2
targetStop = 0.5
junctionTrigger = 1.5
#Variables for turning speed
rotSpd = math.pi/8
linSpd = rotSpd*float(state[0])
#In order not to refer to the same thing too many times:
sonarR = inp.sonars[4] #Sonar readings
sonarL = inp.sonars[0] #Sonar readings
sonarF = inp.sonars[2] #Sonar readings
wallR = sonarDist.getDistanceRight(inp.sonars) #Calculated dist.
#This block of code runs only @ the start, till robot senses right wall
if state[0]==0 and state[2]==0:
if sonarR<2.4: #If no wall on right
print round(sonarR,5), round(sonarL,5), round(wallR,5), "Step 1"
newState=(0,state[1],(sonarR,sonarL,wallR))
else:
newState=state
return (newState, io.Action(fvel = mySpd, rvel = 0))
elif state[0]==0:
print round(sonarR,5), round(sonarL,5), round(wallR,5), "Step 2"
errorGap = sonarR-(state[2][0]+state[2][1]+sonarR+sonarL)/4.0
wallGap = (state[2][2]+wallR)/2.0-errorGap
newState = (wallGap,state[1],wallR)
return (newState, io.Action(fvel = mySpd, rvel = 0))
#Beyond here the robot has completed initialisation steps
print "Curr. gap %8.5g, Target gap %.5g" %(wallR,state[0]),
#State specific actions
if state[2] in range(91,93):
LR = (-1)**(state[2]%2) #91 for right, 92 for left
newState = (state[0],state[1],state[2],state[3]+1)
if newState[3]>40:
newState = (newState[0],state[1],93,1)
print "Done turning! Go straight no. 1"
return (newState, io.Action(fvel = mySpd, rvel = 0))
else:
print "Turn no. %3.f" %newState[3]
return (newState, io.Action(fvel = linSpd, rvel = LR*rotSpd))
elif state[2]==93:
#After turning at junction go straight first
newState = (state[0],state[1],93,state[3]+1)
if newState[3]>5:
newState = (newState[0],state[1],wallR)
print "Cleared the corner!"
return (newState, io.Action(fvel = mySpd, rvel = 0))
else:
print "Go straight no. %2.f" %newState[3]
return (newState, io.Action(fvel = mySpd, rvel = 0))
elif state[2]==99:
#Just go straight at junction
if sonarR>2.4 or sonarL>2.4:
newState = state
else:
newState = (state[0],state[1],wallR)
print "Heading straight!"
return (newState, io.Action(fvel = mySpd, rvel = 0))
elif state[2]==80:
#Arrived at dead end, time to wait
if state[3]>=150:
print "Done waiting!"
newState = (state[0],state[1],81,1)
return (newState, io.Action(fvel = 0, rvel = rotSpd))
else:
newState = (state[0],state[1],80,state[3]+1)
print "Waiting %4.1f seconds" %(newState[3]/10.0)
return (newState, io.Action(fvel = 0, rvel = 0))
elif state[2]==81:
#Turn after waiting state
newState = (state[0],state[1],81,state[3]+1)
if newState[3]>80:
newState=(newState[0],state[1],wallR)
print "Done turning!"
return (newState, io.Action(fvel = mySpd, rvel = 0))
else:
print "Right turn no. %3.f" %newState[3]
return (newState, io.Action(fvel = 0, rvel = rotSpd))
elif state[2]==88:
#Completed job, stay at final position
print "Completed job, now resting."
newState=state
return (newState, io.Action(fvel = 0, rvel = 0))
#This section of code is for normal state
elif sonarF<targetStop:
#Robot senses wall right in front
if state[1]==None:
newState = (state[0],None,80,1)
print "Start waiting ..."
else:
destList = state[1]
if len(destList)==1:
print "Finished all deliveries!"
newState = (state[0],state[1],88)
return (newState,io.Action(fvel = 0, rvel = 0))
currPos = destList.pop(0)
newState = (state[0],destList,80,1)
if currPos == "X":
print "Collecting plates at X!"
else:
print "Exposing plates at %s!" %currPos
#Start waiting
return (newState, io.Action(fvel = 0.0, rvel = 0))
elif sonarF<2*targetStop:
#Robot senses wall in front is near
newState = state
newSpd = min(5*(sonarF-targetStop),mySpd)
if newSpd == mySpd:
print "Target lock!"
else:
print "Slowing down! %.3f left" %sonarF
return (newState, io.Action(fvel = newSpd, rvel = 0))
elif sonarL>junctionTrigger or sonarR>junctionTrigger:
#See a junction. What to do next depends on the instruction list!
#But for now we determine the direction ourselves
#91 for right, 92 for left, 99 for straight
if state[1]==None:
#Set your default direction here yo!
nextDir = 99
newState = (state[0],None,nextDir,1)
else:
dirRef = {"R":91,"L":92,"S":99,}
dirList = state[1]
nextDir = dirRef[dirList.pop(0)]
newState = (state[0],dirList,nextDir,1)
print linSpd, mySpd,
if newState[2] in range(91,93):
LR = (-1)**(newState[2]%2) #91 for right, 92 for left
print "First turn"
return (newState, io.Action(fvel = linSpd, rvel = LR*rotSpd))
else:
print "Go straight!"
return (newState, io.Action(fvel = mySpd, rvel = 0))
## #elif inp.sonars[4]>state[0]+1:#If wall disappears:
## elif sonarL>2.4:#If wall disappears:
## newState=(state[0],10,1)
## print "First turn"
## return (newState, io.Action(fvel = linSpd, rvel = 1*rotSpd))
else:
#If nothing else, check for wall spacing and adjust accordingly
desiredRight = state[0]
prevRight = state[2]
currRight = wallR
newState=(state[0],state[1],currRight)
k1=100
k2=-95
rotSpd = k1*(desiredRight-currRight) + k2*(desiredRight-prevRight)
#Following section of code ensures robot doesn't just spin around
maxRotSpd = mySpd/currRight
if abs(rotSpd)<maxRotSpd:
if abs(rotSpd)<0.10:
rotSpd=0
elif rotSpd > maxRotSpd:
rotSpd = maxRotSpd
else:
rotSpd = -1*maxRotSpd
if rotSpd == 0:
print "Just moving along"
else:
print "Slight turn %8.5f" %rotSpd
return (newState,io.Action(fvel = mySpd,rvel=rotSpd))
def getNextValues(self, state, inp):
global prevE, path, currentTheta, finalTheta, turnForwardVel, \
turnRvel, startTime, junctionHistory, deadEndHistory, outputString, sensorHistory, stationList
# Adjusts inp.sonars values to account for error spikes
# saves the last 5 inp.sonars readings, and takes the average (ignoring max and min values)
for i in range(5):
sensorHistory[i].pop(0)
sensorHistory[i].append(inp.sonars[i])
inp.sonars[i] = (sum(sensorHistory[i]) - max(sensorHistory[i]) - min(sensorHistory[i])) / 3.0
temp = inp.temperature
ldr = inp.light
# use k1 and k2 values to calculate rvel
k1 = 30
k2 = -29.7
v = sonarDist.getDistanceRight(inp.sonars)
e = desiredRight - v
angvel = k1*e + k2*prevE
# cap rvel at 0.4 or -0.4
cap = 0.4
if angvel > cap:
angvel = cap
elif angvel < -cap:
angvel = -cap
# set default output to wallfollower values
output = io.Action(forwardVelocity,angvel)
prevE = e
# junction conditions
juncLimit = 1.2
cond1 = inp.sonars[0] >= 0.7 and inp.sonars[1] >= juncLimit and inp.sonars[3] >= juncLimit and inp.sonars[4] >= 0.7
cond2 = inp.sonars[0] >= juncLimit and inp.sonars[1] >= juncLimit and inp.sonars[2] >= juncLimit and inp.sonars[3] >= 0.7
cond3 = inp.sonars[1] >= 0.7 and inp.sonars[2] >= juncLimit and inp.sonars[3] >= juncLimit and inp.sonars[4] >= juncLimit
junction = cond1 or cond2 or cond3 # junction = True if condition is met
junctionHistory.pop(0)
junctionHistory = junctionHistory + [junction]
didEnterJunction = True
for i in junctionHistory:
didEnterJunction = didEnterJunction and i
# deadend conditions
limitFC = 0.6
deadEnd = inp.sonars[1] <= limitFC and inp.sonars[2] <= limitFC and inp.sonars[3] <= limitFC # deadEnd = True if condition is met
deadEndHistory.pop(0)
deadEndHistory = deadEndHistory + [deadEnd]
didEnterDeadEnd = True
for i in deadEndHistory:
didEnterDeadEnd = didEnterDeadEnd and i
###################################
# startState
###################################
if state == 'startState':
startTime = time.time()
nextState = 'moveForward'
output = io.Action(0, 0)
###################################
# moveForward
###################################
elif state == 'moveForward':
if didEnterJunction: # condition satisfied only when junction is met 5 times in a row
nextState = 'junction'
output = io.Action(0, 0)
elif didEnterDeadEnd: # condition satisfied only when deadEnd is met 5 times in a row
startTime = time.time()
nextState = 'sendData'
output = io.Action(0, 0)
else:
nextState = 'moveForward'
if inp.sonars[3] >= 2.5 and inp.sonars[1] >= 2.5 and inp.sonars[2] >= 1:
output = io.Action(forwardVelocity, 0)
elif time.time() - startTime < 1:
output = io.Action(0, 0)
###################################
# junction
###################################
elif state == 'junction':
currentTheta = inp.odometry.theta
if path[0] == 'F':
startTime = time.time()
nextState = 'straightAhead'
elif path[0] == 'L':
finalTheta = util.fixAnglePlusMinusPi(currentTheta + math.pi/2)
nextState = 'turnLeft'
elif path[0] == 'R':
finalTheta = util.fixAnglePlusMinusPi(currentTheta - math.pi/2 + 0.25)
nextState = 'turnRight'
output = io.Action(0, 0)
###################################
# turnLeft
###################################
elif state == 'turnLeft':
if util.nearAngle(finalTheta, inp.odometry.theta, 0.05):
startTime = time.time() - 5
nextState = 'straightAhead'
output = io.Action(0, 0)
else: # turn completed
nextState = 'turnLeft'
output = io.Action(turnForwardVel, turnRvel)
###################################
# turnRight
###################################
elif state == 'turnRight':
if util.nearAngle(finalTheta, inp.odometry.theta, 0.05):
startTime = time.time() - 5
nextState = 'straightAhead'
output = io.Action(0, 0)
else:
nextState = 'turnRight'
output = io.Action(turnForwardVel, -turnRvel)
###################################
# straightAhead
###################################
elif state == 'straightAhead':
if time.time() - startTime < 10.0:
nextState = 'straightAhead'
output = io.Action(0.1, 0.03)
else:
path.pop(0)
nextState = 'moveForward'
###################################
# sendData
###################################
elif state == 'sendData':
if len(stationList) == 1:
word = 'Finished, and arrived at '
elif stationList[0] == 'X':
word = 'Collect Plates at '
else:
word = 'Expose Plates at '
wtime = time.strftime("%H:%M:%S", time.localtime())
wdate = time.strftime("%d-%m-%Y", time.localtime())
outputString += "<%s> || <%s> || %s%s \n"%(wtime, wdate, word, stationList[0])
writeFile(outputString)
if stationList[0] != 'X':
wd = writeData(stationList[0], ldr, temp)
wd.start()
stationList.pop(0) # X has to be popped also
nextState = 'deadEnd'
output = io.Action(0, 0)
###################################
# deadEnd
###################################
elif state == 'deadEnd':
if len(path) == 0:
nextState = 'stop'
else:
if time.time() - startTime < 7.0:
nextState = 'deadEnd'
else:
currentTheta = inp.odometry.theta
finalTheta = util.fixAnglePlusMinusPi(currentTheta - math.pi + 0.5)
nextState = 'turn180'
output = io.Action(0, 0)
###################################
# turn180
###################################
elif state == 'turn180':
if util.nearAngle(finalTheta, inp.odometry.theta, 0.05):
nextState = 'startState'
output = io.Action(0, 0)
else:
nextState = 'turn180'
output = io.Action(0, -0.5)
###################################
# stop
###################################
elif state == 'stop':
nextState = 'stop'
output = io.Action(0, 0)
###################################
print " %.4f \n"%(inp.sonars[2])
print " %.4f %.4f \n"%(inp.sonars[1], inp.sonars[3])
print "%.4f %.4f\n\n\n"%(inp.sonars[0], inp.sonars[4])
print cond1, cond2, cond3, deadEnd
print state
print output
print stationList
print path
return nextState , output
def getNextValues(self, state, inp):
v = sonarDist.getDistanceRight(inp.sonars)
print 'Dist from robot center to wall on right', v
return (state, v)
def getNextValues(self, state, inp):
global prevE, path, currentTheta, finalTheta, offset, turnForwardVel, \
turnRvel, startTime, junctionHistory, deadEndHistory, outputString, sensorHistory, stationList
for i in range(5):
sensorHistory[i].pop(0)
sensorHistory[i].append(inp.sonars[i])
inp.sonars[i] = (sum(sensorHistory[i]) - max(sensorHistory[i]) - min(sensorHistory[i])) / 3.0
temp = inp.temperature
ldr = inp.light
k1 = 30
k2 = -29.7
v = sonarDist.getDistanceRight(inp.sonars)
e = desiredRight - v
angvel = k1*e + k2*prevE
cap = 0.4
if angvel > cap:
angvel = cap
elif angvel < -cap:
angvel = -cap
output = io.Action(forwardVelocity,angvel)
prevE = e
juncLimit = 1.2
cond1 = inp.sonars[0] >= juncLimit and inp.sonars[1] >= juncLimit and inp.sonars[3] >= juncLimit and inp.sonars[4] >= juncLimit
cond2 = inp.sonars[0] >= juncLimit and inp.sonars[1] >= juncLimit and inp.sonars[2] >= juncLimit
cond3 = inp.sonars[2] >= juncLimit and inp.sonars[3] >= juncLimit and inp.sonars[4] >= juncLimit
limitFC = 0.6
junction = cond1 or cond2 or cond3 # junction = True if condition is met
# deadEnd = inp.sonars[1] <= limitFC and inp.sonars[2] <= limitFC and inp.sonars[3] <= limitFC
deadEnd = inp.sonars[2] <= limitFC
junctionHistory.pop(0)
junctionHistory = junctionHistory + [junction]
deadEndHistory.pop(0)
deadEndHistory = deadEndHistory + [deadEnd]
didEnterJunction = True
for i in junctionHistory:
didEnterJunction = didEnterJunction and i
didEnterDeadEnd = True
for i in deadEndHistory:
didEnterDeadEnd = didEnterDeadEnd and i
if state == 'startState':
startTime = time.time()
nextState = 'moveForward'
output = io.Action(0, 0)
elif state == 'moveForward':
if didEnterJunction: # condition satisfied only when junction is met 5 times in a row
nextState = 'junction'
output = io.Action(0, 0)
print nextState
elif didEnterDeadEnd: # condition satisfied only when deadEnd is met 5 times in a row
startTime = time.time()
nextState = 'sendData'
output = io.Action(0, 0)
print nextState
else:
nextState = 'moveForward'
if inp.sonars[3] >= 2.5 and inp.sonars[1] >= 2.5 and inp.sonars[2] >= 1:
output = io.Action(forwardVelocity, 0)
elif time.time() - startTime < 1:
output = io.Action(0, 0)
elif state == 'junction':
currentTheta = inp.odometry.theta
output = io.Action(0, 0)
if path[0] == 'F':
startTime = time.time()
nextState = 'straightAhead'
print nextState
elif path[0] == 'L':
finalTheta = util.fixAnglePlusMinusPi(currentTheta + math.pi/2)
nextState = 'turnLeft'
print nextState
elif path[0] == 'R':
finalTheta = util.fixAnglePlusMinusPi(currentTheta - math.pi/2)
nextState = 'turnRight'
print nextState
elif state == 'turnLeft':
if util.nearAngle(finalTheta, inp.odometry.theta, 0.05):
output = io.Action(0, 0)
# path.pop(0)
startTime = time.time() - 5
nextState = 'straightAhead'
print nextState
else: # turn completed
output = io.Action(turnForwardVel, turnRvel)
nextState = 'turnLeft'
elif state == 'turnRight':
if util.nearAngle(finalTheta, inp.odometry.theta, 0.05):
output = io.Action(0, 0)
# path.pop(0)
startTime = time.time() - 5
nextState = 'straightAhead'
print nextState
else:
output = io.Action(turnForwardVel, -turnRvel)
nextState = 'turnRight'
elif state == 'straightAhead':
if time.time() - startTime < 10.0:
output = io.Action(0.2, 0)
nextState = 'straightAhead'
else:
path.pop(0)
nextState = 'moveForward'
print nextState
elif state == 'sendData':
if len(stationList) == 1:
word = 'Finished, and arrived at '
elif stationList[0] == 'X':
word = 'Collect Plates at '
else:
word = 'Expose Plates at '
wtime = time.strftime("%H:%M:%S", time.localtime())
wdate = time.strftime("%d-%m-%Y", time.localtime())
outputString += "<%s> || <%s>|| %s%s \n"%(wtime, wdate, word, stationList[0])
writeFile(outputString)
if stationList[0] != 'X':
wd = writeData(stationList[0], ldr, temp)
wd.start()
output = io.Action(0, 0)
nextState = 'deadEnd'
stationList.pop(0) # X has to be popped also
elif state == 'deadEnd':
if len(path) == 0:
nextState = 'stop'
output = io.Action(0, 0)
writeFile(outputString)
print nextState
else:
output = io.Action(0, 0)
if time.time() - startTime < 7.0:
nextState = 'deadEnd'
else:
currentTheta = inp.odometry.theta
finalTheta = util.fixAnglePlusMinusPi(currentTheta - math.pi)
nextState = 'turn180'
print nextState
elif state == 'turn180':
if util.nearAngle(finalTheta, inp.odometry.theta, 0.05):
output = io.Action(0, 0)
nextState = 'startState'
print nextState
else:
output = io.Action(0, -0.5)
nextState = 'turn180'
elif state == 'stop':
nextState = 'stop'
output = io.Action(0, 0)
print " %.4f \n"%(inp.sonars[2])
print " %.4f %.4f \n"%(inp.sonars[1], inp.sonars[3])
print "%.4f %.4f\n\n\n"%(inp.sonars[0], inp.sonars[4])
print cond1
print cond2
print cond3
print deadEnd
print output
print stationList
print path
return nextState , output
def plotDist():
func = lambda: sonarDist.getDistanceRight(io.SensorInput().sonars)
robot.gfx.addStaticPlotFunction(y=('d_o', func))
def getNextValues(self, state, inp):
distance = sonarDist.getDistanceRight(inp.sonars)
print "Distance to wall: %.03f" % distance
return state, distance
def getNextValues(self, state, inp):
#ebot
frontsensor=inp.sonars[2]
rightsensor=inp.sonars[4]
leftsensor=inp.sonars[0]
#amingobot
# frontsensor=(inp.sonars[2]+inp.sonars[3])/2
# rightsensor=inp.sonars[0]
# leftsensor=inp.sonars[5]
fvel=0
rvel=0
if state[0]=='SX':
if inp.sonars[2]>0.3:
if rightsensor<1.5:
k1=30
k2=-29.97
e1=0.7-eBotsonarDist.getDistanceRight(inp.sonars)
e0=state[3]
nextstate=['SX',1,state[2],e1,state[4],state[5]]
rvel=k1*e1+k2*e0
fvel=0.1
else:
fvel=0
rvel=0
print frontsensor,rightsensor,leftsensor,rvel
state[5] = rvel
#nextstate=['SX',state[1],state[2],state[3],state[4],state[5]]
else:
fvel=0.0
rvel=0.0
print "XA,stop"
nextstate=['XA',state[1],inp.odometry.theta,state[3],state[4],state[5]]
if state[0]=='XA':
if state[1]==0:
if inp.odometry.theta-state[2]<math.pi:
rvel=0.5
print inp.odometry.theta-state[2]
nextstate=['XA',state[1],state[2],state[3],state[4],state[5]]
else:
rvel=0.0
print "XA,Uturn done"
nextstate=['XA',1,state[2],rightsensor]
if state[1]==1:
if frontsensor>0.5:
if rightsensor<5:
k1=10
k2=-9.97
e1=1.1-rightsensor
e0=state[3]
print rvel
nextstate=['XA',1,state[2],e1]
rvel=k1*e1+k2*e0
fvel=0.3
else:
fvel=0.0
print "XA,right,stop",rightsensor
nextstate=['XA',2,inp.odometry.theta,state[3],state[4],state[5]]
else:
fvel=0
print "XA,stop"
nextstate=['XA',3,state[2],state[3],state[4],state[5]]
if state[1]==2:
if inp.odometry.theta>0 and inp.odometry.theta<0.1:
rvel=0
fvel=0
nextstate=['XA',1,state[2],state[3],state[4],state[5]]
else:
if (inp.odometry.theta-state[2])*-1<math.pi/2:
rvel=-0.3
fvel=0.25
print "XA,turn right 90 degrees"
nextstate=['XA',2,state[2],state[3],state[4],state[5]]
else:
rvel=0
fvel=0
print "XA,stop"
nextstate=['XA',1,state[2],state[3],state[4],state[5]]
if state[1]==3:
time.sleep(3)
print "XA,sleep"
nextstate=['AX',0,inp.odometry.theta,0,state[4],state[5]]
if state[0]=='AX':
if state[1]==0:
if inp.odometry.theta-state[2]<math.pi:
rvel=0.5
print "AX,Uturn"
nextstate=['AX',state[1],state[2],state[3],state[4],state[5]]
else:
rvel=0.0
print "AX,stop"
nextstate=['AX',1,state[2],rightsensor]
if state[1]==1:
if frontsensor>0.5:
if rightsensor<5:
k1=10
k2=-9.97
e1=1.1-rightsensor
e0=state[3]
print k1*e1+k2*e0
nextstate=['AX',1,state[2],e1]
rvel=(k1*e1+k2*e0)
fvel=0.3
else:
fvel=0.0
print "AX,stop"
nextstate=['AX',2,inp.odometry.theta,state[3],state[4],state[5]]
else:
fvel=0
print "AX,stop"
nextstate=['AX',3,state[2],state[3],state[4],state[5]]
if state[1]==2:
if inp.odometry.theta>0 and inp.odometry.theta<0.1:
rvel=0
fvel=0
nextstate=['AX',1,state[2],state[3],state[4],state[5]]
else:
if (inp.odometry.theta-state[2])<math.pi/2:
rvel=0.3
fvel=0.3
print inp.odometry.theta-state[2]
nextstate=['AX',2,state[2],state[3],state[4],state[5]]
else:
rvel=0
fvel=0
print "AX,stop"
nextstate=['AX',1,state[2],state[3],state[4],state[5]]
if state[1]==3:
time.sleep(3)
print "AX,sleep"
nextstate=['XB',0,inp.odometry.theta,0]
return nextstate, io.Action(fvel,rvel)
def getNextValues(self, state, inp):
global prevE, path, currentTheta, finalTheta, offset, turnForwardVel, \
turnRvel, startTime, junctionHistory, deadEndHistory, outputString, sensorHistory, stationList
for i in range(5):
sensorHistory[i].pop(0)
sensorHistory[i].append(inp.sonars[i])
inp.sonars[i] = (sum(sensorHistory[i]) - max(sensorHistory[i]) -
min(sensorHistory[i])) / 3.0
temp = inp.temperature
ldr = inp.light
k1 = 30
k2 = -29.7
v = sonarDist.getDistanceRight(inp.sonars)
e = desiredRight - v
angvel = k1 * e + k2 * prevE
cap = 0.4
if angvel > cap:
angvel = cap
elif angvel < -cap:
angvel = -cap
output = io.Action(forwardVelocity, angvel)
prevE = e
juncLimit = 1.2
cond1 = inp.sonars[0] >= juncLimit and inp.sonars[
1] >= juncLimit and inp.sonars[3] >= juncLimit and inp.sonars[
4] >= juncLimit
cond2 = inp.sonars[0] >= juncLimit and inp.sonars[
1] >= juncLimit and inp.sonars[2] >= juncLimit
cond3 = inp.sonars[2] >= juncLimit and inp.sonars[
3] >= juncLimit and inp.sonars[4] >= juncLimit
limitFC = 0.6
junction = cond1 or cond2 or cond3 # junction = True if condition is met
# deadEnd = inp.sonars[1] <= limitFC and inp.sonars[2] <= limitFC and inp.sonars[3] <= limitFC
deadEnd = inp.sonars[2] <= limitFC
junctionHistory.pop(0)
junctionHistory = junctionHistory + [junction]
deadEndHistory.pop(0)
deadEndHistory = deadEndHistory + [deadEnd]
didEnterJunction = True
for i in junctionHistory:
didEnterJunction = didEnterJunction and i
didEnterDeadEnd = True
for i in deadEndHistory:
didEnterDeadEnd = didEnterDeadEnd and i
if state == 'startState':
startTime = time.time()
nextState = 'moveForward'
output = io.Action(0, 0)
elif state == 'moveForward':
if didEnterJunction: # condition satisfied only when junction is met 5 times in a row
nextState = 'junction'
output = io.Action(0, 0)
print nextState
elif didEnterDeadEnd: # condition satisfied only when deadEnd is met 5 times in a row
startTime = time.time()
nextState = 'sendData'
output = io.Action(0, 0)
print nextState
else:
nextState = 'moveForward'
if inp.sonars[3] >= 2.5 and inp.sonars[
1] >= 2.5 and inp.sonars[2] >= 1:
output = io.Action(forwardVelocity, 0)
elif time.time() - startTime < 1:
output = io.Action(0, 0)
elif state == 'junction':
currentTheta = inp.odometry.theta
output = io.Action(0, 0)
if path[0] == 'F':
startTime = time.time()
nextState = 'straightAhead'
print nextState
elif path[0] == 'L':
finalTheta = util.fixAnglePlusMinusPi(currentTheta +
math.pi / 2)
nextState = 'turnLeft'
print nextState
elif path[0] == 'R':
finalTheta = util.fixAnglePlusMinusPi(currentTheta -
math.pi / 2)
nextState = 'turnRight'
print nextState
elif state == 'turnLeft':
if util.nearAngle(finalTheta, inp.odometry.theta, 0.05):
output = io.Action(0, 0)
# path.pop(0)
startTime = time.time() - 5
nextState = 'straightAhead'
print nextState
else: # turn completed
output = io.Action(turnForwardVel, turnRvel)
nextState = 'turnLeft'
elif state == 'turnRight':
if util.nearAngle(finalTheta, inp.odometry.theta, 0.05):
output = io.Action(0, 0)
# path.pop(0)
startTime = time.time() - 5
nextState = 'straightAhead'
print nextState
else:
output = io.Action(turnForwardVel, -turnRvel)
nextState = 'turnRight'
elif state == 'straightAhead':
if time.time() - startTime < 10.0:
output = io.Action(0.2, 0)
nextState = 'straightAhead'
else:
path.pop(0)
nextState = 'moveForward'
print nextState
elif state == 'sendData':
if len(stationList) == 1:
word = 'Finished, and arrived at '
elif stationList[0] == 'X':
word = 'Collect Plates at '
else:
word = 'Expose Plates at '
wtime = time.strftime("%H:%M:%S", time.localtime())
wdate = time.strftime("%d-%m-%Y", time.localtime())
outputString += "<%s> || <%s>|| %s%s \n" % (wtime, wdate, word,
stationList[0])
writeFile(outputString)
if stationList[0] != 'X':
wd = writeData(stationList[0], ldr, temp)
wd.start()
output = io.Action(0, 0)
nextState = 'deadEnd'
stationList.pop(0) # X has to be popped also
elif state == 'deadEnd':
if len(path) == 0:
nextState = 'stop'
output = io.Action(0, 0)
writeFile(outputString)
print nextState
else:
output = io.Action(0, 0)
if time.time() - startTime < 7.0:
nextState = 'deadEnd'
else:
currentTheta = inp.odometry.theta
finalTheta = util.fixAnglePlusMinusPi(currentTheta -
math.pi)
nextState = 'turn180'
print nextState
elif state == 'turn180':
if util.nearAngle(finalTheta, inp.odometry.theta, 0.05):
output = io.Action(0, 0)
nextState = 'startState'
print nextState
else:
output = io.Action(0, -0.5)
nextState = 'turn180'
elif state == 'stop':
nextState = 'stop'
output = io.Action(0, 0)
print " %.4f \n" % (
inp.sonars[2])
print " %.4f %.4f \n" % (
inp.sonars[1], inp.sonars[3])
print "%.4f %.4f\n\n\n" % (
inp.sonars[0], inp.sonars[4])
print cond1
print cond2
print cond3
print deadEnd
print output
print stationList
print path
return nextState, output
def plotDist():
func = lambda: sonarDist.getDistanceRight(io.SensorInput().sonars)
robot.gfx.addStaticPlotFunction(y=('d_o', func))
