ADUDU

ROBOTICS

Home

DECODE EP

About aDudu

Open Resources

Odometry Sim

ADUDU

ROBOTICS

this articles is a part of
The FACC'25 event

FACC'25

Programming

Linh kiện điện tử trong Odometry

Linh kiện điện tử trong Odometry

Các dữ liệu cần có trong Odometry và các linh kiện điện tử cung cấp các dữ liệu này.

Các dữ liệu cần có trong Odometry và các linh kiện điện tử cung cấp các dữ liệu này.

Level

Intermediate

Source

Source

Aggregate

Aggregate

Author

Author

Aggregate

Aggregate

Translator

Translator

FTC26749 aDudu

FTC26749 aDudu

Date Published

Date Published

Sep 23, 2025

Sep 23, 2025

TỔNG QUAN VỀ ODOMETRY TRONG FTC

1. Các đại lượng và linh kiện cơ bản

Odometry giúp robot tính toán vị trí (x, y) và góc quay (θ) của nó trên sân thi đấu, nhờ vào:

  • IMU (Inertial Measurement Unit): đo góc quay (heading, orientation)

  • Encoder: cảm biến đo số vòng quay bánh xe (để tính quãng đường di chuyển).

  • Odometry Pods: các bánh xe nhỏ gắn encoder, chuyên dùng để theo dõi vị trí (chính xác hơn so với encoder trong motor drive).

2. Linh kiện chính

A. IMU

  • Được tích hợp trong REV Control Hub / Expansion Hub.

  • Dùng để đo góc quay (yaw, pitch, roll), trong odometry ta thường chỉ dùng yaw (heading).

Code mẫu IMU cơ bản (Java, FTC SDK):

import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.hardware.HardwareMap;

public class IMUExample {
    BNO055IMU imu;
    double heading;

    public void init(HardwareMap hardwareMap) {
        imu = hardwareMap.get(BNO055IMU.class, "imu"); // khai báo thiết bị imu
        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); //khởi tạo imu
        parameters.mode = BNO055IMU.SensorMode.IMU;
        parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; // dùng radian cho toán học
        imu.initialize(parameters);
    }

    public double getHeading() {
        heading = imu.getAngularOrientation().firstAngle; //đo góc cho trục đầu tiên(có thể tùy chỉnh)
        return heading;
    }
}
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.hardware.HardwareMap;

public class IMUExample {
    BNO055IMU imu;
    double heading;

    public void init(HardwareMap hardwareMap) {
        imu = hardwareMap.get(BNO055IMU.class, "imu"); // khai báo thiết bị imu
        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); //khởi tạo imu
        parameters.mode = BNO055IMU.SensorMode.IMU;
        parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; // dùng radian cho toán học
        imu.initialize(parameters);
    }

    public double getHeading() {
        heading = imu.getAngularOrientation().firstAngle; //đo góc cho trục đầu tiên(có thể tùy chỉnh)
        return heading;
    }
}

B. Encoder

  • Encoder thường có sẵn trong motor GoBilda, NeveRest, REV HD Hex.

  • Trả về ticks (đếm số xung) → từ đó tính ra quãng đường:

Code mẫu đọc encoder từ motor:

import com.qualcomm.robotcore.hardware.DcMotor;

public class EncoderExample {
    DcMotor leftMotor;

    public void init(HardwareMap hardwareMap) {
        leftMotor = hardwareMap.get(DcMotor.class, "leftMotor"); //khai báo động cơ
        leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); //set lại giá trị encoder
        leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); //chọn chế độ sử dụng encoder
    }

    public int getPosition() {
        return leftMotor.getCurrentPosition(); //lấy giá trị encoder
    }
}
import com.qualcomm.robotcore.hardware.DcMotor;

public class EncoderExample {
    DcMotor leftMotor;

    public void init(HardwareMap hardwareMap) {
        leftMotor = hardwareMap.get(DcMotor.class, "leftMotor"); //khai báo động cơ
        leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); //set lại giá trị encoder
        leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); //chọn chế độ sử dụng encoder
    }

    public int getPosition() {
        return leftMotor.getCurrentPosition(); //lấy giá trị encoder
    }
}

c. Odometry Pods / Encoder

  • Là bánh xe encoder riêng, không dùng để di chuyển mà chỉ để đo vị trí.

  • Thường có 3 pod

    • X (trái, phải) → đo tiến/lùi.

    • Y (ngang) → đo dịch ngang.

  • Kết hợp cùng IMU để xác định chính xác (x, y, heading).

Code mẫu đọc odometry pods: (sử dụng encoder của động cơ [built-in encoder])

import com.qualcomm.robotcore.hardware.DcMotor;

public class OdometryExample {
    DcMotor leftPod, rightPod, strafePod;

    public void init(HardwareMap hardwareMap) {
        leftPod = hardwareMap.get(DcMotor.class, "leftPod");
        rightPod = hardwareMap.get(DcMotor.class, "rightPod");
        strafePod = hardwareMap.get(DcMotor.class, "strafePod");

        leftPod.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        rightPod.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        strafePod.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        leftPod.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        rightPod.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        strafePod.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    }

    public int getLeft() { return leftPod.getCurrentPosition(); }
    public int getRight() { return rightPod.getCurrentPosition(); }
    public int getStrafe() { return strafePod.getCurrentPosition(); }
}
import com.qualcomm.robotcore.hardware.DcMotor;

public class OdometryExample {
    DcMotor leftPod, rightPod, strafePod;

    public void init(HardwareMap hardwareMap) {
        leftPod = hardwareMap.get(DcMotor.class, "leftPod");
        rightPod = hardwareMap.get(DcMotor.class, "rightPod");
        strafePod = hardwareMap.get(DcMotor.class, "strafePod");

        leftPod.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        rightPod.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        strafePod.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        leftPod.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        rightPod.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        strafePod.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    }

    public int getLeft() { return leftPod.getCurrentPosition(); }
    public int getRight() { return rightPod.getCurrentPosition(); }
    public int getStrafe() { return strafePod.getCurrentPosition(); }
}

3. Tích hợp Odometry + IMU → Tính toán vị trí (x, y, θ)

Lưu ý: Phần giải thích thuật toán và hướng dẫn lập trình chi tiết sẽ được đặt ở phía dưới (phần V của tài liệu), đây chỉ là phần code tham khảo cho odometry

Pseudo-code (ví dụ):

// Giá trị tick hiện tại
int leftTicks = leftPod.getCurrentPosition();
int rightTicks = rightPod.getCurrentPosition();
int strafeTicks = strafePod.getCurrentPosition();

// Chuyển ticks → khoảng cách (inch hoặc cm)
double leftDist = leftTicks * TICKS_TO_CM;
double rightDist = rightTicks * TICKS_TO_CM;
double strafeDist = strafeTicks * TICKS_TO_CM;

// Tính delta di chuyển
double deltaForward = (leftDist + rightDist) / 2.0;
double deltaStrafe = strafeDist;

// Lấy heading từ IMU
double heading = imu.getHeading();

// Cập nhật vị trí
x += deltaForward * Math.cos(heading) - deltaStrafe * Math.sin(heading);
y += deltaForward * Math.sin(heading) + deltaStrafe * Math.cos(heading);
theta = heading;
// Giá trị tick hiện tại
int leftTicks = leftPod.getCurrentPosition();
int rightTicks = rightPod.getCurrentPosition();
int strafeTicks = strafePod.getCurrentPosition();

// Chuyển ticks → khoảng cách (inch hoặc cm)
double leftDist = leftTicks * TICKS_TO_CM;
double rightDist = rightTicks * TICKS_TO_CM;
double strafeDist = strafeTicks * TICKS_TO_CM;

// Tính delta di chuyển
double deltaForward = (leftDist + rightDist) / 2.0;
double deltaStrafe = strafeDist;

// Lấy heading từ IMU
double heading = imu.getHeading();

// Cập nhật vị trí
x += deltaForward * Math.cos(heading) - deltaStrafe * Math.sin(heading);
y += deltaForward * Math.sin(heading) + deltaStrafe * Math.cos(heading);
theta = heading;

4.  Code mẫu ví dụ: Robot tự in ra vị trí

@TeleOp
public class OdometryTest extends LinearOpMode {
    OdometryExample odo = new OdometryExample();
    IMUExample imuEx = new IMUExample();

    double x = 0, y = 0;

    public void runOpMode() throws InterruptedException {
        odo.init(hardwareMap);
        imuEx.init(hardwareMap);

        waitForStart();

        while (opModeIsActive()) {
            // Đọc encoder
            double leftDist = odo.getLeft() * TICKS_TO_CM;
            double rightDist = odo.getRight() * TICKS_TO_CM;
            double strafeDist = odo.getStrafe() * TICKS_TO_CM;

            // Lấy heading từ IMU
            double heading = imuEx.getHeading();

            // Cập nhật vị trí (ví dụ cơ bản)
            double deltaForward = (leftDist + rightDist) / 2.0;
            double deltaStrafe = strafeDist;

            x += deltaForward * Math.cos(heading) - deltaStrafe * Math.sin(heading);
            y += deltaForward * Math.sin(heading) + deltaStrafe * Math.cos(heading);

            telemetry.addData("X", x);
            telemetry.addData("Y", y);
            telemetry.addData("Heading", heading);
            telemetry.update();
        }
    }
}
@TeleOp
public class OdometryTest extends LinearOpMode {
    OdometryExample odo = new OdometryExample();
    IMUExample imuEx = new IMUExample();

    double x = 0, y = 0;

    public void runOpMode() throws InterruptedException {
        odo.init(hardwareMap);
        imuEx.init(hardwareMap);

        waitForStart();

        while (opModeIsActive()) {
            // Đọc encoder
            double leftDist = odo.getLeft() * TICKS_TO_CM;
            double rightDist = odo.getRight() * TICKS_TO_CM;
            double strafeDist = odo.getStrafe() * TICKS_TO_CM;

            // Lấy heading từ IMU
            double heading = imuEx.getHeading();

            // Cập nhật vị trí (ví dụ cơ bản)
            double deltaForward = (leftDist + rightDist) / 2.0;
            double deltaStrafe = strafeDist;

            x += deltaForward * Math.cos(heading) - deltaStrafe * Math.sin(heading);
            y += deltaForward * Math.sin(heading) + deltaStrafe * Math.cos(heading);

            telemetry.addData("X", x);
            telemetry.addData("Y", y);
            telemetry.addData("Heading", heading);
            telemetry.update();
        }
    }
}

Lưu ý:

  • Các bạn nên bắt đầu với IMU vì nó dễ dùng nhất để kiểm tra góc quay.

  • Sau đó học cách đọc encoder trong motor.

  • Cuối cùng mới lắp odometry pods để đạt độ chính xác cao.

  • Nên vẽ sơ đồ sân và log vị trí robot để dễ debug.

ADUDU

ADUDU
ROBOTICS

A proud team of passionate Robotics Enthusiasts competing in nation-wide Technology competitions in Vietnam, the FIRST Tech Challenge and the FIRST Robotics Competition.

navigation

Home

About aDudu

Engineering Portfolios

Odometry Simulation

social
media

Facebook

Instagram

Youtube

open

resources

Programming

Electronics

Mechanical Vex IQ

FTC Lib

Management

CAD

Mechanical

contacts

robotics-team@ftcadudu.com

Tech Specific

tech@ftcadudu.com

For Collaborations Enquiry

outreach@ftcadudu.com

aDudu's Flagship Programs

youth-program@ftcadudu.com

career-development@ftcadudu.com

Head Coach

linhnk@ftcadudu.com

khoinm@ftcadudu.com

Copyright ©

aDudu Robotics

, all rights reserved

Made by aDudu's Programming Department

made by aDudu

made by aDudu

Linh kiện điện tử trong Odometry