mpo_table_saw.py 6.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153
  1. from data_models.mqtt_message import MqttMessage
  2. from pypdevs.DEVS import AtomicDEVS
  3. from pypdevs.infinity import INFINITY
  4. from dataclasses import dataclass
  5. from loguru import logger
  6. from data_models.workpiece import Workpiece
  7. from utils.get_timestamp import get_timestamp
  8. from utils.timed_phase_enum import TimedPhaseEnum
  9. class MPOTableSawPhase(TimedPhaseEnum):
  10. """ Steps in order, along with their timings """
  11. IDLE = ('IDLE', INFINITY)
  12. ROTATE_TO_FRONT = ('ROTATE_TO_FRONT', 2.697) # rotate table to face the gripper
  13. AWAIT_WP = ('AWAIT_WP', INFINITY) # await workpiece delivery by gripper
  14. ROTATE_TO_BACK = ('ROTATE_TO_BACK', 1.791) # rotate table to face saw (at back)
  15. SAW_RIGHT = ('SAW_RIGHT', 3.5)
  16. SAW_LEFT = ('SAW_LEFT', 3.5)
  17. ROTATE_TO_SIDE = ('ROTATE_TO_SIDE', 0.904) # turn table towards conveyor (to the right side)
  18. EJECT_WP = ('EJECT_WP', 0.630) # eject/push workpiece on conveyor
  19. @dataclass
  20. class MPOTableSawState:
  21. delta_t: float = INFINITY
  22. phase: MPOTableSawPhase = MPOTableSawPhase.IDLE
  23. workpiece: Workpiece | None = None
  24. status_requested: bool = True # if true, publish status
  25. visual_update_pending: bool = False
  26. def get_mpo_produced_msg() -> MqttMessage:
  27. """ Get the 'MPO_PRODUCED' mqtt message """
  28. message = MqttMessage()
  29. message.topic = "fl/mpo/ack"
  30. message.payload = {
  31. "code": 2,
  32. "ts": get_timestamp(),
  33. }
  34. return message
  35. class MPOTableSaw(AtomicDEVS):
  36. """ MPO rotating table and table saw """
  37. def __init__(self, name: str):
  38. super(MPOTableSaw, self).__init__(name)
  39. self.gripper_in = self.addInPort("gripper_in") # vacuum gripper which brings us workpiece
  40. self.gripper_out = self.addOutPort("gripper_out") # to acknowledge table turning to the gripper
  41. self.conveyor_out = self.addOutPort("conveyor_out") # conveyor belt output after we're done
  42. self.mqtt_in = self.addInPort("mqtt_in")
  43. self.mqtt_out = self.addOutPort("mqtt_out")
  44. self.state = MPOTableSawState()
  45. def change_phase(self, new_phase: MPOTableSawPhase):
  46. """ Wrapper for changing the phase and time associated with it, helps with logging """
  47. self.state.phase = new_phase
  48. self.state.delta_t = new_phase.timing
  49. logger.trace(f"{type(self).__name__} '{self.name}' phase changed to {new_phase}")
  50. self.state.visual_update_pending = True
  51. def get_visual_update_data(self) -> MqttMessage:
  52. """ Get visual update data for the animation, contains the action taken and the duration of that action left """
  53. message = MqttMessage()
  54. message.topic = "visualization/mpo/table_saw"
  55. duration = self.state.delta_t
  56. if duration == INFINITY: duration = None
  57. message.payload = {
  58. "action": self.state.phase.value,
  59. "duration": duration, # should be equal to the timing of the phase
  60. "workpiece": self.state.workpiece.to_dict() if self.state.workpiece else None,
  61. }
  62. return message
  63. def get_status_message(self, use_next_phase=True) -> MqttMessage:
  64. phase = self.state.phase.next() if use_next_phase else self.state.phase
  65. active = 0 if (phase == MPOTableSawPhase.IDLE) else 1
  66. code = 2 if active == 1 else 1
  67. message = MqttMessage()
  68. message.topic = "f/i/state/mpo"
  69. message.payload = {
  70. "active": active,
  71. "code": code,
  72. "description": '',
  73. "station": 'mpo',
  74. "ts": get_timestamp(),
  75. }
  76. return message
  77. def extTransition(self, inputs):
  78. self.state.delta_t -= self.elapsed
  79. if self.mqtt_in in inputs:
  80. msg = inputs[self.mqtt_in][0]
  81. if msg.topic == "simulation/ctrl/all" and msg.payload.get("action") == "refresh":
  82. self.state.status_requested = True
  83. self.state.visual_update_pending = True
  84. return self.state
  85. elif not (self.state.phase == MPOTableSawPhase.IDLE or self.state.phase == MPOTableSawPhase.AWAIT_WP):
  86. logger.error(f"{type(self).__name__} '{self.name}' received input while not expecting: {inputs}")
  87. return self.state
  88. if self.gripper_in in inputs:
  89. # 2 cases: instruction to turn, or workpiece
  90. if self.state.phase == MPOTableSawPhase.IDLE:
  91. self.change_phase(MPOTableSawPhase.ROTATE_TO_FRONT)
  92. elif self.state.phase == MPOTableSawPhase.AWAIT_WP:
  93. wp = inputs[self.gripper_in][0]
  94. self.state.workpiece = wp
  95. self.change_phase(MPOTableSawPhase.ROTATE_TO_BACK)
  96. else:
  97. logger.error(f"{type(self).__name__} '{self.name}' received gripper input while not expecting: {inputs}")
  98. return self.state # important, return state
  99. def timeAdvance(self):
  100. if self.state.visual_update_pending or self.state.status_requested:
  101. return 0.0
  102. return self.state.delta_t
  103. def outputFnc(self):
  104. if self.state.visual_update_pending:
  105. return {self.mqtt_out: [self.get_visual_update_data()]}
  106. if self.state.status_requested:
  107. return {self.mqtt_out: [self.get_status_message(use_next_phase=False)]}
  108. if self.state.phase == MPOTableSawPhase.ROTATE_TO_FRONT:
  109. return {self.gripper_out: {}, self.mqtt_out: [self.get_status_message()]} # acknowledge rotation
  110. elif self.state.phase == MPOTableSawPhase.EJECT_WP:
  111. # output wp, and publish 'mpo produced' message to mqtt
  112. return {self.conveyor_out: [self.state.workpiece], self.mqtt_out: [get_mpo_produced_msg(), self.get_status_message()]}
  113. return {self.mqtt_out: [self.get_status_message()]}
  114. def intTransition(self):
  115. if self.state.visual_update_pending:
  116. self.state.visual_update_pending = False
  117. return self.state
  118. if self.state.status_requested:
  119. self.state.status_requested = False
  120. return self.state
  121. if self.state.phase == MPOTableSawPhase.EJECT_WP:
  122. self.state.workpiece = None # remove the workpiece we just output to the conveyor
  123. # Transition to next state
  124. self.change_phase(self.state.phase.next())
  125. return self.state