Utility to quickly map servos on an adafruit I2C driver board, to proper names. Can produce a yaml output file, useful as input for other robotics applications.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

318 lines
8.1 KiB

  1. from adafruit_servokit import ServoKit
  2. import getch
  3. import logging
  4. import os
  5. import pprint
  6. import sys
  7. import time
  8. import yaml
  9. class MikesServoMapper:
  10. __BASE_I2C_ADDRESS = 0x40
  11. __CHANNELS_COUNT = 16
  12. __DEFAULT_SERVO_DEGREES = 180
  13. __DEFAULT_JIGGLE_DURATION = 2
  14. __DEFAULT_JIGGLE_SLICES = 50
  15. __ESCAPE_KEY = chr(27)
  16. __DEFAULT_OUTPUT_FILE_NAME = "servo-mappings.yml"
  17. __DEFAULT_SERVO_CHANNEL_MAPPING_KEY = "servo-to-channel"
  18. def __init__(self, config_file: str = None, names=None, output_file: str = None):
  19. # noinspection PyTypeChecker
  20. self.__logger: logging.Logger = None
  21. self.__logger_formatter = None
  22. self.init_logging()
  23. if names is None:
  24. self.__names = list()
  25. else:
  26. self.__names = list(names)
  27. self.__names.sort()
  28. self.__config = None
  29. self.load_config(config_file)
  30. self.pull_config_names()
  31. self.__logger.info("Names: %s" % (pprint.pformat(self.__names)))
  32. self.__output_file_path = output_file
  33. if self.__output_file_path is None:
  34. self.__output_file_path = self.make_default_mappings_output_file_path()
  35. self.__mappings = {}
  36. def init_logging(self):
  37. self.__logger = logging.Logger("Mikes Servo Mapper")
  38. self.__logger_formatter = logging.Formatter(fmt="[%(asctime)s][%(name)s] %(message)s")
  39. stream_handler = logging.StreamHandler(sys.stdout)
  40. stream_handler.setFormatter(self.__logger_formatter)
  41. self.__logger.addHandler(stream_handler)
  42. self.__logger.info("Logging initialized")
  43. def load_config(self, config_file):
  44. if config_file is None:
  45. return
  46. with open(config_file) as f:
  47. config = yaml.safe_load(f)
  48. self.__logger.info("Loaded config: %s" % (pprint.pformat(config),))
  49. self.__config = config
  50. def pull_config_names(self):
  51. if self.__config is None:
  52. self.__logger.info("No config specified; Won't pull names")
  53. return
  54. self.__logger.info("Pulling names from config")
  55. if "names" not in self.__config:
  56. self.__logger.warning("Key \"names\" is not in config; Cannot pull names")
  57. return
  58. config_names = self.__config["names"]
  59. if not isinstance(config_names, list):
  60. self.__logger.warning("Config had key \"names\" but it wasn't a list; Won't pull names")
  61. return
  62. self.__logger.info("Names before pulling from config: %s" % (self.__names,))
  63. for name in config_names:
  64. self.__names.append(name)
  65. self.__names.sort()
  66. self.__logger.info("Names after pulling from config: %s" % (self.__names,))
  67. def set_name_mapping(self, name, channel):
  68. self.__mappings[name] = channel
  69. def get_name_mapping(self, name):
  70. if name in self.__mappings:
  71. return self.__mappings[name]
  72. return None
  73. def determine_i2c_address(self):
  74. return self.__BASE_I2C_ADDRESS
  75. def run(self):
  76. self.__logger.info("Running!")
  77. while True:
  78. self.__logger.info("")
  79. self.__logger.info("Please choose a mode: ")
  80. self.__logger.info("1. Edit mappings")
  81. self.__logger.info("2. Test current mappings")
  82. self.__logger.info("3. Write mappings to file")
  83. self.__logger.info("4. Load previously saved mappings")
  84. self.__logger.info("Q. Quit")
  85. user_choice = getch.getch()
  86. if user_choice == "q" or user_choice == "Q":
  87. self.__logger.info("Quitting!")
  88. break
  89. if user_choice == "1":
  90. self.edit_mappings()
  91. elif user_choice == "2":
  92. self.test_mappings()
  93. elif user_choice == "3":
  94. self.write_mappings()
  95. elif user_choice == "4":
  96. self.load_mappings()
  97. else:
  98. self.__logger.warning("Invalid choice: %s" % user_choice)
  99. def edit_mappings(self):
  100. self.__logger.info("Begin mapping mode !")
  101. i2c_address = self.determine_i2c_address()
  102. servo_kit = ServoKit(
  103. address=i2c_address,
  104. channels=self.__CHANNELS_COUNT
  105. )
  106. #
  107. while True:
  108. # Print all current mappings
  109. self.__logger.info("")
  110. self.__logger.info("Current Mappings:")
  111. menu_number_to_name = {}
  112. for name_index in range(len(self.__names)):
  113. name = self.__names[name_index]
  114. name_number = name_index + 1
  115. menu_number_to_name[str(name_number)] = name
  116. self.__logger.info(
  117. "%s. %s ==> %s"
  118. % (name_number, name, self.get_name_mapping(name=name))
  119. )
  120. self.__logger.info("")
  121. self.__logger.info("Please enter a number to change the corresponding mapping, or Q to quit.")
  122. user_input = getch.getch()
  123. if user_input == "Q" or user_input == "q":
  124. self.__logger.info("Quitting mapping mode")
  125. break
  126. elif user_input in menu_number_to_name:
  127. name = menu_number_to_name[user_input]
  128. channel = self.run_one_mapping(
  129. servo_kit=servo_kit,
  130. name=name,
  131. default_channel=self.get_name_mapping(name)
  132. )
  133. self.set_name_mapping(name=name, channel=channel)
  134. else:
  135. self.__logger.warning("Invalid input: %s" % user_input)
  136. def run_one_mapping(self, servo_kit, name, default_channel=None):
  137. selected_channel = default_channel
  138. while True:
  139. self.__logger.info("")
  140. self.__logger.info("Mapping channel for: %s" % (name,))
  141. self.__logger.info(
  142. "Press a key between 0-9 and A-F to try a channel."
  143. )
  144. self.__logger.info(
  145. "Press the space bar when you've found the correct channel, or escape to abort."
  146. )
  147. self.__logger.info("Currently selected channel: %s" % selected_channel)
  148. key = getch.getch().lower()
  149. if key == self.__ESCAPE_KEY:
  150. self.__logger.info("Aborting")
  151. selected_channel = None
  152. break
  153. elif key == " ":
  154. self.__logger.info("Selected channel: %s" % selected_channel)
  155. break
  156. else:
  157. try:
  158. channel = int(key, 16)
  159. selected_channel = channel
  160. self.jiggle_channel(servo_kit=servo_kit, channel=channel)
  161. except ValueError:
  162. self.__logger.warning("Invalid input!: %s" % (key,))
  163. time.sleep(1)
  164. return selected_channel
  165. def test_mappings(self):
  166. self.__logger.info("Testing mappings!")
  167. for name in self.__mappings.keys():
  168. channel = self.get_name_mapping(name=name)
  169. self.__logger.info("Jiggling mapping: %s ==> %s" % (name, channel))
  170. time.sleep(1)
  171. self.__logger.info("Done testing mappings")
  172. def jiggle_channel(self, servo_kit, channel):
  173. duration = self.__DEFAULT_JIGGLE_DURATION
  174. degrees_per_slice = self.__DEFAULT_SERVO_DEGREES / self.__DEFAULT_JIGGLE_SLICES
  175. seconds_per_slice = duration / self.__DEFAULT_JIGGLE_SLICES
  176. self.__logger.info(
  177. "Jiggling servo on channel #%s using %s slices over %s seconds"
  178. % (channel, self.__DEFAULT_JIGGLE_SLICES, duration)
  179. )
  180. servo = servo_kit.servo[channel]
  181. # Jiggle
  182. for slice_index in range(self.__DEFAULT_JIGGLE_SLICES):
  183. angle = 0 + (degrees_per_slice * slice_index)
  184. servo.angle = angle
  185. time.sleep(seconds_per_slice)
  186. # Center
  187. servo.angle = 90
  188. def make_default_mappings_output_file_path(self):
  189. output_file_path = os.path.join(
  190. "output",
  191. self.__DEFAULT_OUTPUT_FILE_NAME
  192. )
  193. return output_file_path
  194. def write_mappings(self):
  195. output_file_path = self.__output_file_path
  196. self.__logger.info("Writing mappings to output file: %s" % (output_file_path,))
  197. data = {
  198. self.__DEFAULT_SERVO_CHANNEL_MAPPING_KEY: self.__mappings
  199. }
  200. with open(output_file_path, 'w') as f:
  201. yaml.dump(data, f, default_flow_style=False)
  202. def load_mappings(self, file_path=None):
  203. if file_path is None:
  204. file_path = self.__output_file_path
  205. self.__logger.info("Attempting to load mappings from: %s" % (file_path,))
  206. with open(file_path) as f:
  207. loaded_data = yaml.safe_load(f)
  208. if self.__DEFAULT_SERVO_CHANNEL_MAPPING_KEY not in loaded_data.keys():
  209. self.__logger.warning("Could not find key 'servo-mappings' in loaded data")
  210. return
  211. mappings = loaded_data[self.__DEFAULT_SERVO_CHANNEL_MAPPING_KEY]
  212. if not isinstance(mappings, dict):
  213. self.__logger.warning("Mappings aren't in dict format; Won't load")
  214. return
  215. for name in mappings.keys():
  216. self.__logger.info("Examining mapping: %s" % (name,))
  217. channel = mappings[name]
  218. if not isinstance(channel, int):
  219. self.__logger.warning("Mapping isn't an integer; Ignoring: %s" % channel)
  220. continue
  221. self.__logger.info("Loading mapping: %s ==> %s" % (name, channel))
  222. self.set_name_mapping(name=name, channel=channel)
  223. self.__logger.info("Done loading mappings")