def _align_with_puck(self, puck_color: Color) -> None:
while True:
current_image = self._vision_service.take_image()
movement_command = (
self._puck_alignment_corrector.move_forward_until_puck_is_detected(
current_image, puck_color
)
)
self._movement_service.execute_movement_command(movement_command)
if movement_command.get_direction() == Direction.STOP:
if puck_color == Color.WHITE:
self._movement_service.execute_movement_command(
MovementCommand(
Direction.BACKWARDS,
Speed(ROBOT_ALIGNMENT_SPEED),
CommandDuration(1.5),
)
)
self._movement_service.execute_movement_command(
MovementCommand(
Direction.STOP,
Speed(ROBOT_ALIGNMENT_SPEED),
CommandDuration(0),
)
)
break
self._correct_horizontal_alignment_with_puck(puck_color)
self._correct_vertical_alignment(puck_color)
def __init__(self, local_flag):
self._local_flag = local_flag
if not self._local_flag:
self._serial = ThreadSafeSerial(STM_PORT_NAME, STM_BAUD_RATE)
else:
self._serial = MagicMock()
self._led = StmLed(self._serial)
movement_command_factory = MovementCommandFactory(
Speed(ROBOT_MAXIMUM_SPEED),
Speed(SERVOING_CONSTANT),
CommandDuration(BASE_COMMAND_DURATION),
)
self._communication_service = self._create_communication_service()
self._movement_service = self._create_movement_service(
movement_command_factory)
self._maestro = self._create_and_configure_maestro()
self._vision_service = self._create_vision_service()
self._gripper_service = self._create_gripper_service()
self._resistance_service = self._create_resistance_service()
self.stage_service = self._create_stage_service(
movement_command_factory)
self.slave_game_cycle = SlaveGameCycle(self._communication_service,
self.stage_service)
self.communication_runner = CommunicationRunner(
self._communication_service)
self.application_server = ApplicationServer(self.communication_runner,
self.slave_game_cycle)
def _align_with_starting_zone_corner(self) -> None:
self._correct_horizontal_alignment_with_corner()
self._correct_vertical_alignment_with_corner()
self._movement_service.execute_movement_command(
MovementCommand(
Direction.FORWARD, Speed(ROBOT_ALIGNMENT_SPEED), CommandDuration(1.2)
)
)
self._movement_service.execute_movement_command(
MovementCommand(Direction.STOP, Speed(0), CommandDuration(0))
)
def _calculate_horizontal_movement_command(
self, distance_from_center: int
) -> MovementCommand:
if self._is_puck_to_the_left(distance_from_center):
return MovementCommand(
Direction.LEFT,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
return MovementCommand(
Direction.RIGHT,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
def move_forward_until_puck_is_detected(self, image: np.ndarray, puck_color: Color):
try:
self._hsv_puck_detector.detect(image, puck_color)
return self.STOP_MOVEMENT_COMMAND
except PuckNotFoundException:
return MovementCommand(
Direction.FORWARD,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
def test_whenCalculateFromDistanceAndDuration_thenReturnSpeedWithValueDistanceDividedByDuration(
self, ):
a_distance = Distance(self.A_DISTANCE_VALUE)
a_duration = CommandDuration(self.A_DURATION_VALUE)
expected_speed_value = self.A_DISTANCE_VALUE / self.A_DURATION_VALUE
actual_speed = Speed.calculate_from_distance_and_duration(
a_distance, a_duration)
self.assertEqual(expected_speed_value, actual_speed.get_speed())
def test_givenCorrectlyAlignedVertically_whenCalculateVerticalCorrection_thenReturnStopMovementCommand(
self, ):
self.corner_detector.detect_inferior_corner.return_value = self.ALIGNED_POSITION
expected_movement_command = MovementCommand(
Direction.STOP, Speed(ROBOT_ALIGNMENT_SPEED), CommandDuration(0))
actual_movement_command = (
self.corner_alignment_corrector.calculate_vertical_correction(
self.AN_IMAGE))
self.assertEqual(actual_movement_command, expected_movement_command)
def test_givenCornerTooMuchOnTheLeft_whenCalculateHorizontalCorrection_thenReturnLeftMovementCommand(
self, ):
self.corner_detector.detect_inferior_corner.return_value = (
self.CORNER_TOO_MUCH_THE_LEFT)
expected_movement_command = MovementCommand(
Direction.LEFT, Speed(ROBOT_ALIGNMENT_SPEED), CommandDuration(0))
actual_movement_command = (
self.corner_alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE))
self.assertEqual(actual_movement_command, expected_movement_command)
def test_givenCornerTooMuchForward_whenCalculateVerticalCorrection_thenReturnForwardMovementCommand(
self, ):
self.corner_detector.detect_inferior_corner.return_value = (
self.CORNER_TOO_MUCH_FORWARD)
expected_movement_command = MovementCommand(
Direction.FORWARD, Speed(ROBOT_ALIGNMENT_SPEED),
CommandDuration(0))
actual_movement_command = (
self.corner_alignment_corrector.calculate_vertical_correction(
self.AN_IMAGE))
self.assertEqual(actual_movement_command, expected_movement_command)
def test_givenSingleMovementCommand_whenActuateWheels_thenSerializeCommandBeforeWritingOnSerial(
self, ):
a_direction = Direction.LEFT
a_speed = Speed(0.147)
commands = [
MovementCommand(a_direction, a_speed, CommandDuration(100))
]
expected_serialization = b"\x03+\x87\x16>"
self.stm_motor_controller.actuate_wheels(commands)
self.serial_communication.write.assert_called_with(
expected_serialization)
class CornerAlignmentCorrector:
ALIGNMENT_SPEED = Speed(ROBOT_ALIGNMENT_SPEED)
CONTINUOUS_COMMAND_DURATION = CommandDuration(0)
STOP_MOVEMENT_COMMAND = MovementCommand(Direction.STOP, ALIGNMENT_SPEED,
CONTINUOUS_COMMAND_DURATION)
RIGHT_MOVEMENT_COMMAND = MovementCommand(Direction.RIGHT, ALIGNMENT_SPEED,
CONTINUOUS_COMMAND_DURATION)
LEFT_MOVEMENT_COMMAND = MovementCommand(Direction.LEFT, ALIGNMENT_SPEED,
CONTINUOUS_COMMAND_DURATION)
FORWARD_MOVEMENT_COMMAND = MovementCommand(Direction.FORWARD,
ALIGNMENT_SPEED,
CONTINUOUS_COMMAND_DURATION)
def __init__(
self,
corner_detector: ICornerDetector,
corner_alignment_reference_position: Position,
):
self._corner_detector = corner_detector
self._corner_alignment_reference_position = corner_alignment_reference_position
def calculate_horizontal_correction(self,
image: np.ndarray) -> MovementCommand:
corner_position: Position = self._corner_detector.detect_inferior_corner(
image)
horizontal_distance_from_reference_position = (
corner_position.get_x_coordinate() -
self._corner_alignment_reference_position.get_x_coordinate())
if horizontal_distance_from_reference_position == 0:
return self.STOP_MOVEMENT_COMMAND
return self._calculate_horizontal_movement_command(
horizontal_distance_from_reference_position)
def calculate_vertical_correction(self,
image: np.ndarray) -> MovementCommand:
corner_position: Position = self._corner_detector.detect_inferior_corner(
image)
corner_vertical_distance_from_reference_position = (
corner_position.get_y_coordinate() -
self._corner_alignment_reference_position.get_y_coordinate())
if corner_vertical_distance_from_reference_position < 0:
return self.FORWARD_MOVEMENT_COMMAND
return self.STOP_MOVEMENT_COMMAND
def _calculate_horizontal_movement_command(
self, horizontal_distance_from_reference_position: int):
if horizontal_distance_from_reference_position < 0:
return self.LEFT_MOVEMENT_COMMAND
return self.RIGHT_MOVEMENT_COMMAND
def create_from_movement(self,
movement: Movement) -> List[MovementCommand]:
distance_left = movement.get_distance().get_distance()
movement_commands = list()
while distance_left > 0.001:
speed = min(
self._robot_maximum_speed,
Speed(self._servoing_constant * distance_left),
)
if speed * self._base_command_duration.get_duration(
) > distance_left:
speed = Speed.calculate_from_distance_and_duration(
Distance(distance_left), self._base_command_duration)
movement_command = MovementCommand(movement.get_direction(), speed,
self._base_command_duration)
movement_commands.append(movement_command)
distance_left -= speed * self._base_command_duration.get_duration()
movement_commands.append(self.create_stop_command())
return movement_commands
def calculate_horizontal_correction(self, image: np.ndarray) -> MovementCommand:
starting_zone_line_position: Position = (
self._starting_zone_line_detector.detect(image)
)
print("line position: {}".format(starting_zone_line_position.to_tuple()))
horizontal_distance_from_being_aligned = (
self._aligned_ohmmeter_position.get_x_coordinate()
- starting_zone_line_position.get_x_coordinate()
)
if abs(horizontal_distance_from_being_aligned) <= self._horizontal_threshold:
return self.STOP_MOVEMENT_COMMAND
if self._is_line_to_the_right(horizontal_distance_from_being_aligned):
return MovementCommand(
Direction.RIGHT,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
return MovementCommand(
Direction.LEFT,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
class OhmmeterAlignmentCorrector:
CONTINUOUS_COMMAND_DURATION = CommandDuration(0)
STOP_MOVEMENT_COMMAND = MovementCommand(
Direction.STOP, Speed(0), CommandDuration(0)
)
def __init__(
self,
aligned_ohmmeter_position: Position,
horizontal_threshold: int,
starting_zone_line_detector: IStartingZoneLineDetector,
):
self._aligned_ohmmeter_position: Position = aligned_ohmmeter_position
self._horizontal_threshold: int = horizontal_threshold
self._starting_zone_line_detector: IStartingZoneLineDetector = (
starting_zone_line_detector
)
def calculate_horizontal_correction(self, image: np.ndarray) -> MovementCommand:
starting_zone_line_position: Position = (
self._starting_zone_line_detector.detect(image)
)
print("line position: {}".format(starting_zone_line_position.to_tuple()))
horizontal_distance_from_being_aligned = (
self._aligned_ohmmeter_position.get_x_coordinate()
- starting_zone_line_position.get_x_coordinate()
)
if abs(horizontal_distance_from_being_aligned) <= self._horizontal_threshold:
return self.STOP_MOVEMENT_COMMAND
if self._is_line_to_the_right(horizontal_distance_from_being_aligned):
return MovementCommand(
Direction.RIGHT,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
return MovementCommand(
Direction.LEFT,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
def _is_line_to_the_right(
self, horizontal_distance_from_being_aligned: int
) -> bool:
return horizontal_distance_from_being_aligned < 0
def calculate_vertical_correction(
self, image: np.ndarray, puck_color: Color
) -> MovementCommand:
puck_position: Position = self._template_matching_puck_detector.detect(
image, puck_color
)
if (
puck_position.get_y_coordinate()
>= self.correctly_placed_position.get_y_coordinate()
):
return self.STOP_MOVEMENT_COMMAND
return MovementCommand(
Direction.FORWARD,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
def calculate_horizontal_correction_using_hsv(
self, image: np.ndarray, puck_color: Color
) -> MovementCommand:
try:
puck_position: Position = self._hsv_puck_detector.detect(image, puck_color)
horizontal_distance_from_center: int = (
puck_position.get_x_coordinate()
- self.correctly_placed_position.get_x_coordinate()
)
if self._is_puck_position_within_horizontal_threshold(
horizontal_distance_from_center
):
return self.STOP_MOVEMENT_COMMAND
return self._calculate_horizontal_movement_command(
horizontal_distance_from_center
)
except PuckNotFoundException:
return MovementCommand(
Direction.FORWARD,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
class TestPuckAlignmentCorrector(TestCase):
A_PUCK_COLOR = Color.BLUE
AN_IMAGE = MagicMock()
CORRECTLY_PLACED_POSITION: Position = Position(500, 500)
ANY_POSITION = Position(69, 420)
POSITION_WITHIN_HORIZONTAL_THRESHOLD = Position(515, 500)
POSITION_WITHIN_VERTICAL_THRESHOLD = Position(3211, 496)
POSITION_TOO_FAR_TO_THE_RIGHT = Position(600, 500)
POSITION_TOO_FAR_TO_THE_LEFT = Position(300, 500)
POSITION_TOO_FAR_FORWARD = Position(324, 24)
POSITION_TOO_CLOSE = Position(65, 843)
HORIZONTAL_THRESHOLD = 20
UP_THRESHOLD = 320
STOP_MOVEMENT_COMMAND = MovementCommand(Direction.STOP, Speed(0),
CommandDuration(0))
FORWARD_MOVEMENT_COMMAND = MovementCommand(Direction.FORWARD,
Speed(ROBOT_ALIGNMENT_SPEED),
CommandDuration(0))
ALIGNMENT_SPEED = Speed(ROBOT_ALIGNMENT_SPEED)
CONTINUOUS_COMMAND_DURATION = CommandDuration(0)
def setUp(self) -> None:
self.hsv_puck_detector = MagicMock()
self.template_matching_puck_detector = MagicMock()
self.alignment_corrector = PuckAlignmentCorrector(
self.CORRECTLY_PLACED_POSITION,
self.HORIZONTAL_THRESHOLD,
self.UP_THRESHOLD,
self.hsv_puck_detector,
self.template_matching_puck_detector,
)
def test_givenAnImage_whenCalculateHorizontalCorrection_thenPuckPositionIsDetected(
self, ):
self.template_matching_puck_detector.detect.return_value = self.ANY_POSITION
self.alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE, self.A_PUCK_COLOR)
self.template_matching_puck_detector.detect.assert_called_with(
self.AN_IMAGE, self.A_PUCK_COLOR)
def test_givenAnImageWithPuckOnTheRight_whenCalculateHorizontalCorrection_thenReturnMovementToBeHorizontallyAlign(
self, ):
self.template_matching_puck_detector.detect.return_value = (
self.POSITION_TOO_FAR_TO_THE_RIGHT)
expected_movement_command = MovementCommand(
Direction.RIGHT, self.ALIGNMENT_SPEED,
self.CONTINUOUS_COMMAND_DURATION)
actual_movement_command = (
self.alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE, self.A_PUCK_COLOR))
self.assertEqual(actual_movement_command, expected_movement_command)
def test_givenAnImageWithPuckOnTheLeft_whenCalculateHorizontalCorrection_thenLeftMovementToBeHorizontallyAlign(
self, ):
self.template_matching_puck_detector.detect.return_value = (
self.POSITION_TOO_FAR_TO_THE_LEFT)
expected_movement_command = MovementCommand(
Direction.LEFT, self.ALIGNMENT_SPEED,
self.CONTINUOUS_COMMAND_DURATION)
actual_movement_command = (
self.alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE, self.A_PUCK_COLOR))
self.assertEqual(actual_movement_command, expected_movement_command)
def test_givenAnImageWithPuckWithinHorizontalThreshold_whenCalculateHorizontalCorrection_thenReturnStopMovement(
self, ):
self.template_matching_puck_detector.detect.return_value = (
self.POSITION_WITHIN_HORIZONTAL_THRESHOLD)
actual_movement_command = (
self.alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE, self.A_PUCK_COLOR))
self.assertEqual(actual_movement_command, self.STOP_MOVEMENT_COMMAND)
def test_givenAnImageWithPuckTooFarForward_whenCalculateVerticalCorrection_thenMoveForwardToGetCloser(
self, ):
self.template_matching_puck_detector.detect.return_value = (
self.POSITION_TOO_FAR_FORWARD)
expected_movement_command = MovementCommand(
Direction.FORWARD, self.ALIGNMENT_SPEED,
self.CONTINUOUS_COMMAND_DURATION)
actual_movement = self.alignment_corrector.calculate_vertical_correction(
self.AN_IMAGE, self.A_PUCK_COLOR)
self.assertEqual(actual_movement, expected_movement_command)
def test_givenAnImageWithPuckTooClose_whenCalculateVerticalCorrection_thenReturnStopMovementCommand(
self, ):
self.template_matching_puck_detector.detect.return_value = (
self.POSITION_TOO_CLOSE)
actual_movement_command = (
self.alignment_corrector.calculate_vertical_correction(
self.AN_IMAGE, self.A_PUCK_COLOR))
self.assertEqual(actual_movement_command, self.STOP_MOVEMENT_COMMAND)
def test_givenPuckNotFound_whenMoveForwardUntilPuckIsDetected_thenReturnForwardMovementCommand(
self, ):
self.hsv_puck_detector.detect.side_effect = PuckNotFoundException()
actual_movement_command = (
self.alignment_corrector.move_forward_until_puck_is_detected(
self.AN_IMAGE, self.A_PUCK_COLOR))
self.assertEqual(actual_movement_command,
self.FORWARD_MOVEMENT_COMMAND)
def test_givenPuckFound_whenMoveForwardUntilPuckIsDetected_thenReturnStopMovementCommand(
self, ):
self.hsv_puck_detector.detect.return_value = self.ANY_POSITION
actual_movement_command = (
self.alignment_corrector.move_forward_until_puck_is_detected(
self.AN_IMAGE, self.A_PUCK_COLOR))
self.assertEqual(actual_movement_command, self.STOP_MOVEMENT_COMMAND)
class TestStmMotorController(TestCase):
A_DIRECTION = Direction.FORWARD
A_DISTANCE = Distance(distance=10)
A_MOVEMENT = Movement(A_DIRECTION, A_DISTANCE)
A_MOVEMENT_COMMAND = MovementCommand(Direction.LEFT, Speed(0.01),
CommandDuration(0))
A_STOP_COMMAND = MovementCommand(Direction.STOP, Speed(0),
CommandDuration(0.5))
A_MOVEMENT_COMMAND_LIST = [
MovementCommand(Direction.FORWARD, Speed(10), CommandDuration(10)),
A_STOP_COMMAND,
]
def setUp(self) -> None:
self.serial_communication = MagicMock()
self.stm_motor_controller = StmMotorController(
self.serial_communication)
def test_givenMultipleMovementCommands_whenActuateWheels_thenWriteOnSerialOncePerCommand(
self, ):
commands = self.A_MOVEMENT_COMMAND_LIST
self.stm_motor_controller.actuate_wheels(commands)
self.assertEqual(
len(self.A_MOVEMENT_COMMAND_LIST),
self.serial_communication.write.call_count,
)
def test_givenSingleMovementCommand_whenActuateWheels_thenSerializeCommandBeforeWritingOnSerial(
self, ):
a_direction = Direction.LEFT
a_speed = Speed(0.147)
commands = [
MovementCommand(a_direction, a_speed, CommandDuration(100))
]
expected_serialization = b"\x03+\x87\x16>"
self.stm_motor_controller.actuate_wheels(commands)
self.serial_communication.write.assert_called_with(
expected_serialization)
def test_whenRotate_thenSerializeCommandBeforeWritingOnSerial(self):
a_direction = Direction.CLOCKWISE
an_angle = 15.4
rotation_command = RotationCommand(a_direction, an_angle)
expected_serialization = b"\x05ffvA"
self.stm_motor_controller.rotate(rotation_command)
self.serial_communication.write_and_readline.assert_called_with(
expected_serialization)
def test_whenRotate_thenWaitForRobotResponse(self):
a_direction = Direction.CLOCKWISE
an_angle = 15.4
rotation_command = RotationCommand(a_direction, an_angle)
self.stm_motor_controller.rotate(rotation_command)
self.serial_communication.write_and_readline.assert_called()
class TestMovementCommandFactory(TestCase):
AN_ALIGNMENT_SPEED = Speed(0.01)
A_ROBOT_MAXIMUM_SPEED = Speed(0.25)
A_SEVOING_CONSTANT = Speed(5)
A_COMMAND_DURATION = CommandDuration(0.1)
A_DIRECTION = Direction.LEFT
A_DISTANCE = Distance(1)
AN_ANGLE = 43.1
ROTATING_SPEED = Speed(0.2)
ROBOT_RADIUS = 0.1075
SLOW_MOVEMENT_SPEED = Speed(ROBOT_ALIGNMENT_SPEED)
CONTINUOUS_MOVEMENT_DURATION = CommandDuration(0)
NULL_SPEED = Speed(0)
NULL_COMMAND_DURATION = CommandDuration(0)
def setUp(self) -> None:
self.movement_command_factory = MovementCommandFactory(
self.A_ROBOT_MAXIMUM_SPEED,
self.A_SEVOING_CONSTANT,
self.A_COMMAND_DURATION,
)
def test_whenCreateFromMovement_thenLastCommandHasStopDirection(self):
a_movement = Movement(self.A_DIRECTION, self.A_DISTANCE)
movement_commands = self.movement_command_factory.create_from_movement(
a_movement)
last_movement_command = movement_commands[-1]
self.assertEqual(Direction.STOP, last_movement_command.get_direction())
def test_givenMovementGoingForward_whenCreateFromMovement_thenAllCommandsExceptLastHaveForwardDirection(
self, ):
a_movement = Movement(Direction.FORWARD, self.A_DISTANCE)
expected_directions = {Direction.FORWARD}
movement_commands = self.movement_command_factory.create_from_movement(
a_movement)
actual_directions = {
command.get_direction()
for command in movement_commands[:-1]
}
self.assertEqual(expected_directions, actual_directions)
def test_givenMovementGoingBackwards_whenCreateFromMovement_thenAllCommandsExceptLastHaveBackwardsDirection(
self, ):
a_movement = Movement(Direction.BACKWARDS, self.A_DISTANCE)
expected_directions = {Direction.BACKWARDS}
movement_commands = self.movement_command_factory.create_from_movement(
a_movement)
actual_directions = {
command.get_direction()
for command in movement_commands[:-1]
}
self.assertEqual(expected_directions, actual_directions)
def test_givenLongMovement_whenCreateFromMovement_thenFirstCommandsUseMaximumRobotSpeed(
self, ):
a_long_movement = Movement(self.A_DIRECTION, Distance(1000))
movement_commands = self.movement_command_factory.create_from_movement(
a_long_movement)
first_movement, second_movement, *_ = movement_commands
self.assertEqual(self.A_ROBOT_MAXIMUM_SPEED,
first_movement.get_speed())
self.assertEqual(self.A_ROBOT_MAXIMUM_SPEED,
second_movement.get_speed())
def test_whenCreateFromMovement_thenLastCommandsHaveSpeedLessThanMaximumRobotSpeed(
self, ):
a_movement = Movement(self.A_DIRECTION, self.A_DISTANCE)
movement_commands = self.movement_command_factory.create_from_movement(
a_movement)
last_non_stop_command = movement_commands[-2]
self.assertLess(
last_non_stop_command.get_speed(),
self.A_ROBOT_MAXIMUM_SPEED,
)
def test_givenASetCommandDuration_whenCreateFromMovement_thenAllCommandsHaveThisDuration(
self, ):
a_movement = Movement(self.A_DIRECTION, self.A_DISTANCE)
expected_durations = {self.A_COMMAND_DURATION}
movement_commands = self.movement_command_factory.create_from_movement(
a_movement)
actual_durations = {
command.get_duration()
for command in movement_commands
}
self.assertEqual(expected_durations, actual_durations)
def test_givenNegativeAngle_whenCreateFromAngle_thenReturnCommandWithSameAngle(
self, ):
a_negative_angle = -self.AN_ANGLE
actual_command = self.movement_command_factory.create_from_angle(
a_negative_angle)
self.assertEqual(self.AN_ANGLE, actual_command.get_angle())
def test_whenCreateFromAngle_thenReturnCommandWithSameAngle(self):
actual_command = self.movement_command_factory.create_from_angle(
self.AN_ANGLE)
self.assertEqual(self.AN_ANGLE, actual_command.get_angle())
def test_givenAPositiveAngle_whenCreateFromAngle_thenReturnCommandWithCounterClockwiseDirection(
self, ):
a_positive_angle = 12.3
actual_command = self.movement_command_factory.create_from_angle(
a_positive_angle)
self.assertEqual(Direction.COUNTER_CLOCKWISE,
actual_command.get_direction())
def test_givenANegativeAngle_whenCreateFromAngle_thenReturnCommandWithClockwiseDirection(
self, ):
a_negative_angle = -12.3
actual_command = self.movement_command_factory.create_from_angle(
a_negative_angle)
self.assertEqual(Direction.CLOCKWISE, actual_command.get_direction())
def test_givenBackwardsDirection_whenCreateAlignmentMovementCommand_thenSlowContinuousBackwardsMovementCommandIsCreated(
self, ):
expected_movement_command = MovementCommand(
Direction.BACKWARDS,
self.SLOW_MOVEMENT_SPEED,
self.CONTINUOUS_MOVEMENT_DURATION,
)
actual_movement_command = (
self.movement_command_factory.create_alignment_movement_command(
Direction.BACKWARDS))
self.assertEqual(actual_movement_command, expected_movement_command)
def test_whenCreateStopMovementCommand_thenStopMovementCommandIsCreated(
self):
expected_movement_command = MovementCommand(Direction.STOP,
self.NULL_SPEED,
self.A_COMMAND_DURATION)
actual_movement_command = self.movement_command_factory.create_stop_command(
)
self.assertEqual(actual_movement_command, expected_movement_command)
from domain.movement.Distance import Distance
from domain.movement.Movement import Movement
from domain.movement.MovementCommandFactory import MovementCommandFactory
from domain.movement.Speed import Speed
from infra.motor_controller.StmMotorController import StmMotorController
from service.movement.MovementService import MovementService
if __name__ == "__main__":
straight_movements = [Movement(Direction.FORWARD, Distance(1))]
turn_movements = [
Movement(Direction.FORWARD, Distance(0.5)),
Movement(Direction.LEFT, Distance(0.5)),
]
movement_command_factory = MovementCommandFactory(
Speed(ROBOT_MAXIMUM_SPEED),
Speed(SERVOING_CONSTANT),
CommandDuration(BASE_COMMAND_DURATION),
)
motor_controller = StmMotorController(
serial.Serial(STM_PORT_NAME, STM_BAUD_RATE))
movement_service = MovementService(movement_command_factory,
motor_controller)
print("----------- Straight path -------------")
input("Press any key when ready to start......")
movement_service.move(straight_movements)
print(" -------- Straight path over ----------")
def create_stop_command(self):
return MovementCommand(Direction.STOP, Speed(0),
self._base_command_duration)
def create_alignment_movement_command(self, direction: Direction):
return MovementCommand(direction, Speed(ROBOT_ALIGNMENT_SPEED),
CommandDuration(0))
class PuckAlignmentCorrector:
CONTINUOUS_COMMAND_DURATION = CommandDuration(0)
STOP_MOVEMENT_COMMAND = MovementCommand(
Direction.STOP, Speed(0), CONTINUOUS_COMMAND_DURATION
)
def __init__(
self,
correctly_placed_position: Position,
horizontal_threshold: int,
up_threshold: int,
hsv_puck_detector: IPuckDetector,
template_matching_puck_detector: IPuckDetector,
):
self.correctly_placed_position = correctly_placed_position
self._horizontal_threshold = horizontal_threshold
self._up_threshold = up_threshold
self._hsv_puck_detector = hsv_puck_detector
self._template_matching_puck_detector = template_matching_puck_detector
def move_forward_until_puck_is_detected(self, image: np.ndarray, puck_color: Color):
try:
self._hsv_puck_detector.detect(image, puck_color)
return self.STOP_MOVEMENT_COMMAND
except PuckNotFoundException:
return MovementCommand(
Direction.FORWARD,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
def calculate_horizontal_correction(
self, image: np.ndarray, puck_color: Color
) -> MovementCommand:
puck_position: Position = self._template_matching_puck_detector.detect(
image, puck_color
)
horizontal_distance_from_center: int = (
puck_position.get_x_coordinate()
- self.correctly_placed_position.get_x_coordinate()
)
if self._is_puck_position_within_horizontal_threshold(
horizontal_distance_from_center
):
return self.STOP_MOVEMENT_COMMAND
return self._calculate_horizontal_movement_command(
horizontal_distance_from_center
)
def calculate_horizontal_correction_using_hsv(
self, image: np.ndarray, puck_color: Color
) -> MovementCommand:
try:
puck_position: Position = self._hsv_puck_detector.detect(image, puck_color)
horizontal_distance_from_center: int = (
puck_position.get_x_coordinate()
- self.correctly_placed_position.get_x_coordinate()
)
if self._is_puck_position_within_horizontal_threshold(
horizontal_distance_from_center
):
return self.STOP_MOVEMENT_COMMAND
return self._calculate_horizontal_movement_command(
horizontal_distance_from_center
)
except PuckNotFoundException:
return MovementCommand(
Direction.FORWARD,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
def calculate_vertical_correction(
self, image: np.ndarray, puck_color: Color
) -> MovementCommand:
puck_position: Position = self._template_matching_puck_detector.detect(
image, puck_color
)
if (
puck_position.get_y_coordinate()
>= self.correctly_placed_position.get_y_coordinate()
):
return self.STOP_MOVEMENT_COMMAND
return MovementCommand(
Direction.FORWARD,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
def _is_puck_position_within_horizontal_threshold(
self, horizontal_distance_from_center: int
) -> bool:
return abs(horizontal_distance_from_center) <= self._horizontal_threshold
def _calculate_horizontal_movement_command(
self, distance_from_center: int
) -> MovementCommand:
if self._is_puck_to_the_left(distance_from_center):
return MovementCommand(
Direction.LEFT,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
return MovementCommand(
Direction.RIGHT,
Speed(ROBOT_ALIGNMENT_SPEED),
self.CONTINUOUS_COMMAND_DURATION,
)
def _is_puck_to_the_left(self, distance_from_center: int) -> bool:
return distance_from_center < 0
class TestOhmmeterAlignmentCorrector(TestCase):
AN_IMAGE = MagicMock()
ALIGNMENT_SPEED = Speed(ROBOT_ALIGNMENT_SPEED)
HORIZONTAL_THRESHOLD = 10
ALIGNED_OHMMETER_POSITION = Position(350, 200)
ANY_POSITION = Position(69, 420)
A_POSITION_HORIZONTALLY_ALIGNED = Position(345, 2)
A_POSITION_TOO_FAR_TO_THE_LEFT = Position(300, 42)
A_POSITION_TOO_FAR_TO_THE_RIGHT = Position(400, 211)
A_LEFT_POSITION_WITHIN_THRESHOLD = Position(345, 332)
A_RIGHT_POSITION_WITHIN_THRESHOLD = Position(355, 332)
CONTINUOUS_COMMAND_DURATION = CommandDuration(0)
STOP_MOVEMENT_COMMAND = MovementCommand(Direction.STOP, Speed(0),
CONTINUOUS_COMMAND_DURATION)
def setUp(self) -> None:
self.starting_zone_line_detector = MagicMock()
self.ohmmeter_alignment_corrector = OhmmeterAlignmentCorrector(
self.ALIGNED_OHMMETER_POSITION,
self.HORIZONTAL_THRESHOLD,
self.starting_zone_line_detector,
)
def test_givenAnImage_whenCalculateHorizontalAlignment_thenStartingZoneLineIsDetected(
self, ):
self.starting_zone_line_detector.detect.return_value = self.ANY_POSITION
self.ohmmeter_alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE)
self.starting_zone_line_detector.detect.assert_called_with(
self.AN_IMAGE)
def test_givenAnImagePerfectlyAligned_whenCalculateHorizontalAlignment_thenReturnStopMovementCommand(
self, ):
self.starting_zone_line_detector.detect.return_value = (
self.A_POSITION_HORIZONTALLY_ALIGNED)
actual_command_movement = (
self.ohmmeter_alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE))
self.assertEqual(actual_command_movement, self.STOP_MOVEMENT_COMMAND)
def test_givenLineTooMuchToTheLeft_whenCalculateHorizontalAlignment_thenReturnLeftMovementCommand(
self, ):
self.starting_zone_line_detector.detect.return_value = (
self.A_POSITION_TOO_FAR_TO_THE_LEFT)
expected_movement_command = MovementCommand(
Direction.LEFT, self.ALIGNMENT_SPEED,
self.CONTINUOUS_COMMAND_DURATION)
actual_command_movement = (
self.ohmmeter_alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE))
self.assertEqual(actual_command_movement, expected_movement_command)
def test_givenLineTooMuchOnTheRight_whenCalculateHorizontalAlignment_thenReturnRightMovementCommand(
self, ):
self.starting_zone_line_detector.detect.return_value = (
self.A_POSITION_TOO_FAR_TO_THE_RIGHT)
expected_movement_command = MovementCommand(
Direction.RIGHT, self.ALIGNMENT_SPEED,
self.CONTINUOUS_COMMAND_DURATION)
actual_command_movement = (
self.ohmmeter_alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE))
self.assertEqual(actual_command_movement, expected_movement_command)
def test_givenLineToTheLeftWithinThreshold_whenCalculateHorizontalAlignment_thenReturnStopMovementCommand(
self, ):
self.starting_zone_line_detector.detect.return_value = (
self.A_LEFT_POSITION_WITHIN_THRESHOLD)
actual_movement_command = (
self.ohmmeter_alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE))
self.assertEqual(actual_movement_command, self.STOP_MOVEMENT_COMMAND)
def test_givenLineToTheRightWithinThreshold_whenCalculateHorizontalAlignment_thenReturnStopMovementCommand(
self, ):
self.starting_zone_line_detector.detect.return_value = (
self.A_RIGHT_POSITION_WITHIN_THRESHOLD)
actual_movement_command = (
self.ohmmeter_alignment_corrector.calculate_horizontal_correction(
self.AN_IMAGE))
self.assertEqual(actual_movement_command, self.STOP_MOVEMENT_COMMAND)