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
import utility print utility.getAngle([4,4], [2,3], 90)
import utility def turn(angle): currentAngle = int(90) if (angle<-180): print "turnRight" + str(-angle - 180) elif (angle>180): print "turnLeft" + str(angle - 180) elif (angle<0): print "turnLeft" + str(-angle) elif (angle>=0): print "turnRight" + str(angle) currentAngle = (currentAngle + angle) % 360 angleToTurn = utility.getAngle([7, 7], [7, 3], 270) print angleToTurn turn(-270)
import utility print utility.getAngle([4, 4], [2, 3], 90)
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()
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)
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()