//------------------------------------------------------------------------------
void Set_DCO (void)                         // Set DCO to selected frequency
//------------------------------------------------------------------------------
{
  unsigned int Compare, Oldcapture = 0;
  #define DELTA     195                       // Target DCO = DELTA*(4096) ~800 kHz
 // #define DELTA     244                       // Target DCO = DELTA*(4096) ~1000 kHz
//  #define DELTA     600                       // Target DCO = DELTA*(4096) ~2400 kHz
//  #define DELTA     122                       // Target DCO = DELTA*(4096) ~0.5 MHz

  CCTL2 = CM_1 + CCIS_1 + CAP;              // CAP, ACLK
  TACTL = TASSEL_2 + MC_2 + TACLR;          // SMCLK, cont-mode, clear

  while (1)
  {
    while (!(CCIFG & CCTL2));               // Wait until capture occured
    CCTL2 &= ~CCIFG;                        // Capture occured, clear flag
    Compare = CCR2;                         // Get current captured SMCLK
    Compare = Compare - Oldcapture;         // SMCLK difference
    Oldcapture = CCR2;                      // Save current captured SMCLK
    if (DELTA == Compare) break;            // If equal, leave "while(1)"

    else if (DELTA < Compare)
    {
      DCOCTL--;
      if (DCOCTL == 0xFF)                   // DCO is too fast, slow it down
      {
        if (!(BCSCTL1 == (XT2OFF + DIVA_3)))
        BCSCTL1--;                          // Did DCO role under?, Sel lower RSEL
      }
    }
    else
    {
      DCOCTL++;                             // DCO is too slow, speed it down
      if (DCOCTL == 0x00)
      {
        if (!(BCSCTL1 == (XT2OFF + DIVA_3 + 0x07)))
        BCSCTL1++;                          // Did DCO role over? Sel higher RSEL
      }
    }
  }