Cách điều khiển servo tự học lệnh và ghi trêm EEPROM

Đoàn Văn Huân 22/06/2019
cach-dieu-khien-servo-tu-hoc-lenh-va-ghi-trem-eeprom

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ị

Mạch

Code test

  1. //tạo 1 đối tượng servo từ dưới lên trên
  2. #define servo_max 1
  3. // số step lớn nhất , nó sẽ tiêu tốn step_max*servo_max (byte) RAM
  4. // ví dụ cần 200*1=200 byte RAM
  5. #define step_max 200
  6. // nút ấn
  7. #define start_pause_pin 12
  8. #define record_pin 7
  9. #define ENABLE_EEPROM_PIN 2
  10. // đèn báo
  11. #define led_pin 13
  12. // tốc độ nhanh chậm
  13. #define delay_toc_do 5
  14. //cài pin vào analog
  15. byte pin_analog[servo_max] = { A0 };
  16. // cài đặt pin ra cho servo
  17. byte pin_servo[servo_max] = { 3 };
  18. // có 5 servo
  19. unsigned int A0_value;
  20.  
  21. byte get_goc(byte servo_i)
  22. {
  23. // tính toán lấy giá trị góc của biến trở i
  24. switch (servo_i) {
  25. case 0:
  26. A0_value = constrain(analogRead(A0), 200, 823);
  27. return map(A0_value, 200, 823, 0, 180);
  28. break;
  29. default:
  30. break;
  31. }
  32. }
  33. // kích thước eeprom (số byte) trên arduino của bạn
  34. // trong ví dụ này mình dùng arduino uno r3 có 1024 bytes EEPROM
  35. const unsigned int SIZE_MEMORY_EEPROM = 1024;

Code nạp Arduino bên dưới

  1. #include <EEPROM.h>
  2. #include <Servo.h>
  3.  
  4. Servo servo[servo_max];
  5. //pin button
  6. int step_move = 0;
  7. /* 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ử*/
  8.  
  9. byte goc_servo[servo_max][step_max];
  10. // mảng 2 chiều quản lý (servo_max) servo
  11. byte goc_tam_thoi[servo_max] = { 90 };
  12. // lưu góc tạm thời tại thời điểm cần tính
  13.  
  14. void setup()
  15. {
  16.  
  17. pinMode(led_pin, OUTPUT);
  18. //2 pin button
  19. //Serial.begin(9600);
  20. pinMode(start_pause_pin, INPUT_PULLUP);
  21. pinMode(ENABLE_EEPROM_PIN, INPUT_PULLUP);
  22. pinMode(record_pin, INPUT_PULLUP);
  23. // cài chế độ 5 pin analog
  24. for (byte i = 0; i < servo_max; i++) {
  25. pinMode(pin_analog[i], INPUT);
  26. servo[i].attach(pin_servo[i]);
  27. }
  28.  
  29. ADCSRA = ((ADCSRA & (B11111000)) | B00000100); // Cài tần số quét analog 1mhz
  30.  
  31. learn_and_move(); // chạy hàm
  32. }
  33.  
  34. void move_servo(byte i, byte goc_i)
  35. {
  36. servo[i].write(goc_i);
  37. }
  38. void nhap_nhay(unsigned int time_delay, byte count)
  39. {
  40. for (byte i = 0; i < count; i++) {
  41.  
  42. digitalWrite(led_pin, HIGH);
  43. delay(time_delay);
  44. digitalWrite(led_pin, LOW);
  45. delay(time_delay * 2);
  46. }
  47. }
  48. void record(unsigned int step_x)
  49. {
  50. for (byte i = 0; i < servo_max; i++) {
  51. goc_servo[i][step_x] = get_goc(i);
  52. }
  53. nhap_nhay(100, 2);
  54. Serial.println(goc_servo[0][step_x]);
  55. }
  56. void move_all()
  57. {
  58.  
  59. for (byte i = 0; i < servo_max; i++) {
  60. move_servo(i, goc_tam_thoi[i]);
  61. } // di chuyển
  62. }
  63. void control_servo()
  64. {
  65. byte hieu;
  66. for (byte i = 0; i < servo_max; i++) {
  67. hieu = abs(goc_tam_thoi[i] - get_goc(i));
  68. if ((hieu >= 1) && (hieu < 170)) { // chống nhiễu
  69. // bạn cũng không được di chuyển arm điều khiển quá nhanh
  70.  
  71. goc_tam_thoi[i] = get_goc(i);
  72. }
  73. } // lấy góc
  74. move_all();
  75. }
  76.  
  77. void nap_eeprom_sang_ram()
  78. {
  79.  
  80. // nạp eeprom sang RAM
  81.  
  82. step_move = EEPROM.read(SIZE_MEMORY_EEPROM - 1); // lấy lại step_move từ rom
  83. for (byte i = 0; i < servo_max; i++) {
  84.  
  85. for (int step_j = 0; step_j < step_max; step_j++) {
  86. goc_servo[i][step_j] = EEPROM.read(step_j + i * step_max);
  87. //nạp dữ liệu từ rom sang ram
  88. }
  89. }
  90. }
  91.  
  92. void luu_vao_eeprom()
  93. {
  94. // bước 1: lưu dữ liệu vào rom
  95.  
  96. EEPROM.write(SIZE_MEMORY_EEPROM - 1, step_move); // ghi step_move vào rom
  97. delay(15); // đợi 15ms để hoàn thành ghi 1 ô nhớ
  98.  
  99. nhap_nhay(500, 3);
  100. digitalWrite(led_pin, HIGH); // giữ nguyên đèn
  101. for (byte i = 0; i < servo_max; i++) {
  102. //Serial.print("luu");
  103. //Serial.println(i);
  104. for (int step_j = 0; step_j < step_move; step_j++) {
  105. EEPROM.write(step_j + i * step_max, goc_servo[i][step_j]);
  106. //nạp dữ liệu từ ram vào rom
  107.  
  108. delay(15); // đợi 15ms để hoàn thành ghi 1 ô nhớ
  109. }
  110. }
  111.  
  112. nhap_nhay(500, 3);
  113.  
  114. digitalWrite(led_pin, LOW); // tắt đèn
  115. }
  116.  
  117. void pause()
  118. {
  119. // chỉ được lưu dữ liệu vào eeprom khi đã có dữ liệu
  120. // nhấn pause trước, sau đó mới nhấn nút ENABLE_EEPROM_PIN để bắt đầu ghi vào eeprom
  121. //
  122. if (digitalRead(start_pause_pin) == 0) {
  123.  
  124. while (true) {
  125. digitalWrite(led_pin, 1);
  126. delay(300); // chống nhiễu
  127. Serial.println("PAUSE");
  128.  
  129. if (digitalRead(ENABLE_EEPROM_PIN) == 0) {
  130. // nhấn lưu eeprom
  131. delay(300);
  132. luu_vao_eeprom();
  133. }
  134.  
  135. if (digitalRead(start_pause_pin) == 0) {
  136. Serial.println("START");
  137. digitalWrite(led_pin, 0);
  138. delay(300); // chống nhiễu
  139. goto out_pause;
  140. }
  141.  
  142. } //while
  143. } //if
  144. out_pause:;
  145. // thoát lặp
  146. }
  147.  
  148. void auto_move()
  149. {
  150.  
  151. Serial.println(step_move);
  152. nhap_nhay(50, 3);
  153. float hieu_f[servo_max];
  154. unsigned int step = 0, step_next;
  155.  
  156. byte time;
  157. byte thay_doi;
  158. while (true) {
  159. //lấy hiệu góc hiện tại và góc sau
  160.  
  161. if (step < step_move) {
  162. step_next = step + 1; // không viết : step++
  163. }
  164. else {
  165. /*step = step_mov :thì step tiếp theo của step cuối cùng là step đầu tiên*/
  166. step_next = 0;
  167. }
  168.  
  169. for (byte i = 0; i < servo_max; i++) {
  170.  
  171. hieu_f[i] = (float(goc_servo[i][step]) - float(goc_servo[i][step_next]));
  172. }
  173.  
  174. Serial.println(step);
  175. Serial.println(goc_servo[0][step]);
  176. Serial.println(goc_servo[0][step_next]);
  177. Serial.println(hieu_f[0]);
  178. //int denta;
  179. /*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,
  180.  
  181. */
  182. for (float loading = 1.0; loading <= 100.0; loading++) {
  183.  
  184. //denta=30+((sq(loading-150))/1000);
  185. // loading là phần trăm %, đánh giá kết thúc 1 động tác là 100%
  186. for (byte i = 0; i < servo_max; i++) {
  187.  
  188. goc_tam_thoi[i] = byte(float(goc_servo[i][step]) - (((hieu_f[i]) * loading) / 100.0));
  189. }
  190.  
  191. Serial.print(loading);
  192. Serial.print("‚");
  193. Serial.print(hieu_f[0]);
  194. Serial.print("‚");
  195. Serial.println(goc_tam_thoi[0]);
  196. // gia tốc từ chậm->nhanh->chậm
  197.  
  198. if ((loading >= 0.0) && (loading < 20.0)) {
  199. time = delay_toc_do * 2 + 5;
  200. }
  201. else if (loading < 30.0) {
  202. time = delay_toc_do / 2 + 5;
  203. }
  204. else if (loading < 90.0) {
  205. time = delay_toc_do + 5;
  206. }
  207. else {
  208. time = delay_toc_do * 2 + 5;
  209. }
  210.  
  211. // delay(delay_toc_do);
  212. while ((millis() % delay_toc_do) != 0) { // làm trễ
  213. /* cứ sau denta_ms, vòng lặp mới được thoát, */
  214.  
  215. pause();
  216. } //while
  217.  /*
  218.             Đoạn code < delay(time) >sẽ tương đương với < while((millis()%time)!=0){;} >
  219. - 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 ,
  220. arduino sẽ không thể làm gì cho đến khi hết Delay. 
  221. 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.
  222. Đấ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. 
  223.             */
  224. move_all();
  225.  
  226.  
  227. } // for_loading
  228.  
  229. if (step < step_move) {
  230. step++; // tăng cho lần kế tiếp
  231. }
  232. else {
  233. /* step = step_move*/
  234. step = 0; // lại từ đầu
  235. }
  236.  
  237. //kết thúc 1 động tác
  238. delay(100);
  239. } //while
  240. }
  241.  
  242. void learn_and_move()
  243. {
  244. step_move = 0;
  245.  
  246. while (digitalRead(start_pause_pin) != 0) {
  247. //b1: điều khiển servo bằng biến trở
  248. while (digitalRead(record_pin) != 0) {
  249. control_servo();
  250.  
  251. if ((digitalRead(start_pause_pin) == 0) && (step_move == 0)) {
  252.  
  253. // 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
  254. nap_eeprom_sang_ram();
  255. }
  256.  
  257. if ((digitalRead(start_pause_pin) == 0) && (step_move != 0)) {
  258. goto buoc_3;
  259. }
  260. //delay(1);// bỏ delay luôn
  261. }
  262. // nút record đươcj nhấn, thoát lặp
  263. // b2: lưu vào mảng
  264. record(step_move);
  265. if (step_move < step_max) {
  266. step_move++; // tăng step cho bước sau
  267. }
  268. }
  269. // nút start_pause được nhấn, thoát lặp
  270. buoc_3:
  271.  
  272. step_move--; //không có bước sau, giảm step_move xuống 1 đơn vị
  273. //b4:auto move
  274. auto_move();
  275. }
  276.  
  277. void loop()
  278. {
  279.  
  280. } // loop

GIẢI THÍCH CODE VÀ CÁCH DÙNG

Code được chia ra làm 2 phần rõ ràng:

  • Thiết lập người dùng: Cài đặt pin kết nối, số bước  để học.
  • Main code: bạn không cần chỉnh sửa phần này (tối ưu chưa nào ^^).

Trong phần tính toán lấy giá trị của biến trở để quy đổi về góc:

map(A0_value, 200,823, 0, 180);

Như đã biết, giá trị trả về của hàm AnalogRead trong khoảng từ 0-1023 (tương ứng mức điện áp 0->5v). Tuy nhiên, để thực hiện ý tưởng điều khiển tuyến tính servo với góc từ 0->180 thì ta phải lấy khoảng giá trị sao cho góc lệch của servo phải tương ứng với  góc lệch của biến trở.

SIZE_MEMORY_EEPROM:  Kích thước eeprom trên arduino của bạn thui.

Main code

 

Các bạn chú ý

Việc sử dụng EEPROM luôn được cân nhắc với tuổi thọ của bộ nhớ EEPROM.

Do đó, nếu yên tâm với bộ nguồn ổn định (USB hoặc PIN) thì không nhất thiết phải lưu dữ liệu vào EEPROM, Về mặt bản chất, việc dùng EEPROM chỉ là sao chép dữ liệu trên bộ nhớ tạm thời (RAM).

Vì vậy, đối với các dự án cần thay thế nguồn (tạm nghỉ robot, robot đồ chơi, robot công nghiệp..) thì chỉ khi thấy ưng ý với code chạy thử,  bạn mới cần đến lưu dữ liệu .

Mình cũng đã sử dụng thành công IC eeprom làm bộ nhớ ngoài cho dự án này.

Vì ic sử dụng giao tiếp I2C nên  2 chân analog A4 và A5 sẽ bị sử dụng để giao tiếp. Bạn không được dùng nó để đặt thêm biến trở.

Video tham khảo

(Sưu tầm)

Cánh tay robot tự học lệnh 4 bậc tự do Ghi dữ liệu vào EEPROM điều khiển servo học lệnh
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