VEX V5 Main File

#include "main.h"

#include "lemlib/api.hpp"  // IWYU pragma: keep

#include "config.h"

// ── Path Files ────────────────────────────────────────────────────────

// Add one ASSET line per .txt file in static/

ASSET(path_1_txt);   // static/path_1.txt

// ASSET(path_2_txt);

// ── Motors ────────────────────────────────────────────────────────────

pros::MotorGroup left_motors({LEFT_MOTOR_PORT_1, LEFT_MOTOR_PORT_2}, DRIVE_CARTRIDGE);

pros::MotorGroup right_motors({RIGHT_MOTOR_PORT_1, RIGHT_MOTOR_PORT_2}, DRIVE_CARTRIDGE);

// ── Sensors ───────────────────────────────────────────────────────────

pros::Imu      imu(IMU_PORT);

pros::Rotation vertical_encoder(VERTICAL_ENCODER_PORT);

pros::Rotation horizontal_encoder(HORIZONTAL_ENCODER_PORT);

// ── Tracking Wheels ───────────────────────────────────────────────────

lemlib::TrackingWheel vertical_wheel(

    &vertical_encoder, VERTICAL_WHEEL_SIZE, VERTICAL_WHEEL_OFFSET);

lemlib::TrackingWheel horizontal_wheel(

    &horizontal_encoder, HORIZONTAL_WHEEL_SIZE, HORIZONTAL_WHEEL_OFFSET);

// ── Drivetrain ────────────────────────────────────────────────────────

lemlib::Drivetrain drivetrain(

    &left_motors, &right_motors,

    TRACK_WIDTH, DRIVE_WHEEL_SIZE, DRIVETRAIN_RPM, HORIZONTAL_DRIFT);

// ── Odometry ──────────────────────────────────────────────────────────

lemlib::OdomSensors sensors(

    &vertical_wheel, nullptr, &horizontal_wheel, nullptr, &imu);

// ── PID ───────────────────────────────────────────────────────────────

lemlib::ControllerSettings lateral_controller(

    LAT_KP, LAT_KI, LAT_KD, LAT_ANTI_WINDUP,

    LAT_SMALL_ERROR, LAT_SMALL_ERROR_TIMEOUT,

    LAT_LARGE_ERROR, LAT_LARGE_ERROR_TIMEOUT, LAT_SLEW);

lemlib::ControllerSettings angular_controller(

    ANG_KP, ANG_KI, ANG_KD, ANG_ANTI_WINDUP,

    ANG_SMALL_ERROR, ANG_SMALL_ERROR_TIMEOUT,

    ANG_LARGE_ERROR, ANG_LARGE_ERROR_TIMEOUT, ANG_SLEW);

// ── Chassis ───────────────────────────────────────────────────────────

lemlib::Chassis chassis(

    drivetrain, lateral_controller, angular_controller, sensors);

void initialize() {

    chassis.calibrate(); // keep robot completely still during this

    chassis.setPose(START_X, START_Y, START_HEADING);

    // autonomous(); // uncomment this line to test auto on startup

}

void disabled() {}

void competition_initialize() {}

void autonomous() {

    chassis.setPose(START_X, START_Y, START_HEADING);

    chassis.follow(path_1_txt, PATH_LOOKAHEAD, PATH_TIMEOUT);

    // chassis.follow(path_2_txt, PATH_LOOKAHEAD, PATH_TIMEOUT);

    // chassis.follow(path_3_txt, PATH_LOOKAHEAD, PATH_TIMEOUT, false); // false = reverse

}

void opcontrol() {

    pros::Controller master(pros::E_CONTROLLER_MASTER);

    while (true) {

        pros::screen::print(pros::E_TEXT_MEDIUM, 1, "X:   %.2f", chassis.getPose().x);

        pros::screen::print(pros::E_TEXT_MEDIUM, 2, "Y:   %.2f", chassis.getPose().y);

        pros::screen::print(pros::E_TEXT_MEDIUM, 3, "Hdg: %.2f", chassis.getPose().theta);

        int dir  = master.get_analog(ANALOG_LEFT_Y);

        int turn = master.get_analog(ANALOG_RIGHT_X);

        left_motors.move(dir - turn);

        right_motors.move(dir + turn);

        pros::delay(20);

    }

}