 byte crc = 0;
 
 // power measurment //
-#define AMPS_SIZE 30
+#define AMPS_SIZE 20
 #define AMPS_INTERVAL 50
 int amps[AMPS_SIZE];
 byte ampsIndex = 0;
 unsigned long lastAmpsTime = 0;
-long uAh = 0;
+long uAh = 0; // coulomb counter
 Adafruit_INA219 ina219;
 
+// in-robot display //
 #define DISPLAY_ALTER_INTERVAL 1500
 TM1650 tm;
 unsigned long lastTmTime = 0;
 int tmMode = 0;
 
 // common vars //
+#define COMMAND_TIMEOUT 1200
 unsigned long lastCommandTime;
-unsigned long commandTimeout = 2500;
 boolean IsConnected = false;
 
-// RoboClaw //
+// drive //
 #define RC_ADDR 0x80
-RoboClaw roboclaw(DRIVER_RX_PIN, DRIVER_TX_PIN);
-boolean driveEnabled = false;
-unsigned long lastDriveCommandTime;
-unsigned long driveCommandTimeout = 20000;
+#define DRIVE_COMMAND_TIMEOUT 20000
 #define MOTOR_COUNTER_START 3000
 #define MOTOR_TARGET_MAX 127
 #define MOTOR_VELOCITY_0 64
 #define MOTOR_VELOCITY_MAX 63
-#define MOTOR_TARGET_MAX 127
+RoboClaw roboclaw(DRIVER_RX_PIN, DRIVER_TX_PIN);
+boolean driveEnabled = false;
+unsigned long lastDriveCommandTime;
 byte motor1 = MOTOR_VELOCITY_0;
 byte motor1_target = MOTOR_VELOCITY_0;
 byte motor2 = MOTOR_VELOCITY_0;

void telemetry()
{
  int mV = int(ina219.getBusVoltage_V() * 1000);
  int mA = getAvgAmps();
  Serial.write('b');
  Serial.println(mV);
  Serial.write('a');

//          in-robot Volts/Amps measurment

//=======================================

void ResetAmps()
{
  uAh = 0;
  for (int i = 0; i < AMPS_SIZE; i++) amps[i] = 0;
}

void ReadAmps()
{
  unsigned long ampsTime = millis();
  if (ampsTime < lastAmpsTime) lastAmpsTime = 0;
  if (ampsTime - lastAmpsTime >= AMPS_INTERVAL)
  {
    lastAmpsTime = ampsTime;
    amps[ampsIndex] = int(ina219.getCurrent_mA());
    
    // reset uAh counter on charge begin/end
    int prev, cur;
    cur = amps[ampsIndex];
    if (ampsIndex > 0) prev = amps[ampsIndex - 1]; else prev = amps[AMPS_SIZE - 1];
    if ((cur > 0 && prev < 0) || (cur < 0 && prev > 0))
    {
      ResetAmps();
      return;
    }
    
    ampsIndex++;
    if (ampsIndex >= AMPS_SIZE)
    {
      ampsIndex = 0;
      Update_uAh();
    }
  }
}

float getAvgAmps()
{
  long ampsSum = 0;
  for (int i = 0; i < AMPS_SIZE; i++) ampsSum += amps[i];
  return ampsSum / AMPS_SIZE;
}

void Update_uAh()
{
  float mA = getAvgAmps();
  uAh += int(mA * 5 / 18);
}

void DisplayVA()
{
  unsigned long tmTime = millis();
  if (tmTime < lastTmTime) lastTmTime = 0;
  if (tmTime - lastTmTime >= DISPLAY_ALTER_INTERVAL)
  {
    lastTmTime = tmTime;
    
    if (tmMode == 0)
    {
      displayVolts();
