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)
return current_frame
def _open_capture(self):
self._capture = cv2.VideoCapture(self._camera_index)
self._capture.set(cv2.CAP_PROP_FRAME_WIDTH, EMBEDDED_CAMERA_IMAGE_SIZE[0])
self._capture.set(cv2.CAP_PROP_FRAME_HEIGHT, EMBEDDED_CAMERA_IMAGE_SIZE[1])
self._capture.set(cv2.CAP_PROP_BUFFERSIZE, 1)
def _close_capture(self):
self._capture.release()
if __name__ == "__main__":
movement_command_factory = MovementCommandFactory(
ROBOT_MAXIMUM_SPEED,
SERVOING_CONSTANT,
BASE_COMMAND_DURATION,
)
motor_controller = StmMotorController(
serial.Serial(port=STM_PORT_NAME, baudrate=STM_BAUD_RATE)
)
camera = OpenCvEmbeddedCamera(0)
vision_service = VisionService(camera, MagicMock())
ALIGNED_OHMMETER_HORIZONTAL_POSITION = 349
OHMMETER_ALIGNMENT_THRESHOLD = 10
movement_service = MovementService(movement_command_factory, motor_controller)
starting_zone_line_detector = OpenCvStartingZoneLineDetector()
ohmmeter_alignment_corrector = OhmmeterAlignmentCorrector(
def setUp(self) -> None:
self.movement_command_factory = MovementCommandFactory(
self.A_ROBOT_MAXIMUM_SPEED,
self.A_SEVOING_CONSTANT,
self.A_COMMAND_DURATION,
)
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.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 ----------")
input("Press any key when ready to continue......")
self._puck_alignment_corrector.calculate_horizontal_correction(
current_image, puck_color))
if horizontal_movement_command.get_direction() == Direction.STOP:
self._movement_service.execute_movement_command(
horizontal_movement_command)
break
if __name__ == "__main__":
image_center = Position(320, 240)
camera = FakeCamera(0)
vision_service = FakeVisionService(camera)
puck_detector = OpenCvPuckDetector()
serial = serial.Serial(port=STM_PORT_NAME, baudrate=STM_BAUD_RATE)
stm_motor_controller = StmMotorController(serial)
movement_service = MovementService(
MovementCommandFactory(ROBOT_MAXIMUM_SPEED, SERVOING_CONSTANT,
BASE_COMMAND_DURATION, None, None),
stm_motor_controller,
)
puck_alignment_corrector = PuckAlignmentCorrector(image_center, 10,
puck_detector)
horizontal_alignment_corrector = HorizontalAlignmentCorrector(
movement_service, vision_service, puck_alignment_corrector)
puck_color_to_align = Color.YELLOW
horizontal_alignment_corrector.correct_horizontal_alignment(
puck_color_to_align)