def _create_motor_controller(self):
if self._local_flag:
return FakeMotorController()
return StmMotorController(self._serial, )
def setUp(self) -> None:
self.serial_communication = MagicMock()
self.stm_motor_controller = StmMotorController(
self.serial_communication)
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()
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(
Position(ALIGNED_OHMMETER_HORIZONTAL_POSITION, 0),
OHMMETER_ALIGNMENT_THRESHOLD,
starting_zone_line_detector,
)
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......")
print()
print("----------- Turning Path --------------")
input("Press any key when ready to start......")
horizontal_movement_command = (
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)