PROS C++ Drivetrain Class

VEX V5 General Discussion Technical Discussion
, ,
1

I created a class for controlling a drivetrain in autonomous in Purdue PROS C++.
Requires at least two motors on each side and an IMU (inertial sensor).

These are the following commands included:
drive.drive(inches)- Drive forward or backward the specified number of inches
drive.turn(degrees, speed)- Turn by the specified number of degrees at the specified speed
drive.stop()- Stop all drivetrain motors
drive.tare()- Tare sensor positions

Replace * with the corresponding port numbers

You MUST run comprehensive testing in order to find the correct TICKS_PER_INCH. This variable stores how many “ticks” it takes to move an inch. If you find it moving farther than specified, decrease the variable, and vice versa.

class Drivetrain {
    pros::MotorGroup left{*, *, *}; // Left drivetrain track ports
    pros::MotorGroup right{-*, -*, -*}; // Right drivetrain track ports
    pros::Imu imu{*};
    static constexpr double TICKS_PER_INCH = 40.1;  // CALIBRATE!!!

public:
    // ── Drive a distance in inches (negative = backward) ──
    void drive(double inches, int speed = 60) {
        int ticks = -inches * TICKS_PER_INCH;
        left.tare_position();
        right.tare_position();
        left.move_relative(ticks, speed);
        right.move_relative(ticks, speed);
        // Wait until both sides are near target
        int timeout = 0;
        while ((std::abs(left.get_position() - ticks) > 10 ||
                std::abs(right.get_position() - ticks) > 10) && timeout < 1500) { // 15 seconds max
            pros::delay(10);
            timeout++;
        }
        stop();
    }

    // ── Turn by degrees (positive = right, negative = left) ──
    void turn(double degrees, int speed = 150) {
        double start = imu.get_rotation();  // unbounded, doesn't wrap
        double target = start + degrees;

        left.move_velocity(-speed);
        right.move_velocity(speed);

        int timeout = 0;
        while (std::abs(imu.get_rotation() - target) > 1.0 && timeout < 1000) {
            pros::delay(10);
            timeout++;
        }

        stop();
    }

    // ── Stop ──
    void stop() {
        left.move_velocity(0);
        right.move_velocity(0);
    }

    // ── Reset sensors at start of auton ──
    void tare() {
        left.tare_position();
        right.tare_position();
        imu.tare_rotation();
    }
};

Drivetrain drive;

Example:

void driveExample() {
	drive.drive(24, 60);      // Drive forward 24 inches at speed 60
	drive.turn(90, 150);      // Turn right 90 degrees at speed 150
	drive.drive(-12, 80);     // Drive backward 12 inches at speed 80
	drive.turn(-45, 150);     // Turn left 45 degrees at speed 150
	drive.drive(36, 100);     // Drive forward 36 inches at speed 100
	drive.stop();
}

I hope this helps someone out! If you make any improvements, please reply, so I can update my code.

1 Like