#pragma once

const float ACCEL_LIMIT = 6.0; // cm/s/s
const float VEL_LIMIT = 12.0; // cm/s
const float WHEEL_DIAMETER = 6.61; // cm
const float WHEEL_TO_WHEEL = 8.6; // cm

// centimeters per second to rpm
const float CMS_RPM = 60.0 / (PI*WHEEL_DIAMETER);

// degrees to centimeters
const float DEG_CM = (PI*WHEEL_DIAMETER) / 360.0;

const float FORWARD_DISTANCE = 100.0; // cm
const float TURN_AMOUNT = 190.0; // degrees
const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI;

const float KP = 1.50; // proportional
const float KI = 0.12; // integral
const float KD = 0.00; // derivative
const float Kv = 1.85; // onboard velocity feedforward

const float TURN_KP = 1.50; // proportional
const float TURN_KI = 0.10; // integral
const float TURN_KD = 0.00; // derivative
const float TURN_Kv = 2.6; // onboard velocity feedforward

const float KPP = 0.14; // position proportional
const float KPI = 0.12; // position integral
const float KRP = 0.0; // rotation proportional
const float V_MIN = 5.5; // velocity minimum
const float V_KMIN = 0.7; // velocity minimum coefficient