V5 Camp Config File

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

// ── Starting Position ─────────────────────────────────────────────────
// Must match where the robot is placed on the field at the start
// x, y in inches from field center. Heading: 0=right, 90=up
const double START_X       = 0.0;   // TODO
const double START_Y       = 0.0;   // TODO
const double START_HEADING = 90.0;  // TODO

// ── Path Following ────────────────────────────────────────────────────
const double PATH_LOOKAHEAD = 15.0;   // inches — how far ahead robot looks on path
const int    PATH_TIMEOUT   = 10000;  // ms — max time allowed per path segment

// ── Motor Ports ───────────────────────────────────────────────────────
// Negative port number = that motor is reversed
const int LEFT_MOTOR_PORT_1  = -1;  // TODO
const int LEFT_MOTOR_PORT_2  = -2;  // TODO
const int RIGHT_MOTOR_PORT_1 =  3;  // TODO
const int RIGHT_MOTOR_PORT_2 =  4;  // TODO

// ── Motor Cartridge ───────────────────────────────────────────────────
// red = 100rpm | green = 200rpm | blue = 600rpm
const pros::MotorGearset DRIVE_CARTRIDGE = pros::MotorGearset::green; // TODO

// ── Sensor Ports ──────────────────────────────────────────────────────
const int IMU_PORT                = 10; // TODO
const int VERTICAL_ENCODER_PORT   = 11; // TODO
const int HORIZONTAL_ENCODER_PORT = 12; // TODO

// ── Drivetrain Dimensions ─────────────────────────────────────────────
const double TRACK_WIDTH      = 10.0;  // TODO — inches, left to right wheel center
const double DRIVETRAIN_RPM   = 360.0; // TODO — motor RPM x gear ratio
const double HORIZONTAL_DRIFT = 2.0;   // leave as-is for now

// ── Wheel Sizes ───────────────────────────────────────────────────────
// Options: NEW_2 | NEW_275 | NEW_325 | NEW_4
#define DRIVE_WHEEL_SIZE      lemlib::Omniwheel::NEW_4    // TODO
#define VERTICAL_WHEEL_SIZE   lemlib::Omniwheel::NEW_275  // TODO
#define HORIZONTAL_WHEEL_SIZE lemlib::Omniwheel::NEW_275  // TODO

// ── Tracking Wheel Offsets ────────────────────────────────────────────
// Inches from the robot center to the tracking wheel
// Vertical:   negative = left of center
// Horizontal: negative = behind center
const double VERTICAL_WHEEL_OFFSET   = -2.5;  // TODO
const double HORIZONTAL_WHEEL_OFFSET = -5.75; // TODO

// ── Lateral PID — controls forward/backward movement ──────────────────
// Tune after the robot is moving. See Section 8.
const double LAT_KP = 10; const double LAT_KI = 0; const double LAT_KD = 3;
const double LAT_ANTI_WINDUP = 3;
const double LAT_SMALL_ERROR = 1.0; const int LAT_SMALL_ERROR_TIMEOUT = 100;
const double LAT_LARGE_ERROR = 3.0; const int LAT_LARGE_ERROR_TIMEOUT = 500;
const double LAT_SLEW = 20;

// ── Angular PID — controls turning ───────────────────────────────────
// Tune after the robot is moving. See Section 8.
const double ANG_KP = 2;  const double ANG_KI = 0; const double ANG_KD = 10;
const double ANG_ANTI_WINDUP = 3;
const double ANG_SMALL_ERROR = 1.0; const int ANG_SMALL_ERROR_TIMEOUT = 100;
const double ANG_LARGE_ERROR = 3.0; const int ANG_LARGE_ERROR_TIMEOUT = 500;
const double ANG_SLEW = 0;