#define HED    0xAB      // Header def:0xAB
#define EOF1   0x0D      // EOF1
#define EOF2   0x0A      // EOF2 

#rom int 0xf00000={1,2,3,4}

static long dat[3]= {0,0,0};
static unsigned long duty  = 0x1234;

void get_votage(unsigned char);
void show_tlm  (unsigned char,unsigned char,unsigned char);

void main()
{
   unsigned char head;
   unsigned char cmd;
   unsigned char dat1;
   unsigned char dat2;
   unsigned char tmp;

   setup_adc(ADC_CLOCK_DIV_64);
   setup_adc_ports(AN0_TO_AN2_ANALOG|VSS_VDD);
   setup_timer_2(T2_DIV_BY_16,0xFF,1);
   setup_ccp1(CCP_PWM);
   setup_ccp2(CCP_PWM);

   usb_cdc_init();
   usb_init();

   enable_interrupts(INT_RDA);
   enable_interrupts(GLOBAL);

   while(!usb_cdc_connected()) {}

   do {
      usb_task();
      if (usb_enumerated()) {
            if (usb_cdc_kbhit()){

               // Check command header
               head = usb_cdc_getc();
               if (head != HED) continue;

               // Receive command & data
               cmd  = usb_cdc_getc();
               dat1 = usb_cdc_getc();
               dat2 = usb_cdc_getc();

               // Analysis command
               switch(cmd)
               {
                  case 0x00: break;
                  case 0x01: show_tlm(cmd,0x12,0x34); break;
                  case 0x02: show_tlm(cmd,dat1,dat2); break;
                  case 0x03: tmp = duty; show_tlm(cmd,duty >> 8,tmp & 0x00FF); break;

                  case 0xA0: get_votage(0); tmp = dat[0]; show_tlm(cmd,dat[0] >> 8,tmp & 0x00FF); break;
                  case 0xA1: get_votage(1); tmp = dat[1]; show_tlm(cmd,dat[1] >> 8,tmp & 0x00FF); break;
                  case 0xA2: get_votage(2); tmp = dat[2]; show_tlm(cmd,dat[2] >> 8,tmp & 0x00FF); break; 

                  case 0xD0: duty = dat1; duty = ((duty << 8) | dat2 & 0x00FF); break;         //  0%
                  case 0xD1: set_pwm1_duty(duty);    break;                                    //  pwm1
                  case 0xD2: set_pwm2_duty(duty);    break;                                    //  pwm2
                  case 0xD3: duty = 0x1234; break;
               };
               if (duty > 0x03FF) duty = 0x03FF;
               head = 0x00;
            }
         }
   } while (TRUE); 
}
void get_votage(unsigned char chn)
{
   set_adc_channel(chn);
   dat[chn] = read_adc(ADC_START_AND_READ); 

   delay_ms(100);
}
void show_tlm(unsigned char cmd,unsigned char d1,unsigned char d2)
{
   usb_cdc_putc(cmd);
   usb_cdc_putc(d1);
   usb_cdc_putc(d2);
   usb_cdc_putc(EOF1);
   usb_cdc_putc(EOF2);
}