CODE ARDUINO

Arduino cánh tay robot 5 bậc (P2)

Phần tiếp theo liên quan đến cánh tay robot là phần điện tử và điều khiển. Sơ đồ mạch của cánh tay robot tương đối đơn giản bao gồm việc nối 6 ngõ ra của Arduino vào 6 dây điều khiển động cơ servo. Đồng thời chúng ta cần board bluetooth HC-05 dùng để giao tiếp và điều khiển qua bluetooth với điện thoại.

Chúng ta cần sử dụng nguồn ngoài 5V trong trường hợp này vì các module servo và HC-05 cần dòng ít nhất là 2A để có thể hoạt động. Dưới đây là danh sách các linh kiện:

  • MG996R Servo Motor X 3
  • SG90 Micro Servo Motor X 3
  • HC-05 Bluetooth
  • Arduino Board
  • 5V 2A DC Adaptor

Code điều khiển Arduino

Vì đoạn code tương đối dài, để hiểu rõ hơn, chúng ta sẽ chia code của chương trình thành các phần khác nhau và có giải thích cho từng phần. Và cuối bài viết sẽ là source code hoàn chỉnh.

Vì vậy, trước tiên chúng ta cần add thư viện Software Serial để kết nối với mô-đun Bluetooth. Đồng thời ta add thư viện servo. Cả hai thư viện này đều có sẵn trong Arduino IDE, do đó bạn không cần cài đặt chúng. Sau đó, chúng ta cần xác định sáu servo, mô-đun Bluetooth HC-05 và một số biến để lưu trữ vị trí hiện tại và trước đó của các servo, cũng như các mảng để lưu trữ các vị trí hoặc các bước cho chế độ tự động.

#include <SoftwareSerial.h>
#include <Servo.h>
Servo servo01;
Servo servo02;
Servo servo03;
Servo servo04;
Servo servo05;
Servo servo06;
SoftwareSerial Bluetooth(3, 4); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)
// Vị trí hiện tại
int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos;
// Vị trí trước đó
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos;
// Mảng dùng để lưu trữ các bước
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50];
int speedDelay = 20;
int index = 0;
String dataIn = "";

Trong phần setup, chúng ta cần khởi tạo các servo và mô-đun Bluetooth đồng thời di chuyển cánh tay robot đến vị trí ban đầu. Chúng ta thực hiện điều đó bằng cách sử dụng hàm write () dùng để di chuyển servo đến bất kỳ vị trí nào từ 0 đến 180 độ.

void setup() {
  servo01.attach(5);
  servo02.attach(6);
  servo03.attach(7);
  servo04.attach(8);
  servo05.attach(9);
  servo06.attach(10);
  Bluetooth.begin(38400); // Default baud rate of the Bluetooth module
  Bluetooth.setTimeout(1);
  delay(20);
  // Robot arm initial position
  servo1PPos = 90;
  servo01.write(servo1PPos);
  servo2PPos = 150;
  servo02.write(servo2PPos);
  servo3PPos = 35;
  servo03.write(servo3PPos);
  servo4PPos = 140;
  servo04.write(servo4PPos);
  servo5PPos = 85;
  servo05.write(servo5PPos);
  servo6PPos = 80;
  servo06.write(servo6PPos);
}

Tiếp theo, trong phần loop, sử dụng hàm Bluetooth.Available (), chúng ta sẽ liên tục kiểm tra xem có dữ liệu đến từ smart phone hay không. Nếu đúng, sử dụng hàm readString (), chúng ta đọc dữ liệu dưới dạng chuỗi và lưu trữ nó vào biến dataIn. Tùy thuộc vào dữ liệu đến, chúng ta sẽ cho cánh tay robot biết nó cần phải làm gì.

// Check for incoming data
  if (Bluetooth.available() > 0) {
    dataIn = Bluetooth.readString();  // Read the data as string

App điều khiển cánh tay robot – sử dụng app inventor

Cùng nhìn qua xem app sẽ gửi những dữ liệu gì đến cánh tay robot.

Trong trường hợp này cũng ta sử dụng MIT App Inventor để tạo app giao tiếp và điều khiển cánh tay robot. Ở phía trên app sẽ có 2 nút để kết nối với robot thông qua board HC-05. Phía dưới là 6 thanh trượt để điều khiển 6 động cơ servo.

Mỗi thanh trượt có giá trị ban đầu, tối thiểu và tối đa khác nhau phù hợp với các khớp tay robot. Ở dưới cùng của ứng dụng, chúng ta có ba nút, SAVE, RUN và RESET thông qua đó chúng ta có thể lập trình cánh tay robot chạy tự động. Ngoài ra còn có một label bên dưới cho thấy số bước chúng ta đã lưu.

Sau đây, chúng ta đi sâu vào phần code của app một tí. Đầu tiên, ở phía bên trái, chúng ta có các khối để kết nối smart phone với mô-đun Bluetooth.

Sau đó, chúng ta có các khối thanh trượt để điều khiển vị trí servo và các khối nút để lập trình cho cánh tay robot. Vì vậy, nếu chúng ta thay đổi vị trí của thanh trượt, sử dụng chức năng Bluetooth.SendText, chúng ta sẽ gửi một đoạn string tới Arduino. Đoạn string này bao gồm một tiền tố cho biết thanh trượt nào đã được thay đổi cũng như giá trị hiện tại của thanh trượt.

Dưới đây là MIT App Inventor project và file APK:

Do đó, trong Arduino, sử dụng hàm startedWith (), chúng ta kiểm tra tiền tố của từng dữ liệu đến và vì vậy robot biết phải làm gì tiếp theo. Ví dụ: nếu tiền tố là “s1” thì chúng ta biết rằng chúng ta cần di chuyển servo số 1. Sử dụng hàm substring(), chúng ta sẽ có được đoạn string còn lại. Với giá trị vị trí đó, chúng ta chuyển đổi nó thành số int và sử dụng để di chuyển servo đến đúng vị trí.

// If "Waist" slider has changed value - Move Servo 1 to position
    if (dataIn.startsWith("s1")) {
// Extract only the number. E.g. from "s1120" to "120"
      String dataInS = dataIn.substring(2, dataIn.length()); 
      servo1Pos = dataInS.toInt();  // Convert the string into integer

Ở đây chúng ta chỉ cần gọi hàm write () và servo sẽ di chuyển đến vị trí đó, nhưng theo cách đó, servo sẽ chạy ở tốc độ tối đa của nó. Thay vào đó, chúng ta cần kiểm soát tốc độ của các servo, vì vậy chúng ta sử dụng một số vòng lặp FOR để chuyển dần servo từ vị trí trước đó sang vị trí hiện tại bằng cách cho delay giữa mỗi lần lặp. Thay đổi thời gian trễ, bạn có thể thay đổi tốc độ của servo.

// We use for loops so we can control the speed of the servo
      // If previous position is bigger then current position
      if (servo1PPos > servo1Pos) {
        for ( int j = servo1PPos; j >= servo1Pos; j--) {   // Run servo down
          servo01.write(j);
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      // If previous position is smaller then current position
      if (servo1PPos < servo1Pos) {
        for ( int j = servo1PPos; j <= servo1Pos; j++) {   // Run servo up
          servo01.write(j);
          delay(20);
        }
      }
      servo1PPos = servo1Pos;   // set current position as previous position
    }

Phương pháp tương tự được sử dụng để điều khiển từng trục của cánh tay robot.

Đồng thời, nếu chúng ta nhấn nút SAVE, vị trí của mỗi mô tơ servo sẽ được lưu trong một mảng. Với mỗi lần nhấn, biến “index” sẽ được tăng lên.

// If button "SAVE" is pressed
    if (dataIn.startsWith("SAVE")) {
      servo01SP[index] = servo1PPos;  // save position into the array
      servo02SP[index] = servo2PPos;
      servo03SP[index] = servo3PPos;
      servo04SP[index] = servo4PPos;
      servo05SP[index] = servo5PPos;
      servo06SP[index] = servo6PPos;
      index++;                        // Increase the array index
    }

Sau đó, nếu chúng ta nhấn nút RUN, chúng ta gọi hàm tùy chỉnh runservo () để chạy các bước được lưu trữ. Trong hàm này, chúng ta chạy các bước được lưu trữ nhiều lần cho đến khi nhấn nút RESET. Sử dụng vòng lặp FOR, chúng ta chạy qua tất cả các vị trí được lưu trữ trong các mảng và đồng thời chúng ta kiểm tra xem chúng ta có bất kỳ dữ liệu đến từ smart phone hay không. Dữ liệu này có thể là nút RUN / PAUSE, tạm dừng robot và nếu nhấp lại sẽ tiếp tục với chế tự động. Ngoài ra, nếu chúng ta thay đổi vị trí thanh trượt tốc độ, chúng ta sẽ sử dụng giá trị đó để thay đổi thời gian delay giữa mỗi lần lặp trong các vòng lặp FOR để điều khiển tốc độ của động cơ servo.

// Automatic mode custom function - run the saved steps
void runservo() {
  while (dataIn != "RESET") {   // Run the steps over and over again until "RESET" button is pressed
    for (int i = 0; i <= index - 2; i++) {  // Run through all steps(index)
      if (Bluetooth.available() > 0) {      // Check for incomding data
        dataIn = Bluetooth.readString();
        if ( dataIn == "PAUSE") {           // If button "PAUSE" is pressed
          while (dataIn != "RUN") {         // Wait until "RUN" is pressed again
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.readString();
              if ( dataIn == "RESET") {     
                break;
              }
            }
          }
        }
        // If SPEED slider is changed
        if (dataIn.startsWith("ss")) {
          String dataInS = dataIn.substring(2, dataIn.length());
          speedDelay = dataInS.toInt(); // Change servo speed (delay time)
        }
      }
      // Servo 1
      if (servo01SP[i] == servo01SP[i + 1]) {
      }
      if (servo01SP[i] > servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j >= servo01SP[i + 1]; j--) {
          servo01.write(j);
          delay(speedDelay);
        }
      }
      if (servo01SP[i] < servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j <= servo01SP[i + 1]; j++) {
          servo01.write(j);
          delay(speedDelay);
        }
      }

Cuối cùng, nếu chúng ta nhấn nút RESET, chúng ta sẽ xóa tất cả dữ liệu từ các mảng về 0 và cũng đặt lại các chỉ số về 0 để chúng ta có thể lập trình lại cánh tay robot bằng các chuyển động mới.

// If button "RESET" is pressed
    if ( dataIn == "RESET") {
      memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
      memset(servo02SP, 0, sizeof(servo02SP));
      memset(servo03SP, 0, sizeof(servo03SP));
      memset(servo04SP, 0, sizeof(servo04SP));
      memset(servo05SP, 0, sizeof(servo05SP));
      memset(servo06SP, 0, sizeof(servo06SP));
      index = 0;  // Index to 0
    }

Đoạn code hoàn chỉnh của cánh tay robot 5 bậc sử dụng Arduino

/*        
       DIY Arduino Robot Arm Smartphone Control  
        by Dejan, www.HowToMechatronics.com  
*/
#include <SoftwareSerial.h>
#include <Servo.h>
Servo servo01;
Servo servo02;
Servo servo03;
Servo servo04;
Servo servo05;
Servo servo06;
SoftwareSerial Bluetooth(3, 4); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)
int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // current position
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // for storing positions/steps
int speedDelay = 20;
int index = 0;
String dataIn = "";
void setup() {
  servo01.attach(5);
  servo02.attach(6);
  servo03.attach(7);
  servo04.attach(8);
  servo05.attach(9);
  servo06.attach(10);
  Bluetooth.begin(38400); // Default baud rate of the Bluetooth module
  Bluetooth.setTimeout(1);
  delay(20);
  // Robot arm initial position
  servo1PPos = 90;
  servo01.write(servo1PPos);
  servo2PPos = 150;
  servo02.write(servo2PPos);
  servo3PPos = 35;
  servo03.write(servo3PPos);
  servo4PPos = 140;
  servo04.write(servo4PPos);
  servo5PPos = 85;
  servo05.write(servo5PPos);
  servo6PPos = 80;
  servo06.write(servo6PPos);
}
void loop() {
  // Check for incoming data
  if (Bluetooth.available() > 0) {
    dataIn = Bluetooth.readString();  // Read the data as string
    
    // If "Waist" slider has changed value - Move Servo 1 to position
    if (dataIn.startsWith("s1")) {
      String dataInS = dataIn.substring(2, dataIn.length()); // Extract only the number. E.g. from "s1120" to "120"
      servo1Pos = dataInS.toInt();  // Convert the string into integer
      // We use for loops so we can control the speed of the servo
      // If previous position is bigger then current position
      if (servo1PPos > servo1Pos) {
        for ( int j = servo1PPos; j >= servo1Pos; j--) {   // Run servo down
          servo01.write(j);
          delay(20);    // defines the speed at which the servo rotates
        }
      }
      // If previous position is smaller then current position
      if (servo1PPos < servo1Pos) {
        for ( int j = servo1PPos; j <= servo1Pos; j++) {   // Run servo up
          servo01.write(j);
          delay(20);
        }
      }
      servo1PPos = servo1Pos;   // set current position as previous position
    }
    
    // Move Servo 2
    if (dataIn.startsWith("s2")) {
      String dataInS = dataIn.substring(2, dataIn.length());
      servo2Pos = dataInS.toInt();
      if (servo2PPos > servo2Pos) {
        for ( int j = servo2PPos; j >= servo2Pos; j--) {
          servo02.write(j);
          delay(50);
        }
      }
      if (servo2PPos < servo2Pos) {
        for ( int j = servo2PPos; j <= servo2Pos; j++) {
          servo02.write(j);
          delay(50);
        }
      }
      servo2PPos = servo2Pos;
    }
    // Move Servo 3
    if (dataIn.startsWith("s3")) {
      String dataInS = dataIn.substring(2, dataIn.length());
      servo3Pos = dataInS.toInt();
      if (servo3PPos > servo3Pos) {
        for ( int j = servo3PPos; j >= servo3Pos; j--) {
          servo03.write(j);
          delay(30);
        }
      }
      if (servo3PPos < servo3Pos) {
        for ( int j = servo3PPos; j <= servo3Pos; j++) {
          servo03.write(j);
          delay(30);
        }
      }
      servo3PPos = servo3Pos;
    }
    // Move Servo 4
    if (dataIn.startsWith("s4")) {
      String dataInS = dataIn.substring(2, dataIn.length());
      servo4Pos = dataInS.toInt();
      if (servo4PPos > servo4Pos) {
        for ( int j = servo4PPos; j >= servo4Pos; j--) {
          servo04.write(j);
          delay(30);
        }
      }
      if (servo4PPos < servo4Pos) {
        for ( int j = servo4PPos; j <= servo4Pos; j++) {
          servo04.write(j);
          delay(30);
        }
      }
      servo4PPos = servo4Pos;
    }
    // Move Servo 5
    if (dataIn.startsWith("s5")) {
      String dataInS = dataIn.substring(2, dataIn.length());
      servo5Pos = dataInS.toInt();
      if (servo5PPos > servo5Pos) {
        for ( int j = servo5PPos; j >= servo5Pos; j--) {
          servo05.write(j);
          delay(30);
        }
      }
      if (servo5PPos < servo5Pos) {
        for ( int j = servo5PPos; j <= servo5Pos; j++) {
          servo05.write(j);
          delay(30);
        }
      }
      servo5PPos = servo5Pos;
    }
    // Move Servo 6
    if (dataIn.startsWith("s6")) {
      String dataInS = dataIn.substring(2, dataIn.length());
      servo6Pos = dataInS.toInt();
      if (servo6PPos > servo6Pos) {
        for ( int j = servo6PPos; j >= servo6Pos; j--) {
          servo06.write(j);
          delay(30);
        }
      }
      if (servo6PPos < servo6Pos) {
        for ( int j = servo6PPos; j <= servo6Pos; j++) {
          servo06.write(j);
          delay(30);
        }
      }
      servo6PPos = servo6Pos; 
    }
    // If button "SAVE" is pressed
    if (dataIn.startsWith("SAVE")) {
      servo01SP[index] = servo1PPos;  // save position into the array
      servo02SP[index] = servo2PPos;
      servo03SP[index] = servo3PPos;
      servo04SP[index] = servo4PPos;
      servo05SP[index] = servo5PPos;
      servo06SP[index] = servo6PPos;
      index++;                        // Increase the array index
    }
    // If button "RUN" is pressed
    if (dataIn.startsWith("RUN")) {
      runservo();  // Automatic mode - run the saved steps 
    }
    // If button "RESET" is pressed
    if ( dataIn == "RESET") {
      memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
      memset(servo02SP, 0, sizeof(servo02SP));
      memset(servo03SP, 0, sizeof(servo03SP));
      memset(servo04SP, 0, sizeof(servo04SP));
      memset(servo05SP, 0, sizeof(servo05SP));
      memset(servo06SP, 0, sizeof(servo06SP));
      index = 0;  // Index to 0
    }
  }
}
// Automatic mode custom function - run the saved steps
void runservo() {
  while (dataIn != "RESET") {   // Run the steps over and over again until "RESET" button is pressed
    for (int i = 0; i <= index - 2; i++) {  // Run through all steps(index)
      if (Bluetooth.available() > 0) {      // Check for incomding data
        dataIn = Bluetooth.readString();
        if ( dataIn == "PAUSE") {           // If button "PAUSE" is pressed
          while (dataIn != "RUN") {         // Wait until "RUN" is pressed again
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.readString();
              if ( dataIn == "RESET") {     
                break;
              }
            }
          }
        }
        // If speed slider is changed
        if (dataIn.startsWith("ss")) {
          String dataInS = dataIn.substring(2, dataIn.length());
          speedDelay = dataInS.toInt(); // Change servo speed (delay time)
        }
      }
      // Servo 1
      if (servo01SP[i] == servo01SP[i + 1]) {
      }
      if (servo01SP[i] > servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j >= servo01SP[i + 1]; j--) {
          servo01.write(j);
          delay(speedDelay);
        }
      }
      if (servo01SP[i] < servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j <= servo01SP[i + 1]; j++) {
          servo01.write(j);
          delay(speedDelay);
        }
      }
      // Servo 2
      if (servo02SP[i] == servo02SP[i + 1]) {
      }
      if (servo02SP[i] > servo02SP[i + 1]) {
        for ( int j = servo02SP[i]; j >= servo02SP[i + 1]; j--) {
          servo02.write(j);
          delay(speedDelay);
        }
      }
      if (servo02SP[i] < servo02SP[i + 1]) {
        for ( int j = servo02SP[i]; j <= servo02SP[i + 1]; j++) {
          servo02.write(j);
          delay(speedDelay);
        }
      }
      // Servo 3
      if (servo03SP[i] == servo03SP[i + 1]) {
      }
      if (servo03SP[i] > servo03SP[i + 1]) {
        for ( int j = servo03SP[i]; j >= servo03SP[i + 1]; j--) {
          servo03.write(j);
          delay(speedDelay);
        }
      }
      if (servo03SP[i] < servo03SP[i + 1]) {
        for ( int j = servo03SP[i]; j <= servo03SP[i + 1]; j++) {
          servo03.write(j);
          delay(speedDelay);
        }
      }
      // Servo 4
      if (servo04SP[i] == servo04SP[i + 1]) {
      }
      if (servo04SP[i] > servo04SP[i + 1]) {
        for ( int j = servo04SP[i]; j >= servo04SP[i + 1]; j--) {
          servo04.write(j);
          delay(speedDelay);
        }
      }
      if (servo04SP[i] < servo04SP[i + 1]) {
        for ( int j = servo04SP[i]; j <= servo04SP[i + 1]; j++) {
          servo04.write(j);
          delay(speedDelay);
        }
      }
      // Servo 5
      if (servo05SP[i] == servo05SP[i + 1]) {
      }
      if (servo05SP[i] > servo05SP[i + 1]) {
        for ( int j = servo05SP[i]; j >= servo05SP[i + 1]; j--) {
          servo05.write(j);
          delay(speedDelay);
        }
      }
      if (servo05SP[i] < servo05SP[i + 1]) {
        for ( int j = servo05SP[i]; j <= servo05SP[i + 1]; j++) {
          servo05.write(j);
          delay(speedDelay);
        }
      }
      // Servo 6
      if (servo06SP[i] == servo06SP[i + 1]) {
      }
      if (servo06SP[i] > servo06SP[i + 1]) {
        for ( int j = servo06SP[i]; j >= servo06SP[i + 1]; j--) {
          servo06.write(j);
          delay(speedDelay);
        }
      }
      if (servo06SP[i] < servo06SP[i + 1]) {
        for ( int j = servo06SP[i]; j <= servo06SP[i + 1]; j++) {
          servo06.write(j);
          delay(speedDelay);
        }
      }
    }
  }
}

Các bạn có thể tham khảo các dự án khác tại: https://www.abcarduino.com/category/arduino-code/

Nhớ like và share bài viết trên fanpage nhé: https://www.facebook.com/abcArduino