def getNextValues(self, state, inp):
     v = sonarDist.getDistanceRight(inp.sonars)
     print 'Dist from robot center to wall on right', v
     return (state, v)
예제 #2
0
    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
예제 #4
0
 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
예제 #8
0
    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))