constants.ini

# generally will want DEBUG constant set to 2 or 3
# LOG_OFF  = 0
# LOG_ERR  = 1
# LOG_WARN = 2
# LOG_INFO = 3
# LOG_DBG  = 4
DEBUG = 0;

# motor debugging constants, DEBUG_MOTOR_PID is the motor ID to PID log
# for the logging tool graph
DEBUG_MOTOR_PID = -1;

# log server last octet
LOG_SERVER_OCTET = 6;
LOG_SERVER_PORT = 5810;

# ~~~~ BEGIN ARM Subsystem ~~~~
# WRIST 
WRIST_P = 0.8;
WRIST_I = 0;
WRIST_D = 0;
WRIST_F = 0;
WRIST_V = 500;
WRIST_A = 200;
WRIST_GEAR_RATIO = 33;
# WRIST - other
WRIST_MAX_ANGLE = 180;

# CLAW
CLAW_INTAKE_CUBE = 0.70;
#CLAW_INTAKE_CUBE = 0.35;
CLAW_INTAKE_CONE = 1;
CLAW_OFF_CUBE = 0.13;
CLAW_OFF_CONE = 0.22;
CLAW_SCORE_CUBE = -0.18;
CLAW_SCORE_CUBE_GND = -0.2;
CLAW_CUBE = -0.6;
CLAW_L3_SCORE_CONE = -0.06;
CLAW_STOW_CONE = -0.15;

# originally 2
INTAKE_CURRENT_LPF = 0.25;
INTAKE_CURRENT_TRIGGER = 75;

# PIVOT
PIVOT_GEAR_RATIO = 85.00;
# P was 0.04
PIVOT_P = 0.08;
PIVOT_I = 0;
PIVOT_D = 0;
PIVOT_F = 0;
# PIVOT - continuous PID vars
PIVOT_P_BASE = 0;
PIVOT_P_EXT = 0;
PIVOT_P_ANGLE = 0;
# PIVOT - other
PIVOT_MAX_ANGLE = 125;
PIVOT_V = 100000;
PIVOT_A = 70000;

# TELESCOPE
TELESCOPE_GEAR_RATIO = 1.945;
TELESCOPE_UP_P = 0.5;
TELESCOPE_UP_I = 0;
TELESCOPE_UP_D = 0;
TELESCOPE_UP_F = 0;
TELESCOPE_UP_V = 100000;
TELESCOPE_UP_A = 600;
TELESCOPE_DOWN_P = 0.5;
TELESCOPE_DOWN_I = 0;
TELESCOPE_DOWN_D = 0;
TELESCOPE_DOWN_F = 0;
TELESCOPE_DOWN_V = 500;
TELESCOPE_DOWN_A = 300;
TELESCOPE_PID_SWAP_THRESHOLD = 10000000;

TELESCOPE_FF = 0;

# TELESCOPE - other
ARM_MIN_EXT = -1;
ARM_MAX_EXT = 22.5;
ARM_MANUAL_SPEED = 0;

# ARM - other
AFRAME_HEIGHT = 18;
CLAW_LEN = 8;
PIVOT_WITHIN_BOT = 121;
EXT_MANUAL_CTRL = 1;
PIVOT_MANUAL_CTRL = 15;
WRIST_STOW_POS = 10;
WRIST_IN_POS = 60;
PIVOT_WHEN_VERT = 20; #WRIST BUTTON, ARM ANGLE OFFSET
# ~~~~ END ARM Subsystem ~~~~

WRIST_FLIP_OFFSET = 60;
WRIST_OFFSET_CUBE_IN = 0;
WRIST_OFFSET_IN_CUBE = 18;
WRIST_OFFSET_IN_CONE = 29;
WRIST_OFFSET_L2_CUBE = 10;
WRIST_OFFSET_L3_CUBE = 0;
WRIST_OFFSET_SCORE_CONE = -35;
WRIST_OFFSET_HUM_CUBE = -5;
WRIST_OFFSET_HUM_CONE = 0;

CUBE_PIVOT_OFFSET = 5;


# ~~~~ BEGIN ARM STATES ~~~~
# since this is non-pro, this is the (offset back to vert when on hardstop) / 2048
# position: 56478
ARM_ZERO_OFFSET = 27.577;

ARM_IN_ANGLE = 0;
ARM_IN_EXT = 0;
ARM_STOW_ANGLE = 122;
ARM_STOW_EXT = 0;
#ARM_L3_ANGLE = 0;
ARM_L3_CUBE_ANGLE = 40;
ARM_L3_CUBE_EXT = 15;
ARM_L2_CUBE_ANGLE = 40;
ARM_L2_CUBE_EXT = 7;

ARM_L3_CONE_ANGLE = 44;
ARM_L3_CONE_EXT = 21.5;
ARM_L2_CONE_ANGLE = 52;
ARM_L2_CONE_EXT = 12;

ARM_GND_CONE_ANGLE = 120;
ARM_GND_CONE_EXT = -1;
ARM_GND_CUBE_ANGLE = 120;
ARM_GND_CUBE_EXT = -0.5;

ARM_HUM_CUBE_ANGLE = 20;
ARM_HUM_CUBE_EXT = 7.8;
ARM_HUM_CONE_ANGLE = 15;
ARM_HUM_CONE_EXT = 7.0;

SCORE_STOW_ANGLE = 35;
SCORE_STOW_EXT = 0;
# ~~~~ END ARM STATES ~~~~

DRIVER_STOW_THRESHOLD = 8;

TELESCOPE_TOLERANCE = 1;
PIVOT_TOLERANCE = 5;
WRIST_TOLERANCE = 5;
PIVOT_WHILE_EXT_TOLERANCE = 20;

ARM_LPF_BETA = 0.07;
# DRIVE STATOR LIMITING
STATOR_LIMIT = 80;
STATOR_THRESHOLD = 95;
STATOR_DURATION = 0.28;

TIP_LPF = 0.1;

# CAMERA
LIMELIGHT_X_KP = 0.048;
LIMELIGHT_X_KD = 0.1;

LIMELIGHT_Y_KP = 0;
LIMELIGHT_Y_KD = 0;

TARGET_Y_CLOSE = 5000;
TARGET_Y_FAR = -10000;
Y_TARGETING_FLOOR = -20.0;

X_FILTER_SIZE = 2;
Y_FILTER_SIZE = 1;

TRACKING_THRESHOLD = 0.1;

CAMERA_W = 88;
CAMERA_H = 72;
CAMERA_FPS = 120;

# SWERVE + DRIVETRAIN
# 20.21 real max
SWERVE_MAX_SPEED = 20.21;
DESIRED_MAX_SPEED = 20.21;
SWERVE_MAX_ANGULAR_VELOCITY = 215;
DESIRED_MAX_ANG_VEL = 900;

SWERVE_MAX_ROTATIONAL_ACCEL = 4000;

WHEEL_BASE = 1.6;
# 1.0472 below originally
WHEEL_CIRCUMFERENCE = 0.85327;

SWERVE_DRIVE_GEAR_RATIO = 5.5;
SWERVE_ROTATION_GEAR_RATIO = 15.43;

# INDIVIDUAL MODULE

SWERVE_ANGLE_P = 0.32;
SWERVE_ANGLE_I = 0;
SWERVE_ANGLE_D = 0;

# NOT USED, FOR VELOCITY CONTROL
SWERVE_DRIVE_P = 0.01;
SWERVE_DRIVE_I = 0;
SWERVE_DRIVE_D = 0;
SWERVE_DRIVE_F = 0;

# WHOLE ROBOT POSE
AUTO_DRIVE_P = 12;
AUTO_DRIVE_I = 0;
AUTO_DRIVE_D = 0;

AUTO_ROTATION_P = 12;
AUTO_ROTATION_I = 0;
AUTO_ROTATION_D = 0;

AUTO_HOLD_DRIVE_P = 5;
AUTO_HOLD_DRIVE_I = 0;
AUTO_HOLD_DRIVE_D = 0;

AUTO_HOLD_ROTATION_P = 7;
AUTO_HOLD_ROTATION_I = 0;
AUTO_HOLD_ROTATION_D = 0;

# BBOT module encoders
#SWERVE_FL_ENCODER_OFFSET = 103.711;
#SWERVE_FR_ENCODER_OFFSET = 20.566;
#SWERVE_BL_ENCODER_OFFSET = 243.369;
#SWERVE_BR_ENCODER_OFFSET = 10.371;

SWERVE_FL_ENCODER_OFFSET = 260.4204;
SWERVE_FR_ENCODER_OFFSET = 253.037;
SWERVE_BL_ENCODER_OFFSET = 357.539;
SWERVE_BR_ENCODER_OFFSET = 26.367;

STICK_DEADBAND = 0.1;
#STICK_DEADBAND = 0.075;
STICK_EXPONENTIAL_MODIFIER = 1.6;

CUBE_Y_P = 0;
CUBE_Y_I = 0;
CUBE_Y_D = 0;
CUBE_Y_TOLERANCE = 0.02;

CUBE_YAW_P = 0;
CUBE_YAW_I = 0;
CUBE_YAW_D = 0;
CUBE_YAW_TOLERANCE = 5;

CONE_Y_P = 0.25;
CONE_Y_I = 0;
CONE_Y_D = 0.002;
CONE_Y_TOLERANCE = 0.02;
CONE_Y_OVERRIDE_THRESHOLD = 5;

HEADING_PID_THRESHOLD = .5;
HEADING_P = 10;
HEADING_I = 0;
HEADING_D = 0;
HEADING_V = 0;
HEADING_A = 0;

CHARGE_INIT_THRESHOLD = 23;

# was originally 14 (13.5)
CHARGE_TIP_THRESHOLD = 13.5;

GYRO_RESET_ACCEL = 5.5;