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