Esempio n. 1
0
def testSeriesOfCoords():
    pathTwo = [[6, 4], [6, 6], [4, 4], [2, 3], [4, 4]]
    for i in range(len(pathTwo)):
        moveFromCurrentPositionTo(pathTwo[i])
        while arduino.getRobotMotorMovingStatus():
            print WHITE + "arduino.getRobotMotorMovingStatus " + str(
                arduino.getRobotMotorMovingStatus())
def motorSensorChecks():
    pathTwo=[[6,4],[6,6],[6,4],[3,4],[4,4]]
    for i in range(len(pathTwo)):
        time.sleep(1)
        moveFromCurrentPositionTo(pathTwo[i])
        while arduino.getRobotMotorMovingStatus():
            print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
        print WHITE+arduino.getSensorStatus()
    testHead()
Esempio n. 3
0
def motorSensorChecks():
    pathTwo=[[6,4],[6,6],[6,4],[3,4],[4,4]]
    for i in range(len(pathTwo)):
        time.sleep(1)
        moveFromCurrentPositionTo(pathTwo[i])
        while arduino.getRobotMotorMovingStatus():
            print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
        print WHITE+arduino.getSensorStatus()
    testHead()
def testMotorMovingStatus():
    time.sleep(3)
    time.sleep(0.1)
    print 1
    while arduino.getRobotMotorMovingStatus():
        print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
    print 2
    move(30)
    while arduino.getRobotMotorMovingStatus():
        print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
    print 3
Esempio n. 5
0
def testMotorMovingStatus():
    time.sleep(3)
    time.sleep(0.1)
    print 1
    while arduino.getRobotMotorMovingStatus():
        print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
    print 2
    move(30)
    while arduino.getRobotMotorMovingStatus():
        print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
    print 3
Esempio n. 6
0
def moveFromCurrentPositionTo(toLocation):
    global currentPosition
    angleToTurn = utility.getAngle(currentPosition, toLocation, currentAngle) 
    distanceToMove = utility.getDistance(currentPosition, toLocation, cmPerBox)
    turn(angleToTurn)
    while arduino.getRobotMotorMovingStatus():
        pass
    move(distanceToMove)
    while arduino.getRobotMotorMovingStatus():
        pass
    print WHITE+"turn: "+str(angleToTurn)+" distance: "+str(distanceToMove)
    currentPosition=toLocation
def moveFromCurrentPositionTo(toLocation):
    global currentPosition
    angleToTurn = utility.getAngle(currentPosition, toLocation, currentAngle) 
    distanceToMove = utility.getDistance(currentPosition, toLocation, cmPerBox)
    turn(angleToTurn)
    while arduino.getRobotMotorMovingStatus():
        pass
    move(distanceToMove)
    while arduino.getRobotMotorMovingStatus():
        pass
    print WHITE+"turn: "+str(angleToTurn)+" distance: "+str(distanceToMove)
    currentPosition=toLocation
Esempio n. 8
0
def testSensorSimple():
    for i in range(3):
        time.sleep(0.1)
        print WHITE+"testSensor: "+arduino.getSensorStatus()
    turn(90)
    while arduino.getRobotMotorMovingStatus():
        print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
    for i in range(3):
        time.sleep(0.1)
        print WHITE+"testSensor: "+arduino.getSensorStatus()
    turn(-90)
    while arduino.getRobotMotorMovingStatus():
        print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
Esempio n. 9
0
def endingCheck():
    sensorValues = arduino.getSensorStatus()

    sensorValues = sensorValues.split()
        
    isLeftObstacle=utility.parseBoolString(sensorValues[4])
    isRightObstacle=utility.parseBoolString(sensorValues[6])
    print ("==================================")
    print sensorValues

    if (not(isLeftObstacle or isRightObstacle)):
        move(5)
        while arduino.getRobotMotorMovingStatus():
            print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())

    if(currentAngle==90):
        turn(90)
        while arduino.getRobotMotorMovingStatus():
                print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
    if(currentAngle==180):
        turn(-90)
        while arduino.getRobotMotorMovingStatus():
                print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
    if (not(isLeftObstacle or isRightObstacle)):
        move(5)
        while arduino.getRobotMotorMovingStatus():
            print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
Esempio n. 10
0
def endingCheck():
    sensorValues = arduino.getSensorStatus()

    sensorValues = sensorValues.split()

    isLeftObstacle = utility.parseBoolString(sensorValues[4])
    isRightObstacle = utility.parseBoolString(sensorValues[6])
    print("==================================")
    print sensorValues

    if (not (isLeftObstacle or isRightObstacle)):
        move(5)
        while arduino.getRobotMotorMovingStatus():
            print WHITE + "arduino.getRobotMotorMovingStatus " + str(
                arduino.getRobotMotorMovingStatus())

    if (currentAngle == 90):
        turn(90)
        while arduino.getRobotMotorMovingStatus():
            print WHITE + "arduino.getRobotMotorMovingStatus " + str(
                arduino.getRobotMotorMovingStatus())
    if (currentAngle == 180):
        turn(-90)
        while arduino.getRobotMotorMovingStatus():
            print WHITE + "arduino.getRobotMotorMovingStatus " + str(
                arduino.getRobotMotorMovingStatus())
    if (not (isLeftObstacle or isRightObstacle)):
        move(5)
        while arduino.getRobotMotorMovingStatus():
            print WHITE + "arduino.getRobotMotorMovingStatus " + str(
                arduino.getRobotMotorMovingStatus())
Esempio n. 11
0
def testSensorSimple():
    for i in range(3):
        time.sleep(0.1)
        print WHITE + "testSensor: " + arduino.getSensorStatus()
    turn(90)
    while arduino.getRobotMotorMovingStatus():
        print WHITE + "arduino.getRobotMotorMovingStatus " + str(
            arduino.getRobotMotorMovingStatus())
    for i in range(3):
        time.sleep(0.1)
        print WHITE + "testSensor: " + arduino.getSensorStatus()
    turn(-90)
    while arduino.getRobotMotorMovingStatus():
        print WHITE + "arduino.getRobotMotorMovingStatus " + str(
            arduino.getRobotMotorMovingStatus())
Esempio n. 12
0
def manualControl(updownleftright):
    global currentPosition
    sendMessages("msg manualControl: "+updownleftright,2)
    if(runningPhrase==0):
        if(updownleftright=="up"):
            if(currentAngle<180):
                angle = 0 - currentAngle
            else:
                angle = 360 - currentAngle
        if(updownleftright=="right"):
            if(currentAngle<270):
                angle = 180 - (currentAngle + 90)
            else:
                angle = 450 - currentAngle
        if(updownleftright=="down"):
            angle = 180 - currentAngle
        if(updownleftright=="left"):
            if(currentAngle<90):
                angle = -currentAngle - 90
            else:
                angle = -currentAngle + 270

        turn(angle)
        while arduino.getRobotMotorMovingStatus():
            pass
        move(10)
Esempio n. 13
0
def manualControl(updownleftright):
    global currentPosition
    sendMessages("msg manualControl: "+updownleftright,2)
    if(runningPhrase==0):
        if(updownleftright=="up"):
            if(currentAngle<180):
                angle = 0 - currentAngle
            else:
                angle = 360 - currentAngle
        if(updownleftright=="right"):
            if(currentAngle<270):
                angle = 180 - (currentAngle + 90)
            else:
                angle = 450 - currentAngle
        if(updownleftright=="down"):
            angle = 180 - currentAngle
        if(updownleftright=="left"):
            if(currentAngle<90):
                angle = -currentAngle - 90
            else:
                angle = -currentAngle + 270

        turn(angle)
        while arduino.getRobotMotorMovingStatus():
            pass
        move(10)
Esempio n. 14
0
def testTurning():
    for i in range (10):
        turn(90)
        while arduino.getRobotMotorMovingStatus():
            print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
    
        move(30)
        while arduino.getRobotMotorMovingStatus():
            print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
            
    time.sleep(10) 
    for i in range (10):
        turn(-90)
        while arduino.getRobotMotorMovingStatus():
            print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
        move(30)        
        while arduino.getRobotMotorMovingStatus():
            print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
Esempio n. 15
0
def testTurning():
    for i in range(10):
        turn(90)
        while arduino.getRobotMotorMovingStatus():
            print WHITE + "arduino.getRobotMotorMovingStatus " + str(
                arduino.getRobotMotorMovingStatus())

        move(30)
        while arduino.getRobotMotorMovingStatus():
            print WHITE + "arduino.getRobotMotorMovingStatus " + str(
                arduino.getRobotMotorMovingStatus())

    time.sleep(10)
    for i in range(10):
        turn(-90)
        while arduino.getRobotMotorMovingStatus():
            print WHITE + "arduino.getRobotMotorMovingStatus " + str(
                arduino.getRobotMotorMovingStatus())
        move(30)
        while arduino.getRobotMotorMovingStatus():
            print WHITE + "arduino.getRobotMotorMovingStatus " + str(
                arduino.getRobotMotorMovingStatus())
Esempio n. 16
0
def phraseOne():
    setRunningPhrase(1)
    threading.Thread(target=utility.startTimer).start()
    path = []
    goBack = False
    getPath = True
    #getPath = True

    #turn 90 at very first

    while True:

        #to return on the same path used
        isPath = True

        time.sleep(1)
        if (endPosition == currentPosition):
            sendMessages(
                "Reached EndPosition in " + str(utility.getTime()) +
                " seconds", 3)
            #print "=========================>>>>> reach endpoint"
            #phraseOneCheckIfAtGoal()
            break

        try:
            if (currentPosition == path[0]):
                path.pop(0)
        except:
            pass
        #print "dddd"
        #if getPath is true, get optimal path (90degrees)
        if (getPath or len(path) == 0):
            getPath = False
            sendMessages("currentAngle " + str(currentAngle), 3)
            sendMessages(
                "currentPosition " + str(currentPosition[0]) + " " +
                str(currentPosition[1]), 3)
            sendMessages("phraseOneFindOptimalPath", 1)
            returnMsgInString = comms.getMsg()
            #print "aaaaaa"
            #print returnMsgInString
            returnMsgInString = returnMsgInString.split(';')
            pathInString = returnMsgInString[0]
            pathInString = pathInString.translate(string.maketrans('', ''),
                                                  '[] ')
            pathInString = pathInString.split(',')

            changeSafetyDistance = utility.parseBoolString(
                returnMsgInString[1])

            #print "bbbb"
            #print changeSafetyDistance
            #print pathInString
            '''
            if (pathInString=='[]'):
                goBack=True
                continue
            '''

            path = []
            i = 0
            #print "======== "+str(pathInString)

            while i < len(pathInString):
                path.append([int(pathInString[i]), int(pathInString[i + 1])])
                i = i + 2
            print "get new path:::"
            print path

        #base on path, get direction to face
        directionToFace = utility.getAngle(currentPosition, path[0],
                                           currentAngle)
        distanceToNextPoint = utility.getDistance(currentPosition, path[0],
                                                  cmPerBox)
        #print ("direction  "+str(directionToFace))
        #print ("distance   "+str(distanceToNextPoint))
        print "currentPosition = " + str(currentPosition)
        if (directionToFace != 0):
            if (currentPosition == startPosition):
                isPath = False
            elif (len(returnPath) > 0):
                if (returnPath[0] == currentPosition):
                    isPath = False
            if (isPath):
                newPoint = [0, 0]
                newPoint[0] = currentPosition[0]
                newPoint[1] = currentPosition[1]
                returnPath.insert(0, newPoint)
            turn(directionToFace)
            while arduino.getRobotMotorMovingStatus():
                pass
        #print (WHITE+"current position  "+str(currentPosition))
        #print (WHITE+"current angle  "+str(currentAngle))
        #print (WHITE+"disatnce to next point"+str(distanceToNextPoint))
        #check left and right sensor for obs
        sensorValues = arduino.getSensorStatus()
        #print WHITE+"sensor values"
        #print WHITE+sensorValues
        sensorValues = sensorValues.split()

        isLeftObstacle = utility.parseBoolString(sensorValues[4])
        isRightObstacle = utility.parseBoolString(sensorValues[6])

        print "sensor values:::::"
        print sensorValues
        if (currentPosition[0] >= 34 and currentPosition[1] >= 24
                and (isRightObstacle or isLeftObstacle)):
            sendMessages(
                "Reached EndPosition in " + str(utility.getTime()) +
                " seconds", 3)
            print "not very accurate, but regard as reached"
            #print "=========================>>>>> reach endpoint"
            #phraseOneCheckIfAtGoal()
            break

        if (isRightObstacle):  #if right sensor is true
            #print ("current "+str(currentPosition))
            #print "currentPosition = "+str(currentPosition)
            #print "left obstacle"
            setObstacle("right", 10)
            obsTrueCoords = comms.getMsg()
            sendMessages("map setObstacle " + obsTrueCoords, 2)
            getPath = True

        #bottom sensor is for front left
        if (isLeftObstacle):  #if bottom sensor is true, front obs within 10cm
            #print ("current "+str(currentPosition))
            #print "currentPosition = "+str(currentPosition)
            #print "right obstacle"
            setObstacle("left", 10)
            obsTrueCoords = comms.getMsg()
            sendMessages("map setObstacle " + obsTrueCoords, 2)
            getPath = True

        if ((not getPath) or changeSafetyDistance):
            move(10)
            changeSafetyDistance = False
        else:
            continue

        while arduino.getRobotMotorMovingStatus():
            pass
        continue
    endingCheck()
Esempio n. 17
0
def testSeriesOfCoords():
    pathTwo=[[6,4],[6,6],[4,4],[2,3],[4,4]]
    for i in range(len(pathTwo)):
        moveFromCurrentPositionTo(pathTwo[i])
        while arduino.getRobotMotorMovingStatus():
            print WHITE+"arduino.getRobotMotorMovingStatus "+str(arduino.getRobotMotorMovingStatus())
Esempio n. 18
0
def phraseOne():
    setRunningPhrase(1)
    threading.Thread(target=utility.startTimer).start()
    path = []
    goBack=False
    getPath=True
    #getPath = True

    #turn 90 at very first

    while True:

        #to return on the same path used
        isPath=True

        time.sleep(1)
        if(endPosition==currentPosition):
            sendMessages("Reached EndPosition in "+str(utility.getTime())+" seconds",3)
            #print "=========================>>>>> reach endpoint"
            #phraseOneCheckIfAtGoal()
            break

        try:
            if(currentPosition==path[0]):
                path.pop(0)
        except:
            pass
        #print "dddd"
        #if getPath is true, get optimal path (90degrees)
        if(getPath or len(path)==0):
            getPath = False
            sendMessages("currentAngle "+str(currentAngle),3)
            sendMessages("currentPosition "+str(currentPosition[0])+" "+str(currentPosition[1]),3)
            sendMessages("phraseOneFindOptimalPath",1)
            returnMsgInString = comms.getMsg()
            #print "aaaaaa"
            #print returnMsgInString
            returnMsgInString = returnMsgInString.split(';')
            pathInString = returnMsgInString[0]
            pathInString = pathInString.translate(string.maketrans('', ''), '[] ')
            pathInString = pathInString.split(',')
            
            
            changeSafetyDistance = utility.parseBoolString(returnMsgInString[1]);

            #print "bbbb"
            #print changeSafetyDistance
            #print pathInString

            '''
            if (pathInString=='[]'):
                goBack=True
                continue
            '''

            
            
            
            path=[]
            i=0
            #print "======== "+str(pathInString)

            
            while i<len(pathInString):
                path.append([int(pathInString[i]),int(pathInString[i+1])])
                i=i+2
            print "get new path:::"
            print path

        #base on path, get direction to face
        directionToFace = utility.getAngle(currentPosition, path[0], currentAngle)
        distanceToNextPoint = utility.getDistance(currentPosition, path[0], cmPerBox)
        #print ("direction  "+str(directionToFace))
        #print ("distance   "+str(distanceToNextPoint)) 
        print "currentPosition = "+str(currentPosition)
        if(directionToFace!=0):
            if(currentPosition==startPosition):
                isPath=False
            elif(len(returnPath)>0):
                if(returnPath[0]==currentPosition):
                    isPath=False
            if(isPath):
                newPoint=[0,0]
                newPoint[0]=currentPosition[0]
                newPoint[1]=currentPosition[1]
                returnPath.insert(0,newPoint)
            turn(directionToFace)
            while arduino.getRobotMotorMovingStatus():
                pass
        #print (WHITE+"current position  "+str(currentPosition))
        #print (WHITE+"current angle  "+str(currentAngle))
        #print (WHITE+"disatnce to next point"+str(distanceToNextPoint))
        #check left and right sensor for obs
        sensorValues = arduino.getSensorStatus()
        #print WHITE+"sensor values"
        #print WHITE+sensorValues
        sensorValues = sensorValues.split()
        
        isLeftObstacle=utility.parseBoolString(sensorValues[4])
        isRightObstacle=utility.parseBoolString(sensorValues[6])

        print "sensor values:::::"
        print sensorValues
        if (currentPosition[0]>=34 and currentPosition[1]>=24 and (isRightObstacle or isLeftObstacle)):
            sendMessages("Reached EndPosition in "+str(utility.getTime())+" seconds",3)
            print "not very accurate, but regard as reached"
            #print "=========================>>>>> reach endpoint"
            #phraseOneCheckIfAtGoal()
            break


        if(isRightObstacle):#if right sensor is true
            #print ("current "+str(currentPosition))
            #print "currentPosition = "+str(currentPosition)
            #print "left obstacle"
            setObstacle("right",10)
            obsTrueCoords = comms.getMsg()
            sendMessages("map setObstacle "+obsTrueCoords,2)
            getPath=True

        #bottom sensor is for front left
        if(isLeftObstacle):#if bottom sensor is true, front obs within 10cm
            #print ("current "+str(currentPosition))
            #print "currentPosition = "+str(currentPosition)
            #print "right obstacle"
            setObstacle("left",10)
            obsTrueCoords = comms.getMsg()
            sendMessages("map setObstacle "+obsTrueCoords,2)
            getPath=True
        
        if ((not getPath) or changeSafetyDistance):
            move(10)
            changeSafetyDistance=False
        else:
            continue


    
        while arduino.getRobotMotorMovingStatus():
            pass
        continue
    endingCheck()
Esempio n. 19
0
        #print ("direction  "+str(directionToFace))
        #print ("distance   "+str(distanceToNextPoint)) 
        print "currentPosition = "+str(currentPosition)
        if(directionToFace!=0):
            if(currentPosition==startPosition):
                isPath=False
            elif(len(returnPath)>0):
                if(returnPath[0]==currentPosition):
                    isPath=False
            if(isPath):
                newPoint=[0,0]
                newPoint[0]=currentPosition[0]
                newPoint[1]=currentPosition[1]
                returnPath.insert(0,newPoint)
            turn(directionToFace)
            while arduino.getRobotMotorMovingStatus():
                pass
        #print (WHITE+"current position  "+str(currentPosition))
        #print (WHITE+"current angle  "+str(currentAngle))
        #print (WHITE+"disatnce to next point"+str(distanceToNextPoint))
        #check left and right sensor for obs
        sensorValues = arduino.getSensorStatus()
        #print WHITE+"sensor values"
        #print WHITE+sensorValues
        sensorValues = sensorValues.split()
        
        isLeftObstacle=utility.parseBoolString(sensorValues[4])
        isRightObstacle=utility.parseBoolString(sensorValues[6])

        print "sensor values:::::"
        print sensorValues