Cánh tay robot học lệnh 5 bậc tự do

Đoàn Văn Huân 22/06/2019
canh-tay-robot-hoc-lenh-5-bac-tu-do

Robot arm, robot nhện, robot múa.. hay các robot mini có sử dụng động cơ servo đều là những sản phẩm gây ấn tượng với những chuyển động đẹp mắt. Đúng như tiêu chí của ARDUINO, mình sẽ làm một dự án ROBOT tự học lệnh cực kì COOL. 

Về robot tự học lệnh

Đây sẽ là một robot sử dụng động cơ servo như một phần chi tiết của nó.

Ta sẽ sử dụng chiết áp như một cảm biến góc.

Điều khiển servo ở hai chế độ:

  • Bằng tay: Sử dụng cảm biến để điều khiển trực tiếp.
  • Tự động: Dạy cho servo rồi để nó tự thực hiện lại động tác mà nó đã học.

Chuẩn bị

Sơ đồ mạch

Code

  1. #define servo_max 5
  2. // số step lớn nhất , nó sẽ tiêu tốn step_max*servo_max (byte) RAM
  3. // ví dụ cần 200*5=1000 byte RAM
  4. #define step_max 200
  5. // nút ấn
  6. #define start_pause_pin 12
  7. #define record_pin 7
  8. #define ENABLE_EEPROM_PIN 2
  9. // đèn báo
  10. #define led_pin 13
  11. // tốc độ nhanh chậm
  12. #define delay_toc_do 20
  13. //cài pin vào analog
  14. byte pin_analog[servo_max] = { A0, A1, A2, A3, A4 };
  15. // cài đặt pin ra cho servo
  16. byte pin_servo[servo_max] = { 3, 5, 6, 9, 10 };
  17. // có 5 servo
  18. unsigned int A0_value, A1_value, A2_value, A3_value, A4_value;
  19.  
  20. byte get_goc(byte servo_i)
  21. {
  22. // tính toán lấy giá trị góc của biến trở i
  23. switch (servo_i) {
  24. case 0:
  25. A0_value = constrain(analogRead(A0), 200, 823);
  26. return map(A0_value, 200, 823, 4, 175);
  27. break;
  28. case 1:
  29. A1_value = constrain(analogRead(A1), 200, 823);
  30. return map(A1_value, 200, 823, 4, 175);
  31. break;
  32. case 2:
  33. A2_value = constrain(analogRead(A2), 200, 823);
  34. return map(A2_value, 200, 823, 175, 4);
  35. break;
  36. case 3:
  37. A3_value = constrain(analogRead(A3), 200, 823);
  38. return map(A3_value, 200, 823, 175, 4);
  39. break;
  40.  
  41. case 4:
  42. A4_value = constrain(analogRead(A4), 200, 823);
  43. return map(A4_value, 200, 823, 100, 175); // tay gắp, góc quay 100°->180°
  44. break;
  45. default:
  46. break;
  47. }
  48. }
  49. // kích thước eeprom (số byte) trên arduino của bạn
  50. // trong ví dụ này mình dùng arduino uno r3 có 1024 bytes EEPROM
  51. const unsigned int SIZE_MEMORY_EEPROM = 1024;
  52.  
  53. ///////////////////////////////////////////////////////////////////////////////////
  54. /////////////////////////MAIN CODE- BẠN KHÔNG CẦN CHỈNH SỬA PHẦN NÀY////////////////////////////////////////////
  55. //////////////////////////////////////////////////////////////////////////////////
  56. #include <EEPROM.h>
  57. #include <Servo.h>
  58.  
  59. Servo servo[servo_max];
  60. //pin button
  61. int step_move = 0;
  62. /* mảng lưu góc cho 5 servo, 90 là giá trị góc khởi tạo cho toàn bộ phần tử*/
  63.  
  64. byte goc_servo[servo_max][step_max];
  65. // mảng 2 chiều quản lý (servo_max) servo
  66. byte goc_tam_thoi[servo_max] = { 90 };
  67. // lưu góc tạm thời tại thời điểm cần tính
  68.  
  69. void setup()
  70. {
  71.  
  72. pinMode(led_pin, OUTPUT);
  73. //2 pin button
  74. //Serial.begin(9600);
  75. pinMode(start_pause_pin, INPUT_PULLUP);
  76. pinMode(ENABLE_EEPROM_PIN, INPUT_PULLUP);
  77. pinMode(record_pin, INPUT_PULLUP);
  78. // cài chế độ 5 pin analog
  79. for (byte i = 0; i < servo_max; i++) {
  80. pinMode(pin_analog[i], INPUT);
  81. servo[i].attach(pin_servo[i]);
  82. }
  83.  
  84. ADCSRA = ((ADCSRA & (B11111000)) | B00000100); // Cài tần số quét analog 1mhz
  85.  
  86. learn_and_move(); // chạy hàm
  87. }
  88.  
  89. void move_servo(byte i, byte goc_i)
  90. {
  91. servo[i].write(goc_i);
  92. }
  93. void nhap_nhay(unsigned int time_delay, byte count)
  94. {
  95. for (byte i = 0; i < count; i++) {
  96.  
  97. digitalWrite(led_pin, HIGH);
  98. delay(time_delay);
  99. digitalWrite(led_pin, LOW);
  100. delay(time_delay * 2);
  101. }
  102. }
  103. void record(unsigned int step_x)
  104. {
  105. for (byte i = 0; i < servo_max; i++) {
  106. goc_servo[i][step_x] = get_goc(i);
  107. }
  108. nhap_nhay(100, 2);
  109. Serial.println(goc_servo[0][step_x]);
  110. }
  111. void move_all()
  112. {
  113.  
  114. for (byte i = 0; i < servo_max; i++) {
  115. move_servo(i, goc_tam_thoi[i]);
  116. } // di chuyển
  117. }
  118. void control_servo()
  119. {
  120. byte hieu;
  121. for (byte i = 0; i < servo_max; i++) {
  122. hieu = abs(goc_tam_thoi[i] - get_goc(i));
  123. if ((hieu >= 1) && (hieu < 170)) { // chống nhiễu
  124. // bạn cũng không được di chuyển arm điều khiển quá nhanh
  125.  
  126. goc_tam_thoi[i] = get_goc(i);
  127. }
  128. } // lấy góc
  129. move_all();
  130. }
  131.  
  132. void nap_eeprom_sang_ram()
  133. {
  134.  
  135. // nạp eeprom sang RAM
  136.  
  137. step_move = EEPROM.read(SIZE_MEMORY_EEPROM - 1); // lấy lại step_move từ rom
  138. for (byte i = 0; i < servo_max; i++) {
  139.  
  140. for (int step_j = 0; step_j < step_max; step_j++) {
  141. goc_servo[i][step_j] = EEPROM.read(step_j + i * step_max);
  142. //nạp dữ liệu từ rom sang ram
  143. }
  144. }
  145. }
  146.  
  147. void luu_vao_eeprom()
  148. {
  149. // bước 1: lưu dữ liệu vào rom
  150.  
  151. EEPROM.write(SIZE_MEMORY_EEPROM - 1, step_move); // ghi step_move vào rom
  152. delay(15); // đợi 15ms để hoàn thành ghi 1 ô nhớ
  153.  
  154. nhap_nhay(500, 3);
  155. digitalWrite(led_pin, HIGH); // giữ nguyên đèn
  156. for (byte i = 0; i < servo_max; i++) {
  157. //Serial.print("luu");
  158. //Serial.println(i);
  159. for (int step_j = 0; step_j < step_move; step_j++) {
  160. EEPROM.write(step_j + i * step_max, goc_servo[i][step_j]);
  161. //nạp dữ liệu từ ram vào rom
  162. delay(15); // đợi 15ms để hoàn thành ghi 1 ô nhớ
  163. }
  164. }
  165.  
  166. nhap_nhay(500, 3);
  167.  
  168. digitalWrite(led_pin, LOW); // tắt đèn
  169. }
  170.  
  171. void pause()
  172. {
  173. // chỉ được lưu dữ liệu vào eeprom khi đã có dữ liệu
  174. // nhấn pause trước, sau đó mới nhấn nút ENABLE_EEPROM_PIN để bắt đầu ghi vào eeprom
  175. //
  176. if (digitalRead(start_pause_pin) == 0) {
  177.  
  178. while (true) {
  179. digitalWrite(led_pin, 1);
  180. delay(300); // chống nhiễu
  181. Serial.println("PAUSE");
  182.  
  183. if (digitalRead(ENABLE_EEPROM_PIN) == 0) {
  184. // nhấn lưu eeprom
  185. delay(300);
  186. luu_vao_eeprom();
  187. }
  188.  
  189. if (digitalRead(start_pause_pin) == 0) {
  190. Serial.println("START");
  191. digitalWrite(led_pin, 0);
  192. delay(300); // chống nhiễu
  193. goto out_pause;
  194. }
  195.  
  196. } //while
  197. } //if
  198. out_pause:;
  199. // thoát lặp
  200. }
  201.  
  202. void auto_move()
  203. {
  204.  
  205. Serial.println(step_move);
  206. nhap_nhay(50, 3);
  207. float hieu_f[servo_max];
  208. unsigned int step = 0, step_next;
  209.  
  210. byte time;
  211. byte thay_doi;
  212. while (true) {
  213. //lấy hiệu góc hiện tại và góc sau
  214.  
  215. if (step < step_move) {
  216. step_next = step + 1; // không viết : step++
  217. }
  218. else {
  219. /*step = step_mov :thì step tiếp theo của step cuối cùng là step đầu tiên*/
  220. step_next = 0;
  221. }
  222.  
  223. for (byte i = 0; i < servo_max; i++) {
  224.  
  225. hieu_f[i] = (float(goc_servo[i][step]) - float(goc_servo[i][step_next]));
  226. }
  227.  
  228. Serial.println(step);
  229. Serial.println(goc_servo[0][step]);
  230. Serial.println(goc_servo[0][step_next]);
  231. Serial.println(hieu_f[0]);
  232. //int denta;
  233. /*vận tốc sẽ tăng dần khi khởi đầu, đạt max , vận tốc giảm dần khi ở cuối quá trình,
  234.  
  235. */
  236. for (float loading = 1.0; loading <= 100.0; loading++) {
  237.  
  238. //denta=30+((sq(loading-150))/1000);
  239. // loading là phần trăm %, đánh giá kết thúc 1 động tác là 100%
  240. for (byte i = 0; i < servo_max; i++) {
  241.  
  242. goc_tam_thoi[i] = byte(float(goc_servo[i][step]) - (((hieu_f[i]) * loading) / 100.0));
  243. }
  244.  
  245. Serial.print(loading);
  246. Serial.print("‚");
  247. Serial.print(hieu_f[0]);
  248. Serial.print("‚");
  249. Serial.println(goc_tam_thoi[0]);
  250. // gia tốc từ chậm->nhanh->chậm
  251.  
  252. if ((loading >= 0.0) && (loading < 20.0)) {
  253. time = delay_toc_do * 2 + 5;
  254. }
  255. else if (loading < 30.0) {
  256. time = delay_toc_do / 2 + 5;
  257. }
  258. else if (loading < 90.0) {
  259. time = delay_toc_do + 5;
  260. }
  261. else {
  262. time = delay_toc_do * 2 + 5;
  263. }
  264.  
  265. // delay(delay_toc_do);
  266. while ((millis() % delay_toc_do) != 0) { // làm trễ
  267. /* cứ sau denta_ms, vòng lặp mới được thoát, */
  268.  
  269. pause();
  270. } //while
  271.  
  272. /*
  273. Đoạn code < delay(time) >sẽ tương đương với < while((millis()%time)!=0){;} >
  274. - Lý do mình mình không chọn delay :về bản chất Delay sẽ vô hiệu hóa hoàn toàn chương tình arduino ,
  275. arduino sẽ không thể làm gì cho đến khi hết Delay.
  276. Trong khi đó chúng ta muốn ấn nút PAUSE(tạm dừng robot) ngay lập tức Robot sẽ không dừng ngay vì bị Delay vô hiệu hóa.
  277. Đấy là lí do mình đã cho hàm pause() lồng vào bên trong khối while() như bạn đã thấy.
  278. */
  279. move_all();
  280.  
  281.  
  282. } // for_loading
  283.  
  284. if (step < step_move) {
  285. step++; // tăng cho lần kế tiếp
  286. }
  287. else {
  288. /* step = step_move*/
  289. step = 0; // lại từ đầu
  290. }
  291.  
  292. //kết thúc 1 động tác
  293. delay(100);
  294. } //while
  295. }
  296.  
  297. void learn_and_move()
  298. {
  299. step_move = 0;
  300.  
  301. while (digitalRead(start_pause_pin) != 0) {
  302. //b1: điều khiển servo bằng biến trở
  303. while (digitalRead(record_pin) != 0) {
  304. control_servo();
  305.  
  306. if ((digitalRead(start_pause_pin) == 0) && (step_move == 0)) {
  307.  
  308. // nếu chưa có cài đặt náo mà vô thẳng phần chạy thì hiểu : lấy cài đặt từ eeprom
  309. nap_eeprom_sang_ram();
  310. }
  311.  
  312. if ((digitalRead(start_pause_pin) == 0) && (step_move != 0)) {
  313. goto buoc_3;
  314. }
  315. //delay(1);// bỏ delay luôn
  316. }
  317. // nút record đươcj nhấn, thoát lặp
  318. // b2: lưu vào mảng
  319. record(step_move);
  320. if (step_move < step_max) {
  321. step_move++; // tăng step cho bước sau
  322. }
  323. }
  324. // nút start_pause được nhấn, thoát lặp
  325. buoc_3:
  326.  
  327. step_move--; //không có bước sau, giảm step_move xuống 1 đơn vị
  328. //b4:auto move
  329. auto_move();
  330. }
  331.  
  332. void loop()
  333. {
  334.  
  335. } // loop
  • Hàm quy đổi MAP được mình cài đặt phù hợp với con robot của mình, các bạn có thể cài lại cho phù hợp với dự án.
  • Bạn chỉ cần thêm vài Code để thêm servo , kể từ phần MAIN CODE trở xuống bạn không cần làm gì thêm ( nếu chú ý sẽ thấy phần Main Code ở cả 2 ví dụ trên là giống nhau).
  • Việc thêm số lượng servo cũng tương tự nhé.
  • Cấu trúc phân vùng nhớ:

(Sưu tầm)

Cánh tay robot
Bình luận
Nội dung này chưa có bình luận, hãy gửi bình luận đầu tiên của bạn.
VIẾT BÌNH LUẬN CỦA BẠN