#pragma once

const float ACCEL_LIMIT = 5.0; // cm/s/s
const float VEL_LIMIT = 10.0; // cm/s
const float BACKLASH_RIGHT = 0.0; // degrees
const float BACKLASH_LEFT = 0.0; // degrees
const float WHEEL_DIAMETER = 9.25; // cm
const float WHEEL_TO_WHEEL = 10.0; // 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 = 180.0; // degrees
const float TURN_DISTANCE = (TURN_AMOUNT / 360.0) * WHEEL_TO_WHEEL * PI;
const float RETURN_DISTANCE = 100.0; // cm

const float FF_ACCEL = 0.7; // motor acceleration feedforward
const float FF_VEL = 0.6; // motor velocity feedforward
const float FF_STAT = 15.4; // motor static friction
const float KP = 2.0; // proportional
const float KI = 0.5; // integral
const float KD = 0.05; // derivative
const float KPP = 3.55; // position proportional
const float KPI = 1.50; // position integral