//***************************************************************************************
// gp1  General Test Board for MSP-EXP430G2 Launchpad
// gp1-adc2.c
// MSP430G2553

// Read two channels of ADC and display on LCD
// Use simple RC filtering

// Use 16 MHz DCO for SIMCLK and MCLK

// Requires that optimization be turned off in 'Project'.
//    Otherwise the compiler might remove software timers

// Jed Margolin  11/18/2018

//  Built with Code Composer Studio v7
//***************************************************************************************
// main.c

#include <msp430.h>

#define DELTA_1MHZ    244       // 244 x 4096Hz = 999.4Hz
#define DELTA_8MHZ    1953      // 1953 x 4096Hz = 7.99MHz
#define DELTA_12MHZ   2930      // 2930 x 4096Hz = 12.00MHz
#define DELTA_16MHZ   3906      // 3906 x 4096Hz = 15.99MHz

void domsg(unsigned int, unsigned char);

// Size of msg array is number of message chars + 1
//  (the +1 adds a zero to the string to indicate the end of the string)
const unsigned char msg1[6] = {"POT 1"};
const unsigned char msgadr1 = 0x80;

const unsigned char msg2[6] = {"POT 2"};
const unsigned char msgadr2 = 0xC0;

void wait(unsigned int);
void wait1(void);
void wait2(void);
void wait4(unsigned int);

void Set_DCO(unsigned int Delta);

void lcd_reset(void);
void wait10ms(void);
void lcd_wr4(unsigned int);
void lcd_wr_instr(unsigned int);
void lcd_wr_data(unsigned int);
void lcd_wr_bcd3(unsigned int, unsigned int);
void lcd_wr_bcd5(unsigned int, unsigned long);
void lcd_wr_bcd6(unsigned int, unsigned long);
void lcd_clear(unsigned char);
void lcd_wr_hex_byte(unsigned char, unsigned char);
void lcd_wr_hex_int(unsigned char, unsigned int);
int test_bcd(void);

// Globals
volatile unsigned int timer_flag = 0;
//================================================
int main(void)
  {
    unsigned char caldata1, caldata2;       // Temp. storage for MSP430 constants
    unsigned int n;
    volatile int adcval[2], new_adcval_0, new_adcval_1;
    volatile long adcfilter0 = 0, adcfilter1 = 0, adctemp0, adctemp1;

    WDTCTL = WDTPW + WDTHOLD;   // Stop watchdog timer

    wait(150);              // give the crystal 500ms to stabilize

// P1.0 used for ADC 0
// P1.1 used for ADC 1
// ADC is setup later

//    P1SEL  &= ~BIT0;         // In   SW1
//    P1SEL2 &= ~BIT0;
//    P1REN |= BIT0;          // pullup/pulldown enabled
//    P1OUT |= BIT0;          // pullup
//    P1DIR &= ~BIT0;       // In

//    P1SEL  &= ~BIT1;         // In   SW2
//    P1SEL2 &= ~BIT1;
//    P1REN |= BIT1;          // pullup/pulldown enabled
//    P1OUT |= BIT1;          // pullup
//    P1DIR &= ~BIT1;       // In

    P1SEL  &= ~BIT2;         // In   SW3
    P1SEL2 &= ~BIT2;
    P1REN |= BIT2;          // pullup/pulldown enabled
    P1OUT |= BIT2;          // pullup
    P1DIR &= ~BIT2;       // In

    P1SEL  &= ~BIT3;         // In   SW3
    P1SEL2 &= ~BIT3;
    P1REN |= BIT3;          // pullup/pulldown enabled
    P1OUT |= BIT3;          // pullup
    P1DIR &= ~BIT3;       // In

    P1SEL  &= ~(BIT4);      // Out - LED1
    P1SEL2 &= ~(BIT4);
    P1DIR |= BIT4;          // out
    P1OUT &= ~(BIT4);       // init to 0

    P1SEL  &= ~(BIT5);      // Out - LED2
    P1SEL2 &= ~(BIT5);
    P1DIR |= BIT5;          // out
    P1OUT &= (~BIT5);       // init to 0

    P1SEL  &= ~(BIT6);      // Out - LED3
    P1SEL2 &= ~(BIT6);
    P1DIR |= BIT6;          // out
    P1OUT &= (~BIT6);       // init to 0

    P1SEL  &= ~(BIT7);      // Out - LED4
    P1SEL2 &= ~(BIT7);
    P1DIR |= BIT7;          // Out
    P1OUT &= ~BIT7;         // init to 0

// Port 2 for LCD
    P2SEL  &= ~(0x3F);      // Out
    P2SEL2 &= ~(0x3F);
    P2DIR |= 0x3F;          // Out
    P2OUT &= 0x3F;          // LCD D4-7 = 0; LCD E = 0; LCD RS = 0

// Lock the DCO to the 32768 crystal - 16 MHz
    Set_DCO(DELTA_16MHZ);       // Set DCO and obtain constants
    caldata1 = DCOCTL;      // for debugging
    caldata2 = BCSCTL1;

    BCSCTL2 = 0x00;              // DIVM0 | DIVS0 |   // MCLK and SMCLK dividers

//    k = test_bcd();

// Initialize LCD
    lcd_reset();
    domsg(msg1, msgadr1);
    domsg(msg2, msgadr2);

// Set up ADC10
    adcval[0] = 0;
    adcval[1] = 0;

    ADC10CTL1 = INCH_1 + CONSEQ_1;         // 0x1002 = A1,A0  single conversion multi channel

    ADC10AE0 = 0x03;                       // P1.0, P1.1 ADC option select
    ADC10DTC1 = 0x02;                      // 2 conversions
    ADC10SA = adcval;                      // Data buffer start

// ADC10CTL0 = SREF = 0: VCC and VSS; ADCSHT = 10: 16 ADC10CLKs;
//             ADC10SR = 0: sampling rate up to 200 ksps; REFOUT = 0; REFBURST = 0
//             MSC = 1; REF2_5V = 1; REFON = 1; ADC10ON = 1; ADC10IE = 0: interrupts off;
//             ADC10IFG: Interrupt flag (conversion complete); ENC: enable conversion;
//             ADC10SC: start conversion   [0x10F0]
    ADC10CTL0 = 0x10F0;


// Set up timer Interrupts - 4ms
    CCTL0 = CCIE;                    // CCR0 interrupt enabled
    CCR0 = 16000;                    // 4ms interrupts
    TACTL = TASSEL_2 | ID_3 | MC_2;   // SMCLK, contmode
    TACTL |= TACLR;          // clear timer Resets TAR, clock divider, count direction

    __bis_SR_register(GIE);       // Timer interrupts without LPM0

    n = 60;  // 60 x 4 = 240 ms
//===========================================
    while (1)           // main loop
    {
       while(!timer_flag)  { }        // wait for timer_flag to go high (4 ms interrupt)
       timer_flag = 0;

       ADC10SA = adcval;                      // Data buffer start address
       ADC10CTL0 |= ENC + ADC10SC;             // Go

       while (ADC10CTL1 & ADC10BUSY);          // Wait if ADC10 core is active
//           P1OUT &= ~BIT3;                         // P1.3 low

// do simple RC filter
       adctemp0 = adcval[1];        // convert to long
       adctemp0 = adctemp0<<16;

       // time constant is determined by the number of shifts for the sample rate
       adctemp0 = (adctemp0 - adcfilter0)>>6;

       // fraction of (new value - oldvalue) + old value, long
       adcfilter0 = adctemp0 + adcfilter0;

       adctemp1 = adcval[0];        // convert to long
       adctemp1 = adctemp1<<16;

       // time constant is determined by the number of shifts for the sample rate
       adctemp1 = (adctemp1 - adcfilter1)>>6;

       // fraction of (new value - oldvalue) + oldvalue, long
       adcfilter1 = adctemp1 + adcfilter1;

       if(--n == 0)      // 60 * 4 ms = 240ms
       {
//           P1OUT ^= BIT4;           // Toggle P1.7 for testing

          n = 60;

          lcd_wr_hex_int(0x87, (adcfilter0>>16) & 0xFFFF);  // make and display an int
          lcd_wr_hex_int(0xC7, (adcfilter1>>16) & 0xFFFF);  // make and display an int
        }

        __no_operation();                       // space for debugger

    }  // end while main loop

} // end main

//==================================
// Timer A0 interrupt service routine
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=TIMER0_A0_VECTOR
__interrupt void Timer_A (void)
#elif defined(__GNUC__)
void __attribute__ ((interrupt(TIMER0_A0_VECTOR))) Timer_A (void)
#else
#error Compiler not supported!
#endif
{
//  P1OUT ^= BIT3;                          // Toggle P1.1
  CCR0 += 16000;                            // Add Offset to CCR0
  timer_flag = 1;
}
// ====================================
void wait(unsigned int time)        // 500 uS
{
    volatile unsigned int i,j;

    for(i=time; i>0; i--)
    {
        for(j=382; j>0; j--);
        {
        }
    }

    return;
}
//==========================================================
void wait4(unsigned int time)        // 500 uS
{
    volatile unsigned int i,j;

    for(i=time; i>0; i--)
    {
        for(j=20; j>0; j--);
        {
        }
    }

    return;
}
//==========================================================
void wait2()
{
volatile unsigned int i;

    for(i=0; i<48; i++){}
    return;
}

void wait1()
{
volatile unsigned int i;

    i = 0;
//    for(i=0; i<1; i++){}
    return;
}
//========================================================
void Set_DCO(unsigned int Delta)            // Set DCO to selected frequency
{
  unsigned int Compare, Oldcapture = 0;

// XT2OFF; LF Mode; DIVA_3: ACLK = LFXT1CLK/1
  BCSCTL1 |= DIVA_3;

// MCLK is DCOCLK; Divider /1; SIMCLK is DCOCLK; SMCLK Divider /1; DCOR
// BCSCTL2 = 0;  Reset = 0 anyway

// Oscillator capacitor 12.5pF
//  BCSCTL3 |= XCAP_3;              // MSP-EXP430G2ET has them on the board

// Timer/Capture
  TACCTL0 = CM_1 + CCIS_1 + CAP;

// Timer A Control
  TACTL = TASSEL_2 + MC_2 + TACLR;          // SMCLK, cont-mode, clear

  while (1)
  {
    while (!(CCIFG & TACCTL0));             // Wait until capture occured
    TACCTL0 &= ~CCIFG;                      // Capture occured, clear flag
    Compare = TACCR0;                       // Get current captured SMCLK
    Compare = Compare - Oldcapture;         // SMCLK difference
    Oldcapture = TACCR0;                    // Save current captured SMCLK

    if (Delta == Compare)
      break;                                // If equal, leave "while(1)"
    else if (Delta < Compare)
    {
      DCOCTL--;                             // DCO is too fast, slow it down
      if (DCOCTL == 0xFF)                   // Did DCO roll under?
        if (BCSCTL1 & 0x0f)
          BCSCTL1--;                        // Select lower RSEL
    }
    else
    {
      DCOCTL++;                             // DCO is too slow, speed it up
      if (DCOCTL == 0x00)                   // Did DCO roll over?
        if ((BCSCTL1 & 0x0f) != 0x0f)
          BCSCTL1++;                        // Sel higher RSEL
    }
  }
  TACCTL0 = 0;                              // Stop TACCR0
  TACTL = 0;                                // Stop Timer_A

  BCSCTL1 &= ~DIVA_3;                       // 0 = ACLK is divide by 1
}
//==========================================================
void lcd_reset(void)
{
/*
P2.0 - LCD D4
P2.1 - LCD D5
P2.2 - LCD D6
P2.3 - LCD D7

P2.4 - LCD E (Enable High, Write Data, Enable Low)
P2.5 - LCD RS (Register Select; RS low = Instruction, RS High = Data)
 */

// Wait at least 16 ms after power-up
    wait10ms();    // wait 10 ms
    wait10ms();    // wait 10 ms

    P2OUT &= (~BIT5);    // RS = 0

    lcd_wr4(0x03);      // write nybble and wait 200us
    wait10ms();

    lcd_wr4(0x03);      // write nybble and wait 200us
    lcd_wr4(0x03);      // write nybble and wait 200us
    lcd_wr4(0x02);      // write nybble and wait 200us
    lcd_wr_instr(0x28); //  2 lines, 5x7 dots
    lcd_wr_instr(0x08); //  display off
    lcd_wr_instr(0x01); //  display on

    wait10ms();

//    lcd_wr_instr(0x0F); //  display on, cursor on, blink on
    lcd_wr_instr(0x0c);  //  display on, cursor oFF, blink oFF

    lcd_wr_instr(0x06);  //  entry mode set: cursor increment, no shift
}
//======================
// wait 10 ms
void wait10ms(void)
{
   wait(41);
}
//======================
void lcd_wr4(unsigned int data)      // write nybble and wait 200us
{

//    P2.4 - LCD E (Enable High, Write Data, Enable Low)
//    P2.5 - LCD RS (Register Select; RS low = Instruction, RS High = Data)

    P2OUT |= BIT4;     // Enable High

    data &= 0x0F;
    P2OUT &= 0xF0;
    P2OUT |= data;     // write nybble

    wait4(2);         // 20 us for data setup

    P2OUT &= (~BIT4);  //  /E low

    wait4(20);         // 200 us
}
//======================
// LCD - Write 8-bit instruction
// P2.5 - LCD RS (Register Select; RS low = Instruction, RS High = Data)
void lcd_wr_instr(unsigned int data)
{
   unsigned int dsave;

   P2OUT &= (~BIT5);    // RS = 0

   dsave = data;
   dsave  = dsave >>4;       // high nybble
   lcd_wr4(dsave);  // write nybble and wait 200 us

   data &=0xF;      // low nybble4
   lcd_wr4(data);
}
//======================
// LCD - Write 8-bit data
// P2.5 - LCD RS (Register Select; RS low = Instruction, RS High = Data)
void lcd_wr_data(unsigned int data)
{
    unsigned int dsave;

    P2OUT |= BIT5;    // RS = 1

    dsave = data;
    dsave = dsave >>4;       // high nybble
    lcd_wr4(dsave);  // write nybble and wait 200 us

    data &=0xF;      // low nybble4
    lcd_wr4(data);
}
//======================
// Binary to ASCII - adapted from 8-bit microcontroller code using a DAA instruction
int test_bcd(void)
{
    unsigned int bin = 0x386; // 0x67 = 103d; 0x386 = 902d
    unsigned char bcd0 = 0;
    unsigned char bcd1 = 0;
    unsigned char bcd2 = 0;
    int i;
    unsigned char hcarry;

    for(i=0; i<12; i++)
    {
//---------- BCD0
        bcd0 = bcd0<<1;

        if(bin & 0x800)
        {bcd0 = bcd0 +1;}

        bin = bin<<1;

        if(bcd0 >9)
        {
            bcd0 = bcd0 - 10;
            hcarry = 1;
        }
        else {hcarry = 0;

        }
//---------- BCD1
        bcd1 = bcd1<<1;
        bcd1 = bcd1 + hcarry;

        if(bcd1 >9)
        {
            bcd1 = bcd1 - 10;
            hcarry = 1;
        }
        else {hcarry = 0;}

//---------- BCD2
        bcd2 = bcd2<<1;
        bcd2 = bcd2 + hcarry;

        if(bcd2 >9)
        {
            bcd2 = bcd2 - 10;
//            bcd3 = bcd3 + 1;
//            hcarry = 1;
        }
//        else {hcarry = 0;}

    }

    return(1);
}

//======================
void lcd_wr_bcd3(unsigned int pos, unsigned int bin)
{
// First Convert Binary to ASCII
// Adapted from 8-bit microcontroller code using a DAA instruction

// unsigned int bin = 0x386; // 0x67 = 103d; 0x386 = 902d
   unsigned char bcd0 = 0;
   unsigned char bcd1 = 0;
   unsigned char bcd2 = 0;
   unsigned char char1, char2, char3;
   int i;
   unsigned char hcarry;

   for(i=0; i<12; i++)
   {
//---------- BCD0
      bcd0 = bcd0<<1;

      if(bin & 0x800)
      {bcd0 = bcd0 +1;}

      bin = bin<<1;

      if(bcd0 >9)
      {
         bcd0 = bcd0 - 10;
         hcarry = 1;
      }
      else {hcarry = 0; }

//---------- BCD1
      bcd1 = bcd1<<1;
      bcd1 = bcd1 + hcarry;

      if(bcd1 >9)
      {
          bcd1 = bcd1 - 10;
          hcarry = 1;
      }
      else {hcarry = 0;}

//---------- BCD2
      bcd2 = bcd2<<1;
      bcd2 = bcd2 + hcarry;

     if(bcd2 >9)
     {
         bcd2 = bcd2 - 10;
//       bcd3 = bcd3 + 1;
//       hcarry = 1;
     }
//        else {hcarry = 0;}

   }

// Convert to ascii
   char1 = bcd2 + 0x30;
   char2 = bcd1 + 0x30;
   char3 = bcd0 + 0x30;

// Display it

   for(i=0; i<2; i++)
   {
       if(char1 != 0x30) {break;}
       else {char1 = ' ';}
       if(char2 != 0x30) {break;}
       else {char2 = ' ';}
   }

   lcd_wr_instr(pos);
   lcd_wr_data(char1);
   lcd_wr_data(char2);
   lcd_wr_data(char3);
   lcd_wr_data('.');

}
//=========================
// clear a line
void lcd_clear(unsigned char address)
{
   int n;

   lcd_wr_instr(address);

   for(n=1; n<16; n++)
   {
       lcd_wr_data(' ');
   }
}
//=========================
void lcd_wr_hex_byte(unsigned char pos, unsigned char hex)
{
    unsigned char digit1, digit2;

    digit1 = hex>>4;
    digit2 = hex & 0xF;

// Convert to ascii
   if(digit1<10)  {digit1 = digit1 + 0x30;}
   else {digit1 = digit1 + 0x37;}

   if(digit2<10)  {digit2 = digit2 + 0x30;}
   else {digit2 = digit2 + 0x37;}

   // Display it
   lcd_wr_instr(pos);
   lcd_wr_data(digit1);
   lcd_wr_data(digit2);
}
//=========================
void lcd_wr_hex_int(unsigned char pos, unsigned int hex)
{
    unsigned char digit1, digit2, digit3, digit4;

    digit1 = hex>>12 & 0x0F;
    digit2 = (hex>>8) & 0x0F;
    digit3 = (hex>>4) & 0x0F;
    digit4 = hex & 0xF;

// Convert to ascii
   if(digit1<10)  {digit1 = digit1 + 0x30;}
   else {digit1 = digit1 + 0x37;}

   if(digit2<10)  {digit2 = digit2 + 0x30;}
   else {digit2 = digit2 + 0x37;}

   if(digit3<10)  {digit3 = digit3 + 0x30;}
   else {digit3 = digit3 + 0x37;}

   if(digit4<10)  {digit4 = digit4 + 0x30;}
   else {digit4 = digit4 + 0x37;}

   // Display it
   lcd_wr_instr(pos);
   lcd_wr_data(digit1);
   lcd_wr_data(digit2);
   lcd_wr_data(digit3);
   lcd_wr_data(digit4);
}
//=========================
void domsg(unsigned int msg, unsigned char adr)
{
   unsigned char *ptr = msg;
//   unsigned char temp;

   lcd_wr_instr(adr);   // position

   while(1)
   {
      if( (*ptr == 0))  {break;}    // strings end with a value of 0x00
      lcd_wr_data(*ptr);   // data
      ptr++;
      asm(" nop");    // for emulator
   }
}
//=========================
void lcd_wr_bcd6(unsigned int pos, unsigned long bin)
{
// First Convert Binary to ASCII
// Adapted from 8-bit microcontroller code with a DAA instruction

   unsigned char bcd0 = 0;
   unsigned char bcd1 = 0;
   unsigned char bcd2 = 0;
   unsigned char bcd3 = 0;
   unsigned char bcd4 = 0;
   unsigned char bcd5 = 0;
   unsigned char bcd6 = 0;
   unsigned char bcd7 = 0;

   unsigned char char1, char2, char3, char4, char5, char6, char7;
   int i;
   unsigned char hcarry;

//  signed long bin1 = 1048575;             // maximun value is 1048575 = 0xFFFFF
//  bin = bin1;

   for(i=0; i<18; i++)      // 16
   {
//---------- BCD0
      bcd0 = bcd0<<1;

      if(bin & 0x80000)      // 0x8000
      {bcd0 = bcd0 +1;}

//      bin = bin<<1;

      if(bcd0 >9)
      {
         bcd0 = bcd0 - 10;
         hcarry = 1;
      }
      else {hcarry = 0; }

//---------- BCD1
      bcd1 = bcd1<<1;
      bcd1 = bcd1 + hcarry;

      if(bcd1 >9)
      {
          bcd1 = bcd1 - 10;
          hcarry = 1;
      }
      else {hcarry = 0;}

//---------- BCD2
      bcd2 = bcd2<<1;
      bcd2 = bcd2 + hcarry;

      if(bcd2 >9)
      {
         bcd2 = bcd2 - 10;
         hcarry = 1;
     }
     else {hcarry = 0;}

//---------- BCD3
     bcd3 = bcd3<<1;
     bcd3 = bcd3 + hcarry;

     if(bcd3 >9)
     {
        bcd3 = bcd3 - 10;
        hcarry = 1;
     }
     else {hcarry = 0;}

//---------- BCD4
     bcd4 = bcd4<<1;
     bcd4 = bcd4 + hcarry;

     if(bcd4 >9)
     {
        bcd4 = bcd4 - 10;
        hcarry = 1;
     }
     else {hcarry = 0;}

//---------- BCD5
     bcd5 = bcd5<<1;
     bcd5 = bcd5 + hcarry;

     if(bcd5 >9)
     {
        bcd5 = bcd5 - 10;
        hcarry = 1;
     }
     else {hcarry = 0;}

//---------- BCD6
     bcd6 = bcd6<<1;
     bcd6 = bcd6 + hcarry;

     if(bcd6 >9)
     {
        bcd6 = bcd6 - 10;
        hcarry = 1;
        bcd7 = 1;
     }
     else {hcarry = 0;}

     bin = bin<<1;
   }

// Convert to ascii
   char1 = bcd6 + 0x30;
   char2 = bcd5 + 0x30;
   char3 = bcd4 + 0x30;
   char4 = bcd3 + 0x30;
   char5 = bcd2 + 0x30;
   char6 = bcd1 + 0x30;
   char7 = bcd0 + 0x30;


// Remove leading zeros
   for(i=0; i<4; i++)
   {
       if(char1 != 0x30) {break;}
       else {char1 = ' ';}
       if(char2 != 0x30) {break;}
       else {char2 = ' ';}
       if(char3 != 0x30) {break;}
              else {char3 = ' ';}
       if(char4 != 0x30) {break;}
              else {char4 = ' ';}
   }

// Display it
   lcd_wr_instr(pos);
   lcd_wr_data(char1);
   lcd_wr_data(char2);
   lcd_wr_data(char3);
   lcd_wr_data(char4);
   lcd_wr_data(char5);
//   lcd_wr_data('.');   // you can put decimal points if you want to
   lcd_wr_data(char6);
   lcd_wr_data(char7);
   lcd_wr_data('.');
//   lcd_wr_data('H');  // You can add letters if you want without using do_msg
//   lcd_wr_data('z');

}
//==============================
//=========================
void lcd_wr_bcd5(unsigned int pos, unsigned long bin)
{
// First Convert Binary to ASCII
// Adapted from 8-bit microcontroller code with a DAA instruction

   unsigned char bcd0 = 0;
   unsigned char bcd1 = 0;
   unsigned char bcd2 = 0;
   unsigned char bcd3 = 0;
   unsigned char bcd4 = 0;
   unsigned char bcd5 = 0;
   unsigned char bcd6 = 0;
   unsigned char bcd7 = 0;

   unsigned char char1, char2, char3, char4, char5, char6, char7;
   int i;
   unsigned char hcarry;

//  signed long bin1 = 1048575;             // maximun value is 1048575 = 0xFFFFF
//  bin = bin1;

   for(i=0; i<16; i++)      // 16
   {
//---------- BCD0
      bcd0 = bcd0<<1;

      if(bin & 0x8000)      // 0x8000
      {bcd0 = bcd0 +1;}

//      bin = bin<<1;

      if(bcd0 >9)
      {
         bcd0 = bcd0 - 10;
         hcarry = 1;
      }
      else {hcarry = 0; }

//---------- BCD1
      bcd1 = bcd1<<1;
      bcd1 = bcd1 + hcarry;

      if(bcd1 >9)
      {
          bcd1 = bcd1 - 10;
          hcarry = 1;
      }
      else {hcarry = 0;}

//---------- BCD2
      bcd2 = bcd2<<1;
      bcd2 = bcd2 + hcarry;

      if(bcd2 >9)
      {
         bcd2 = bcd2 - 10;
         hcarry = 1;
     }
     else {hcarry = 0;}

//---------- BCD3
     bcd3 = bcd3<<1;
     bcd3 = bcd3 + hcarry;

     if(bcd3 >9)
     {
        bcd3 = bcd3 - 10;
        hcarry = 1;
     }
     else {hcarry = 0;}

//---------- BCD4
     bcd4 = bcd4<<1;
     bcd4 = bcd4 + hcarry;

     if(bcd4 >9)
     {
        bcd4 = bcd4 - 10;
        hcarry = 1;
     }
     else {hcarry = 0;}

//---------- BCD5
     bcd5 = bcd5<<1;
     bcd5 = bcd5 + hcarry;

     if(bcd5 >9)
     {
        bcd5 = bcd5 - 10;
        hcarry = 1;
     }
     else {hcarry = 0;}

//---------- BCD6
     bcd6 = bcd6<<1;
     bcd6 = bcd6 + hcarry;

     if(bcd6 >9)
     {
        bcd6 = bcd6 - 10;
        hcarry = 1;
        bcd7 = 1;
     }
     else {hcarry = 0;}

     bin = bin<<1;
   }

// Convert to ascii
   char1 = bcd6 + 0x30;
   char2 = bcd5 + 0x30;
   char3 = bcd4 + 0x30;
   char4 = bcd3 + 0x30;
   char5 = bcd2 + 0x30;
   char6 = bcd1 + 0x30;
   char7 = bcd0 + 0x30;


// Remove leading zeros
   for(i=0; i<4; i++)
   {
       if(char1 != 0x30) {break;}
       else {char1 = ' ';}
       if(char2 != 0x30) {break;}
       else {char2 = ' ';}
       if(char3 != 0x30) {break;}
              else {char3 = ' ';}
       if(char4 != 0x30) {break;}
              else {char4 = ' ';}
   }

   // Display it
   lcd_wr_instr(pos);
//   lcd_wr_data(char1);
//   lcd_wr_data(char2);
   lcd_wr_data(char3);
   lcd_wr_data(char4);
   lcd_wr_data(char5);
//   lcd_wr_data('.');   // you can put a decimal point here if you want to
   lcd_wr_data(char6);
   lcd_wr_data(char7);
   lcd_wr_data('.');
}
//==============================