def move_to(cur, goal):
global vdir, mode
robot.update_cell(robot_id, cur, 'visited')
stderr.write("move from " + str(cur) + " to " + str(goal) + '\n')
# left, straight, right ?
aim = direction(cur, goal)
# get angle in degrees
angle = asin(cross(vdir, aim)) * 180.0 / pi
# check if this angle is obtuse
if dot_prod(vdir, aim) < 0:
angle = 180 - angle
# rotate to correct direction
if angle > EPS:
robot.rotate(angle)
# move to next cell
robot.straight()
# update vdir
vdir = direction(cur, goal)
# get auto / manual
mode = robot.get_mode(robot_id)
# update robot
robot.update_robot(robot_id, goal, vdir)
robot.update_cell(robot_id, goal, 'robot')
return goal
def test_returns_correct_value_for_counter_clockwise(self):
self.assertEqual(robot.rotate(3, -1), 2)
self.assertEqual(robot.rotate(0, -1), 3)
from BrickPi import * from robot import initialiseDiffDriveRobot, rotate initialiseDiffDriveRobot() rotate(360)
def test_returns_correct_value_for_clockwise(self):
self.assertEqual(robot.rotate(0, 1), 1)
self.assertEqual(robot.rotate(3, 1), 0)
from BrickPi import * from robot import initialiseDiffDriveRobot, rotate initialiseDiffDriveRobot() rotate(90)
