 {
   IsConnected = true;
   lastCommandTime = millis();
+  tm.displayString("CONN");
 }
 
 //=======================================

  }
  else
  if (cmd == 'r') driveReset();
  else
  if (cmd == 'i') motor_increment = bctoi(2, 2);
  else
  {
    nak();
  }
}

// update motor velocities

  // serial input
  ReadSerial();
  if (IsConnected) CheckCommandTimeout();
  else UpdateVA();
  if (driveEnabled) CheckDriveCommandTimeout();
  
  // motors
  if (motor_counter > 0) motor_counter--;
  if (motor_counter == 0) updateMotors();
}

void UpdateVA()
{
  unsigned long tmTime = millis();
  if (tmTime < lastTmTime) lastTmTime = 0;
  if (tmTime - lastTmTime > DISPLAY_ALTER_INTERVAL)
  {
    if (tmMode == 0)
    {
      float shuntvoltage = ina219.getShuntVoltage_mV();
      float busvoltage = ina219.getBusVoltage_V();
      int voltage = (int)(busvoltage * 100 + shuntvoltage / 10);
      displayInt(voltage, 2);
      tmMode = 1;
    }
    else
    if (tmMode == 1)
    {
      int mA = abs(int(ina219.getCurrent_mA()));
      displayInt(mA, 1);
      tmMode = 0;
    }
    
    lastTmTime = tmTime;
  }
}

void displayInt(int value, int dot)
{
    char c[4];
    c[0] = '0' + int(value / 1000);
    c[1] = '0' + int(value % 1000 / 100);
    c[2] = '0' + int(value % 100 / 10);
    c[3] = '0' + int(value % 10);
    if (dot == 1) c[0] = c[0] | 0b10000000;
    else
    if (dot == 2) c[1] = c[1] | 0b10000000;
    else
    if (dot == 3) c[2] = c[2] | 0b10000000;
    tm.displayString(c);
}

void ina()
{
  uint16_t reg;
  cmd = buf[1]; 
  if (cmd == '0') ina219.wireReadRegister(0, &reg);
  else if (cmd == '1') ina219.wireReadRegister(1, &reg);
  else if (cmd == '2') ina219.wireReadRegister(2, &reg);
  else if (cmd == '3') ina219.wireReadRegister(3, &reg);
  else if (cmd == '4') ina219.wireReadRegister(4, &reg);
  else if (cmd == '5') ina219.wireReadRegister(5, &reg);
  Serial.print('i');
  Serial.println(reg);
}

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