def go(self, speed, direction):
"""update the speed and direction of the robot"""
rightM = 0
leftM = 0
if direction == 0:
rightM = 100
leftM = 100
elif direction == 45:
rightM = 50
leftM = 100
elif direction == -45:
rightM = 100
leftM = 50
elif direction == 90:
rightM = -80
leftM = 80
elif direction == -90:
rightM = 80
leftM = -80
elif direction == 135:
rightM = -50
leftM = -100
elif direction == -135:
rightM = -100
leftM = -50
elif abs(direction) == 180:
rightM = -100
leftM = -100
speed = speed / 100.0
rightM = rightM * speed
leftM = leftM * speed
print rightM, leftM
self.pinsTL.acquire() # acquire a Thread Lock
if RobotHelper.isPositive(rightM):
self.pinArray["RightBack"].stop()
self.pinArray["RightFront"].start(rightM)
else:
self.pinArray["RightFront"].stop()
self.pinArray["RightBack"].start(abs(rightM))
if RobotHelper.isPositive(leftM):
self.pinArray["LeftBack"].stop()
self.pinArray["LeftFront"].start(leftM)
else:
self.pinArray["LeftFront"].stop()
self.pinArray["LeftBack"].start(abs(leftM))
self.pinsTL.release() # release the Thread Lock
def runMission(self):
"""Internal function for running the mission in a separate thread"""
drc = 0
speed = 100
newDrc = 0
for move in self.moves:
if self.missionStopEv.is_set():
return
command = move['command']
wait_time = move['delay']
# deal with the special commands
if command == "LIGHTON":
self.robot.setLight(1)
elif command == "LIGHTOFF":
self.robot.setLight(0)
elif command == "S": # deal with the speed change
speed = int(move['speed'])
self.robot.go(speed, drc)
else:
newDrc = RobotHelper.getDirection(command) # Try and get the directions from the command
# if we manage to parse the command, send it to the robot otherwise stop it
if not newDrc == None:
drc = newDrc
self.robot.go(speed, drc)
else:
self.robot.stop()
# wait while still listening for the stop event
self.missionStopEv.wait(wait_time / 1000.0)
def getDirection(self, keys):
"""find the direction from the key configuration"""
if keys == [0, 0, 0, 0]:
return None
direction = ""
if keys[0] == 1:
direction += "F"
elif keys[2] == 1:
direction += "B"
if keys[1] == 1:
direction += "L"
elif keys[3] == 1:
direction += "R"
return RobotHelper.getDirection(direction)
def getDirection(self, keys):
"""find the direction from the key configuration"""
if keys == [0, 0, 0, 0]:
return None
direction = ""
if keys[0] == 1:
direction += "F"
elif keys[2] == 1:
direction += "B"
if keys[1] == 1:
direction += "L"
elif keys[3] == 1:
direction += "R"
return RobotHelper.getDirection(direction)
