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

// Uses LCD Display in 4-bit mode
// Uses 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 v8
//***************************************************************************************
// 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 end of string)
const unsigned char msg1[6] = {"Ready"};
const unsigned char msgadr1 = 0x80;

const unsigned char msg2[11] = {"Hello Dere"};
const unsigned char msgadr2 = 0xC4;

const unsigned char msg3[12] = {"SPI Display"};
const unsigned char msgadr3 = 0xC4;

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;

// For reading, debouncing, and processing the switches
struct myswitch
{
    unsigned char debounce;     // debounce byte
    unsigned char state;        // debounced state of the switch
    unsigned char old;          // old state (not used here)
    unsigned char press;        // switch has been pressed (not used here)
    unsigned char count;        // hos long the press clock has been running (not used here)
    unsigned char release;      // how long the release clock has been running (not used here)
    unsigned char clockrun;     // is the clock running? (not used here)
};

struct myswitch switch1 = {0xFF,0x01,0x00,0x00, 0x00, 0x00, 0xFF};
struct myswitch switch2 = {0xFF,0x01,0x00,0x00, 0x00, 0x00, 0xFF};
struct myswitch switch3 = {0xFF,0x01,0x00,0x00, 0x00, 0x00, 0xFF};
struct myswitch switch4 = {0xFF,0x01,0x00,0x00, 0x00, 0x00, 0xFF};

void do_switch(unsigned char, unsigned char, struct myswitch *);  // PortIn, Bit, switch structure
//================================================
int main(void)
{
    unsigned char caldata1, caldata2;       // Temp. storage for MSP430 constants
    unsigned int n;

    WDTCTL = WDTPW + WDTHOLD;   // Stop watchdog timer

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

    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

// Initialize LCD and put out stuff
    lcd_reset();

    domsg(msg1, msgadr1);
//    domsg(msg2, msgadr2);

    lcd_wr_hex_byte(0x86, 0x12);  // display address, hex byte
    lcd_wr_bcd3(0x89, 0x12);      // display address, BCD byte

    lcd_wr_hex_int(0xC0, 0xFEDC); // display address, hex int
    lcd_wr_bcd5(0xC6, 0xFEDC);    // display address, BCD int with leading zero suppression

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

    __bis_SR_register(GIE);       // Timer interrupts without LPM0

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

       if(--n == 0)      // 250 * 4 ms = 1 Second
       {
           P1OUT ^= BIT5;           // Toggle LED1 (P1.5) for testing
           n = 250;
       }

// Debounce and process the switches
       do_switch(P1IN, BIT0, &switch1);      // PortIn, Bit Mask, switch structure

       if(switch1.state == 0)       // switch is pressed
           {P1OUT |= BIT4;}         // Use LED1 (P1.4) for testing switch debouncing
       else
           {P1OUT &= ~BIT4;}

    }  // 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 BCD
// 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)
{
   char *ptr = msg;
//   unsigned char temp;

   lcd_wr_instr(adr);   // position

   while(1)
   {
//      temp = *ptr;   // data
      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, char8;
   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('.');
}
//==============================
//=========================
void lcd_wr_bcd5(unsigned int pos, unsigned long bin)
{
// First Convert Binary to BCD
// 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, char8;
   int i;
   unsigned char hcarry;

//  signed long bin1 = 1048575;    // testing: maximum 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 if you want to
   lcd_wr_data(char6);
   lcd_wr_data(char7);
   lcd_wr_data('.');
}
//==============================
//=========================
void do_switch(unsigned char port, unsigned char bit, struct myswitch *sptr)          // PortIn, Bit, switch structure
{
    unsigned char temp;

    // Debounce and process the switches
    // sptr points to the switch's data block
    sptr->debounce  = sptr->debounce <<1;    // shifts in a zero
    if(port & bit)   // a zero has already been shifted in
    {
       sptr->debounce++ ;      // so make LSB a one
    }

// Debounced state is either 0x00 or 0xFF
    if(sptr->debounce == 0x00)  {sptr->state = 0x00;}
    if(sptr->debounce == 0xFF)  {sptr->state = 0x01;}

// We now know the state of the switch
// for testing
    temp = sptr->state;
    asm(" nop");    // for emulator
}
//===========================================