czwartek, 23 listopada 2017

Program sterujący "układu jezdnego" :->

Kurde ale trzeba być durniem, żeby pisać blog, który czyta może z pięć osób - porażka...

Dziś przetestowałem układ jezdny. Na razie daleko temu jest do autonomicznego robota ale przy dobrych wiatrach wszystko się może uda... jakoś.

W moim ostatnim poście, próbowałem pokazać Wam drodzy czytelnicy jak można połączyć platformę BeagleBone Black z podwoziem produkowanym przez DAGU, czyli - Rover'em 5.
Do połączenia i sterowania silnikami DC użyłem dedykowanej do tego zestawu elektroniki czyli kontrolera silników DAGU.
Cóż jeszcze - wykonałem krótki test, prostym programikiem napisanym w Pythonie i pomęczyłem się z niedziałającymi obrazami systemu Debian, który w tym komputerku "siedzi" i jego konfiguracją - sporo tego było - niemal miesiąc ślęczenia praktycznie w miejscu i związanej z tym frustracji :(

Jeśli ktoś mnie teraz zapyta czy przyszedł czas na udanie się do "WC dla personelu" to - nie będzie w błędzie. Otóż podczas pisania tego tekstu chorowałem na tak zwaną "jelitówkę" i generalnie oprócz osłabienia, przesiadywałem głównie w "Świątyni dumania", więc miałem sporo czasu na myślenie :-D Pomiędzy posiedzeniami tam gdzie "Król chadza bez korony", wykorzystywałem swój wolny czas (a taki wolny czas jest cenny), bowiem pozwoliło na dalsze prace nad platformą i ostateczne poprawki w napisanym kodzie programu sterującego w Pythonie, oraz programu, który przetestuje moje założenia i mniej więcej jak będzie wyglądało sterowanie robotem.
Zastanawiam się tylko czy nie być "rasowym Cebulakiem" i udostępnić kod pospólstwu i tak chyba zrobię :-)
Każdy wie, że każda maszyna cyfrowa (w tym komputer robota) nie umie nic bez oprogramowania. Należy więc zdefiniować podstawowe ruchy maszyny jak np.: jazda do przodu, do tyłu, skręcanie w prawo i lewo, przyśpieszanie, zwalnianie itd. itp. Oczywiście założyłem, że zmęczę to wszystko w Pythonie, bowiem jest bardzo dużo ciekawych przykładów po sieci i zawsze czegoś nowego się dowiem. :)
W tej części przedstawiam moduł sterujący robotem. Plik dla przykładu nazwałem: rover_beaglebone_v2.py.
Wersja 2 bo przymiarki i kombinacje z tym jak to uruchomić już od nieomal wakacji uskuteczniałem, no i skutkiem tych "prac koncepcyjno - studialnych" => (ale suchar :/) - powstały w sumie trzy wersje tego programu różnie działające, mniej lub bardziej zadawalające mnie. Ta ostatnia "v2" (nie mylić z niemieckimi rakietami do zabijania von Brauna) to efekt właśnie posiedzeń na "batyskafie" w towarzystwie kolegów rotawirusów :)
Oczywiście dokładnie też postaram się opisać co i jak działa ale to może już nie w tym poście... tylko w następnym - bo tak i już ha ha :)
Kod programu:
  1. import Adafruit_BBIO.PWM as PWM
  2. import Adafruit_BBIO.GPIO as GPIO
  3. import time

  5. PIN_SPEED_CH1_Right = "P9_14"
  6. PIN_SPEED_CH2_Left = "P9_16"
  7. PIN_SPEED_CH3_Right = "P8_19"
  8. PIN_SPEED_CH4_Left = "P8_13"
  9. PIN_DIR_CH1_Right = "P9_27"
  10. PIN_DIR_CH2_Left = "P9_30"
  11. PIN_DIR_CH3_Right = "P8_17"
  12. PIN_DIR_CH4_Left = "P8_15"
  13. MAX_SPEED = 100
  14. MIN_SPEED = 23
  15. CHANGE_RATE = 5
  16. STOP_SPEED = 0
  17. FORWARD_DIR = 1
  18. REVERSE_DIR = 0

  20. current_speed_CH1_Right = STOP_SPEED
  21. current_speed_CH2_Left = STOP_SPEED
  22. current_speed_CH3_Right = STOP_SPEED
  23. current_speed_CH4_Left = STOP_SPEED
  24. current_dir_CH1_Right = FORWARD_DIR
  25. current_dir_CH2_Left = FORWARD_DIR
  26. current_dir_CH3_Right = FORWARD_DIR
  27. current_dir_CH4_Left = FORWARD_DIR

  29. #zainicjowanie ruchu podwozia ROVER ustawienie stanu pinów
  30. def init_rover():
  31. PWM.start(PIN_SPEED_CH1_Right, 0)
  32. PWM.start(PIN_SPEED_CH2_Left, 0)
  33. PWM.start(PIN_SPEED_CH3_Right, 0)
  34. PWM.start(PIN_SPEED_CH4_Left, 0)
  35. GPIO.setup(PIN_DIR_CH1_Right, GPIO.OUT)
  36. GPIO.setup(PIN_DIR_CH2_Left, GPIO.OUT)
  37. GPIO.setup(PIN_DIR_CH3_Right, GPIO.OUT)
  38. GPIO.setup(PIN_DIR_CH4_Left, GPIO.OUT)

  40. def stop_rover():
  41. all_stop()

  43. #funkcja pomocnicza - sprawdza utrzymanie predkosci obrotowej kol

  45. def check_speed(speed):
  46. if speed < MIN_SPEED and speed != STOP_SPEED:
  47. speed = MIN_SPEED
  48. if speed > MAX_SPEED:
  49. speed = MAX_SPEED
  50. return speed

  52. def fastest_speed_front(): 
  53. global current_speed_CH1_Right, current_speed_CH2_Left
  54. ret_speed_front = current_speed_CH1_Right
  55. if current_speed_CH2_Left > current_speed_CH1_Right:
  56. ret_speed_front = current_speed_CH2_Left
  57. return ret_speed_front

  59. def fastest_speed_rear():
  60. global current_speed_CH3_Right, current_speed_CH4_Left
  61. ret_speed_rear = current_speed_CH3_Right
  62. if current_speed_CH4_Left > current_speed_CH3_Right:
  63. ret_speed_rear = current_speed_CH4_Left
  64. return ret_speed_rear

  66. def slowest_speed_front():
  67. global current_speed_CH1_Right, current_speed_CH2_Left
  68. ret_speed_front = current_speed_CH1_Right
  69. if current_speed_CH2_Left < current_speed_CH1_Right:
  70. ret_speed_front = current_speed_CH2_Left
  71. return ret_speed_front

  73. def slowest_speed_rear():
  74. global current_speed_right_CH3_Right, current_speed_CH4_Left
  75. ret_speed_rear = current_speed_CH3_Right
  76. if current_speed_CH4_Left < current_speed_CH3_Right:
  77. ret_speed_rear = current_speed_CH4_Left
  78. return ret_speed_rear

  80. #set speed - ustaw predkosci kol
  81. def set_right_speed_wheel_front(speed): #prawa gasienica kolo przednie
  82. global current_speed_CH1_Right, current_speed_CH2_Left
  83. new_speed = check_speed(speed)
  84. PWM.set_duty_cycle(PIN_SPEED_CH1_Right,new_speed)
  85. current_speed_CH1_Right = new_speed

  87. def set_right_speed_wheel_rear(speed): #prawa gasienica kolo tylne
  88. global current_speed_CH3_Right, current_speed_CH4_Left
  89. new_speed = check_speed(speed)
  90. PWM.set_duty_cycle(PIN_SPEED_CH3_Right,new_speed)
  91. current_speed_CH3_Right = new_speed

  93. def set_left_speed_wheel_front(speed): #lewa gasienica kolo przednie
  94. global current_speed_CH1_Right, current_speed_CH2_Left
  95. new_speed = check_speed(speed)
  96. PWM.set_duty_cycle(PIN_SPEED_CH2_Left,new_speed)
  97. current_speed_CH2_Left = new_speed

  99. def set_left_speed_wheel_rear(speed): #lewa gasienica kolo tylne
  100. global current_speed_CH3_Right, current_speed_CH4_Left
  101. new_speed = check_speed(speed)
  102. PWM.set_duty_cycle(PIN_SPEED_CH4_Left,new_speed)
  103. current_speed_CH4_left = new_speed

  105. def set_speed(speed):
  106. set_right_speed_wheel_front(speed)
  107. set_right_speed_wheel_rear(speed)
  108. set_left_speed_wheel_front(speed)
  109. set_left_speed_wheel_rear(speed)
  110.     
  111. #Increase speed - Zwekszenie predkosci
  112. def increase_right_speed_wheel_front(): #prawa gasienica kolo przednie
  113. global current_speed_CH1_Right, current_speed_CH2_Left
  114. set_right_speed_wheel_front(current_speed_CH1_Right + CHANGE_RATE)

  116. def increase_right_speed_wheel_rear(): #prawa gasienica kolo tylne
  117. global current_speed_CH3_Right, current_speed_CH4_Left
  118. set_right_speed_wheel_rear(current_speed_CH3_Right + CHANGE_RATE)

  120. def increase_left_speed_wheel_front(): #lewa gasienica kolo przednie
  121. global current_speed_CH1_Right, current_speed_CH2_Left
  122. set_left_speed_wheel_front(current_speed_CH2_Left + CHANGE_RATE)

  124. def increase_left_speed_wheel_rear(): #lewa gasienica kolo tylne
  125. global current_speed_CH3_Right, current_speed_CH4_Left
  126. set_left_speed_wheel_rear(current_speed_CH4_Left + CHANGE_RATE)

  128. def increase_speed():
  129. increase_right_speed_wheel_front()
  130. increase_right_speed_wheel_rear()
  131. increase_left_speed_wheel_front()
  132. increase_left_speed_wheel_rear()

  134. #Decrease speed - Zmniejszenie predkosci
  135. def decrease_right_speed_wheel_front(): #prawa gasienica kolo przednie
  136. global current_speed_CH1_Right, current_speed_CH2_Left
  137. set_right_speed_wheel_front(current_speed_CH1_Right - CHANGE_RATE)

  139. def decrease_right_speed_wheel_rear(): #prawa gasienica kolo tylne
  140. global current_speed_CH3_Right, current_speed_CH4_Left
  141. set_right_speed_wheel_rear(current_speed_CH3_Right - CHANGE_RATE)

  143. def decrease_left_speed_wheel_front(): #lewa gasienica kolo przednie
  144. global current_speed_CH1_Right, current_speed_CH2_Left
  145. set_left_speed_wheel_front(current_speed_CH2_Left - CHANGE_RATE)

  147. def decrease_left_speed_wheel_rear(): #lewa gasienica kolo tylne
  148. global current_speed_CH3_Right, current_speed_CH4_Left
  149. set_left_speed_wheel_rear(current_speed_CH4_Left - CHANGE_RATE)

  151. def decrease_speed():
  152. decrease_right_speed_wheel_front()
  153. decrease_right_speed_wheel_rear()
  154. decrease_left_speed_wheel_front()
  155. decrease_left_speed_wheel_rear()

  157. #Set direction forward - Ustawianie kierunku do przodu
  158. def forward_right_dir_wheel_front(): #naprzod przednie kolo prawe
  159. global current_dir_CH1_Right, current_dir_CH2_Left
  160. if current_dir_CH1_Right == REVERSE_DIR:
  161. all_stop()
  162. GPIO.output(PIN_DIR_CH1_Right, GPIO.HIGH) #LOW
  163. current_dir_CH1_Right = FORWARD_DIR

  165. def forward_right_dir_wheel_rear(): #naprzod tylne kolo prawe
  166. global current_dir_CH3_Right, current_dir_CH4_Left
  167. if current_dir_CH3_Right == REVERSE_DIR:
  168. all_stop()
  169. GPIO.output(PIN_DIR_CH3_Right, GPIO.LOW)
  170. current_dir_CH3_Right = FORWARD_DIR

  172. def forward_left_dir_wheel_front(): #naprzod przenie kolo lewe
  173. global current_dir_CH1_Right, current_dir_CH2_Left
  174. if current_dir_CH2_Left == REVERSE_DIR:
  175. all_stop()
  176. GPIO.output(PIN_DIR_CH2_Left, GPIO.HIGH)
  177. current_dir_CH2_Left = FORWARD_DIR

  179. def forward_left_dir_wheel_rear(): #naprzod tylne kolo lewe
  180. global current_dir_CH3_Right, current_dir_CH4_Left
  181. if current_dir_CH4_Left == REVERSE_DIR:
  182. all_stop()
  183. GPIO.output(PIN_DIR_CH4_Left, GPIO.LOW)
  184. current_dir_CH4_Left = FORWARD_DIR

  186. def forward_dir():
  187. forward_right_dir_wheel_front()
  188. forward_right_dir_wheel_rear()
  189. forward_left_dir_wheel_front()
  190. forward_left_dir_wheel_rear()
  192. #Set reverse direction - Ustawienie kierunku do tylu
  193. def reverse_right_dir_wheel_front(): #wstecz przednie kolo prawe
  194. global current_dir_CH1_Right, current_dir_CH2_Left
  195. if current_dir_CH1_Right == FORWARD_DIR:
  196. all_stop()
  197. GPIO.output(PIN_DIR_CH2_Left, GPIO.LOW) # HIGH
  198. current_dir_CH1_Right = REVERSE_DIR

  200. def reverse_right_dir_wheel_rear(): #wstecz tylne kolo prawe
  201. global current_dir_CH3_Right, current_dir_CH4_Left
  202. if current_dir_CH3_Right == FORWARD_DIR:
  203. all_stop()
  204. GPIO.output(PIN_DIR_CH4_Left, GPIO.HIGH)
  205. current_dir_CH3_Right = REVERSE_DIR

  207. def reverse_left_dir_wheel_front(): #wstecz przednie kolo lewe
  208. global current_dir_CH1_Right, current_dir_CH2_Left
  209. if current_dir_CH2_Left == FORWARD_DIR:
  210. all_stop()
  211. GPIO.output(PIN_DIR_CH1_Right, GPIO.LOW) # HIGH
  212. current_dir_CH2_Left = REVERSE_DIR

  214. def reverse_left_dir_wheel_rear(): #wstecz tylne kolo lewe
  215. global current_dir_CH3_Right, current_dir_CH4_Left
  216. if current_dir_CH4_Left == FORWARD_DIR:
  217. all_stop()
  218. GPIO.output(PIN_DIR_CH3_Right, GPIO.HIGH) #or HIGH
  219. current_dir_CH4_Left = REVERSE_DIR

  221. def reverse_dir():
  222. reverse_right_dir_wheel_front()
  223. reverse_right_dir_wheel_rear()
  224. reverse_left_dir_wheel_front()
  225. reverse_left_dir_wheel_rear()

  227. #Stop rover - Zatrzymanie rovera
  228. def stop_left_wheel_front():
  229. set_left_speed_wheel_front(STOP_SPEED)

  231. def stop_left_wheel_rear():
  232. set_left_speed_wheel_rear(STOP_SPEED)

  234. def stop_right_whell_front():
  235. set_right_speed_wheel_front(STOP_SPEED)

  237. def stop_right_wheel_rear():
  238. set_right_speed_wheel_rear(STOP_SPEED)

  240. def all_stop():
  241. stop_left_wheel_front()
  242. stop_left_wheel_rear()
  243. stop_right_whell_front()
  244. stop_right_wheel_rear()

  246. #Full speed - Cala naprzod
  247. def full_speed_left_wheel_front():
  248. set_left_speed_wheel_front(MAX_SPEED)

  250. def full_speed_left_wheel_rear():
  251. set_left_speed_wheel_rear(MAX_SPEED)

  253. def full_speed_right_wheel_front():
  254. set_right_speed_wheel_front(MAX_SPEED)

  256. def full_speed_right_wheel_rear():
  257. set_right_speed_wheel_rear(MAX_SPEED)

  259. def all_full_speed():
  260. full_speed_left_wheel_front()
  261. full_speed_left_wheel_rear()
  262. full_speed_right_wheel_front()
  263. full_speed_right_wheel_rear()

  265. #Spin rover - Obracanie rovera - skrecanie - zawracanie
  266. def spin_right(speed):
  267. all_stop()
  268. forward_dir()
  269. forward_right_dir_wheel_front()
  270. forward_right_dir_wheel_rear()
  271. reverse_left_dir_wheel_front()
  272. reverse_left_dir_wheel_rear()
  273. set_right_speed_wheel_front(speed)
  274. set_right_speed_wheel_rear(speed)
  275. set_left_speed_wheel_front(speed)
  276. set_left_speed_wheel_rear(speed)

  278. def spin_left(speed):
  279. all_stop()
  280. forward_dir()
  281. forward_left_dir_wheel_front()
  282. forward_left_dir_wheel_rear()
  283. reverse_right_dir_wheel_front()
  284. reverse_right_dir_wheel_rear()
  285. set_right_speed_wheel_front(speed)
  286. set_right_speed_wheel_rear(speed)
  287. set_left_speed_wheel_front(speed)
  288. set_left_speed_wheel_rear(speed)
Rozumiecie??? :-D Widzicie??? :-D
No to jak rozumiecie i widzicie to wszystko po kolei wyjaśnię:)
Do kontroli pinów z tak zwanych przeze mnie "hederów" P8 i P9 BBB, używam biblioteki Adafruit BBIO omówionej parę postów wcześniej.
Ale ponieważ już nie chce mi się pisać więcej to resztę omówię przy okazji następnego postu.
Łostańcie z Bogiem.
Autor: o

Brak komentarzy:

Prześlij komentarz

Subskrybuj: Komentarze do posta (Atom)