Thiết kế Robot tìm đường trong mê cung V0.1

Robot tìm đường trong mê cung V0.1

Robot tìm đường trong mê cung V0.1

Đây là một bài toán điển hình, nhiệm vụ là một chú Robot tìm đường đi đến đích trong một mê cung cho trước, mức độ phức tạp tăng lên khi kích thước của mê cung tăng lên, dưới đây là một ví dụ giúp bạn giải quyết tốt bài toán với một mê cung kích thước vừa phải.

Bài viết sơ lược các bước kèm cả mã nguồn tham khảo, để các bạn có thể nhanh chóng tiếp cận cách giải quyết vấn đề với một kiến trúc Robot không quá phức tạp.

Nguyên lý cơ bản:

Video: https://youtu.be/JFrexM1UKO4

Sign: (L)eft, (R)ight, or (S)traight are on either end of a (B)ack as follows:

    • LBR = B
    • LBS = R
    • LBL = S
    • SBL = R
    • SBS = B
    • RBL = B

    For example, from the starting point:

    • Start moving
    • (L) eft turn is made at found intersection
    • Add "L" to list
    • Check if last three on list can be shortened: NO, only one turn recorded.
    • (B) ack at a found dead end
    • Add "B" to list
    • Check if last three on list can be shortened: NO, only two turns recorded.
    • (L) eft turn again when it finds the initial intersection at which it previously turned left
    • Add "L" to list
    • Check if last three on list can be shortened: YES.
    • Replace "LBL" with "S" traight
    • Continue until end point is found.

    (Lý thuyết: https://en.wikipedia.org/wiki/Maze_solving_algorithm)

    Đầu tiên chúng ta cần làm một Robot dò đường, phần này đã có bài viết nên tôi chỉ sơ lược lại để các bạn liên kết với bài toán:

    Line Following Robot with Basic PD (Proportional-Derivative) control

    Tune the loop

    http://en.wikipedia.org/wiki/PID_controller

    Kết nối & ký hiệu:

    // chân bộ cảm biến vạch đen
    Pololu QTR-8RC RC Reflectance Sensor Array
    QTR8RC Schematic
    Digital pin 2 - QTR Sensor pin1
    Digital pin 4 - QTR Sensor pin2
    Digital pin 5 - QTR Sensor pin3
    Digital pin 6 - QTR Sensor pin4
    Digital pin 7 - QTR Sensor pin5
    Digital pin 8 - QTR Sensor pin6
    Digital pin 9 - QTR Sensor pin7
    Digital pin 10 - QTR Sensor pin8
    Analog pin 0 - QTR Sensor pin LEDON
    A 1 - Chiều tiến (Negative wire for Right motor)
    A 2 - Lùi (Negative wire for Right motor)
    B 3 - Tiến (Positive wire for Left motor)
    B 4 - Lùi (Negative wire for Left motor)
    GND - QTR Sensor pin GND
    5V - QTR Sensor pin VCC
     

    (Note: Digital Pin 3 dùng cho mạch điều khiển động cơ)​

    'f' - move forward
    'b' - move backward
    'l' - rotate left
    'r' - rotate right
    's' - stop

    'v' - version 

    'h' - help

     

    Phần mềm kết nối serial bất kỳ: 

    HyperTerminal - http://www.hilgraeve.com/hyperterminal/


    Realterm - http://realterm.sourceforge.net/

    Setup a the Terminal Program

    Review the Example Program

    Code:

    void follow_line(int line_position) //follow the line 

    {


    switch(line_position)
    {

    These to cases handle the situation where the line is lost and attempt to rotate back to the line


    // line has moved off the left edge of sensor
    case 0: 


    digitalWrite(dir_a, LOW);
    analogWrite(pwm_a, 200);
    digitalWrite(dir_b, HIGH); 
    analogWrite(pwm_b, 200);
    Serial.println("Rotate Left\n");
    break;

    // line has moved off the right edit of the sensor
    case 7000:
    digitalWrite(dir_a, HIGH);
    analogWrite(pwm_a, 200);
    digitalWrite(dir_b, LOW); 
    analogWrite(pwm_b, 200);
    Serial.println("Rotate Right\n");
    break;


    This line calculate the error for the control loop. 3500 is the "setpoint" and can be adjusted to move the line position the robot is shooting for.

    default:
    error = (float)line_position - 3500;

    // set the motor speed based on proportional and derivative PID terms
    // kp is the a floating-point proportional constant (maybe start with a value around 0.5)
    // kd is the floating-point derivative constant (maybe start with a value around 1)
    // note that when doing PID, it's very important you get your signs right, or else the
    // control loop will be unstable

    These are the variables that should be adjusted to fine tune the control loop


    kp=.5; This is the proporational value
    kd=1; This is the derivative value

    PV = kp * error + kd * (error - lastError);
    lastError = error;


    PWM values (motor speed) must be between 0 and 255. This code limits the values. It also limits the low end of the PWM value. Depending on the batteries, motors and load, the motors will usually stall at a value much greater the 0. In this example, the low end is 170.

    //this codes limits the PV (motor speed pwm value) 
    // limit PV to 55
    if (PV > 55)
    {
    PV = 55;
    }

    if (PV < -55)
    {
    PV = -55;
    }


    This calculated the individual PWM value for each motor, note the sign difference.

    m1Speed = 170 + PV;
    m2Speed = 170 - PV;

    //set motor speeds

    This sets the updated motor speed and direction

    digitalWrite(dir_a, LOW); 
    analogWrite(pwm_a, m2Speed);
    digitalWrite(dir_b, LOW); 
    analogWrite(pwm_b, m1Speed);
    break;
    }


    } // end follow line

     

    Mã nguồn của robot dò đường: Tại đây

    Làm mê cung mẫu:

    Make your maze

    Kiểm tra lại kết nối theo nhiệm vụ dò đường mê cung:
    Digital pin 2 - QTR Sensor pin1
    Digital pin 4 - QTR Sensor pin2
    Digital pin 5 - QTR Sensor pin3
    Digital pin 6 - QTR Sensor pin4
    Digital pin 7 - QTR Sensor pin5
    Digital pin 8 - QTR Sensor pin6
    Digital pin 9 - QTR Sensor pin7
    Digital pin 10 - QTR Sensor pin8
    GND - QTR Sensor pin GND
    5V - QTR Sensor pin VCC
    Terminal A 1 - Black wire of Left motor
    Terminal A 2 - Red wire of Left motor
    Terminal B 3 - Black wire of Right motor
    Terminal B 4 - Red wire of Right motor
    Terminal VIN (+) - Positive/Red wire from battery holder
    Terminal VIN (-) - Negative/Black wire from battery holder

     

    (Note: Digital Pin 3 dùng cho motor driver. Arduno dung các chân pins 11, 12, and 13 nhiệm vụ khác)
     

    Mã nguồn tham khảo:  RabitMazeSolver

    Nạp mã và kiểm tra lại:

    Một số lưu ý điều chỉnh:
    Nếu Robot hoạt động khó khăn hoặc không thể nhận đúng đường đi thì bạn điều chỉnh "calSpeed".
    Nếu robot của bạn có gặp khó khăn khi di chuyển hoặc chạy ra ngoài đường thì điều chỉnh biến tốc độ.

    Khi qua giao lộ cần điều chỉnh hợp lý để bánh bám đường, điều chỉnh "drivePastDelay" nếu các bánh xe không qua giao lộ khi nó rẽ.​

    Hình ảnh có liên quan

     

    Chúc các bạn có bài thực hành vui vẻ!