#include <c:\piccw\devices\16f874.h>
#device 16f874 *=8 adc=10
#use delay(clock=4000000)
#use RS232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7)
#priority rtcc,timer1,rda,tbe     // Interrupt priority setting
#byte porta=0x05
#byte portb=0x06
#byte portc=0x07
#byte portd=0x08

void go_forward(void);
void turn_right(void);
void turn_left(void);
void spin_right(void);
void spin_left(void);
void slow_down(void);
void back_off(void);

const char PHASE_TABLE[4]={0x33,0x66,0xcc,0x99};
const char ACC_TABLE[255]={ 
 0xec, 0xd5, 0xc4, 0xb6, 0xab, 0xa2,
 0x9a, 0x93, 0x8d, 0x88, 0x83, 0x7f, 0x7b, 0x77, 0x74, 0x71,
 0x6e, 0x6b, 0x69, 0x67, 0x65, 0x63, 0x61, 0x5f, 0x5d, 0x5c,
 0x5a, 0x59, 0x57, 0x56, 0x55, 0x53, 0x52, 0x51, 0x50, 0x4f,
 0x4e, 0x4d, 0x4c, 0x4b, 0x4a, 0x4a, 0x49, 0x48, 0x47, 0x47,
 0x46, 0x45, 0x45, 0x44, 0x43, 0x43, 0x42, 0x41, 0x41, 0x40,
 0x40, 0x3f, 0x3f, 0x3e, 0x3e, 0x3d, 0x3d, 0x3c, 0x3c, 0x3b,
 0x3b, 0x3b, 0x3a, 0x3a, 0x39, 0x39, 0x39, 0x38, 0x38, 0x38,
 0x37, 0x37, 0x37, 0x36, 0x36, 0x36, 0x35, 0x35, 0x35, 0x34,
 0x34, 0x34, 0x33, 0x33, 0x33, 0x33, 0x32, 0x32, 0x32, 0x32,
 0x31, 0x31, 0x31, 0x31, 0x30, 0x30, 0x30, 0x30, 0x30, 0x2f,
 0x2f, 0x2f, 0x2f, 0x2e, 0x2e, 0x2e, 0x2e, 0x2e, 0x2d, 0x2d,
 0x2d, 0x2d, 0x2d, 0x2c, 0x2c, 0x2c, 0x2c, 0x2c, 0x2c, 0x2b,
 0x2b, 0x2b, 0x2b, 0x2b, 0x2b, 0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
 0x2a, 0x2a, 0x29, 0x29, 0x29, 0x29, 0x29, 0x29, 0x29, 0x28,
 0x28, 0x28, 0x28, 0x28, 0x28, 0x28, 0x27, 0x27, 0x27, 0x27,
 0x27, 0x27, 0x27, 0x27, 0x26, 0x26, 0x26, 0x26, 0x26, 0x26,
 0x26, 0x26, 0x26, 0x25, 0x25, 0x25, 0x25, 0x25, 0x25, 0x25,
 0x25, 0x25, 0x25, 0x24, 0x24, 0x24, 0x24, 0x24, 0x24, 0x24,
 0x24, 0x24, 0x24, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23,
 0x23, 0x23, 0x23, 0x23, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22,
 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x21, 0x21, 0x21, 0x21,
 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21, 0x21,
 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
 0x20, 0x20, 0x20, 0x20, 0x1f, 0x1f, 0x1f, 0x1f, 0x1f, 0x1f,
 };
 
int l_speed_aim=0,r_speed_aim=0;
int l_speed=0,r_speed=0;
int key_value=53;           // initial key is Center (Stop moving)
signed int l_sequence=0,r_sequence=0; 
short l_forward=1,r_forward=1;



#int_TBE           // RS232 Transmit complete interrupt ISR
TBE_isr()
{
}



#int_RDA           // RS232 Receive complete interrupt ISR
RDA_isr()
{
  char c;
  c=getc();        // Read 1Byte that received from RS232
  // printf("Key value received from RS232 is %u\n\r",(int)c);
  key_value=(int)c;
}



#int_RTCC               // Left Step Motor Pulse generate
RTCC_isr()             
{
  disable_interrupts(int_rtcc);      // Timer0 interrupt disable
  portd=(portd & 0xf0)|(0x0f & PHASE_TABLE[(int)l_sequence]);    // output signal
  
  switch(l_forward) {
    case 0 :                 // Left Step Motor rotate for reverse
      l_sequence++;
      if(l_sequence==4) l_sequence=0;
      break;
    case 1 :                 // Left Step Motor rotate for forward
      l_sequence--;                  
      if(l_sequence<0) l_sequence=3;
      break;
    }
  
  if(l_speed<l_speed_aim) l_speed++;
  else if(l_speed>l_speed_aim) l_speed--;
  
  set_rtcc(255-ACC_TABLE[l_speed]);  // Setting new value to Timer0
  enable_interrupts(int_rtcc);        // Timer0 interrupt enable again...
}



#int_timer1
timer1_isr()
{
  disable_interrupts(int_timer1);       // Timer2 interrupt disable
  portd=(portd & 0x0f)|(0xf0 & PHASE_TABLE[(int)r_sequence]);    // output signal
  
  switch(r_forward) {
    case 0 :                            // Right Step Motor rotate for reverse
      r_sequence++;
      if(r_sequence==4) r_sequence=0;
      break;
    case 1 :                            // Right Step Motor rotate for forward
      r_sequence--;                  
      if(r_sequence<0) r_sequence=3;
      break;
    }
  
  if(r_speed<r_speed_aim) r_speed++;
  else if(r_speed>r_speed_aim) r_speed--;
  
  set_timer1(65535-8*((long)ACC_TABLE[r_speed]));  // Setting new value to Timer2
  enable_interrupts(int_timer1);        // Timer2 interrupt enable again...
  
}



#int_timer2             // Right Step Motor Pulse generate
timer2_isr()
{

}



#int_rb
rb_isr()                           //PortB(4-7bit) change ISR 
{

}


void light_blink(void)
{
  int i;
  for(i=0;i<20;i++) {
    bit_set(portc,0);
    delay_ms(30);
    bit_clear(portc,0);
    delay_ms(20);
    }
}


void main() 
{
   int a=0,b,c;
   
   set_tris_a(0x00);
   set_tris_b(0x00);       
   set_tris_c(0x80);         // portc.0  Front light on/off
   set_tris_d(0x00);         // portd.0~3 Left Step Motor (Left SLA7024)
                             // portd.4~7 Right Step Motor (Right SLA7024)
   
   porta=0x00;
   portb=0x00;
   portc=0x00;
   portd=0x33;
   portb=0x00;
   
   setup_counters(RTCC_internal,rtcc_div_64);  //Timer 0 overflows every 4096us.
   setup_timer_1(t1_internal|t1_div_by_8);
   set_timer1(64515);
   setup_timer_2(t2_div_by_16,255,2);      //Timer 2 overflows every 4096us.
   
//   enable_interrupts(int_rb);
   enable_interrupts(int_rtcc);
   enable_interrupts(int_timer1);
//   enable_interrupts(int_timer2);
//   enable_interrupts(INT_TBE);
   enable_interrupts(INT_RDA);
   
   light_blink();
   
   printf(" ___________________________ \n\r");
   printf("|                           |\n\r");
   printf("|  RS-232 Controled Robot   |\n\r");
   printf("|___________________________|\n\r\n\r");

   printf(" Use Keypad Arrows to get moving!! \n\r Ready to Go.....^^ \n\r");
   l_forward=1;
   r_forward=1;
   enable_interrupts(GLOBAL);
   delay_ms(3000);
   

   while(1) {
     if(key_value!=53) {
       enable_interrupts(int_rtcc);
       enable_interrupts(int_timer1);
       }
       
     switch(key_value) {
       case 56 : go_forward();
                 break;
       case 57 : turn_right();
                 break;
       case 55 : turn_left();
                 break;
       case 52 : spin_left();
                 break;
       case 54 : spin_right();
                 break;
       case 53 : slow_down();
                 break;
       case 50 : back_off();
                 break;
       }
     if( (r_speed_aim!=r_speed) || (l_speed_aim!=l_speed) ) 
       bit_set(portc,0);          // LED on when changing velocity.
     else bit_clear(portc,0);     // LED off
     }
}



void go_forward(void)
{
  if(l_forward==0 && r_forward==1) {      // When spining right before
    while(l_speed>0) l_speed_aim=0;
    l_forward=1;
    delay_ms(100);
    }
  else if(l_forward==1 && r_forward==0) { // When spining left before
    while(r_speed>0) r_speed_aim=0;
    r_forward=1;
    delay_ms(100);
    }
  if(l_forward==0 && r_forward==0) {     // When going reverse before
    while(r_speed>0 && l_speed>0) { l_speed_aim=0; r_speed_aim=0; }
    l_forward=1; r_forward=1;
    delay_ms(100);
    }
  else { l_speed_aim=225; r_speed_aim=225; }
}

      

void turn_right(void) {
  if(l_forward==0 && r_forward==1) {      // When spining left before
    while(l_speed>0) l_speed_aim=0;
    l_forward=1;
    delay_ms(100);
    }
  else if(l_forward==1 && r_forward==0) { // When spining right before
    while(r_speed>0) r_speed_aim=0;
    r_forward=1;
    delay_ms(100);
    }
  if(l_forward==0 && r_forward==0) {     // When going reverse before
    while(r_speed>0 || l_speed>0) { l_speed_aim=0; r_speed_aim=0; }
    l_forward=1; r_forward=1;
    delay_ms(100);
    }
  else { l_speed_aim=200; r_speed_aim=50; }
}



void turn_left(void) 
{
  if(l_forward==0 && r_forward==1) {      // When spining left before
    while(l_speed>0 || r_speed>0) { l_speed_aim=0; r_speed_aim=0; }
    l_forward=1; r_forward=0;
    delay_ms(100);
    }
  else if(l_forward==1 && r_forward==0) { // When spining right before
    while(r_speed>0) r_speed_aim=0;
    r_forward=1;
    delay_ms(100);
    }
  if(l_forward==0 && r_forward==0) {     // When going reverse before
    while(r_speed>0 || l_speed>0) { l_speed_aim=0; r_speed_aim=0; }
    l_forward=1; r_forward=1;
    delay_ms(100);
    }
  else { l_speed_aim=50; r_speed_aim=200; }
}




void spin_right(void)
{
  if(l_forward==0 && r_forward==1) {      // When spining left before
    while(l_speed>0 || r_speed>0) { l_speed_aim=0; r_speed_aim=0; }
    l_forward=1; r_forward=0;
    delay_ms(100);
    }
  else if(l_forward==1 && r_forward==1) { // When going forward before
    while(r_speed>0) r_speed_aim=0;
    r_forward=0;
    delay_ms(100);
    }
  if(l_forward==0 && r_forward==0) {     // When going reverse before
    while(l_speed>0) l_speed_aim=0;
    l_forward=1; r_forward=0;
    delay_ms(100);
    }
  else { l_speed_aim=30; r_speed_aim=30; }
}



void spin_left(void)
{
  if(l_forward==1 && r_forward==0) {      // When spining right before
    while(l_speed>0 || r_speed>0) { l_speed_aim=0; r_speed_aim=0; }
    l_forward=0; r_forward=1;
    delay_ms(100);
    }
  else if(l_forward==1 && r_forward==1) { // When going forward before
    while(l_speed>0) l_speed_aim=0;
    l_forward=0;
    delay_ms(100);
    }
  if(l_forward==0 && r_forward==0) {     // When going reverse before
    while(r_speed>0) r_speed_aim=0;
    l_forward=0; r_forward=1;
    delay_ms(100);
    }
  else { l_speed_aim=30; r_speed_aim=30; }
}


void slow_down(void)
{
  if(l_forward==0 && r_forward==1) {      // When spining right before
    while(l_speed>0) l_speed_aim=0;
    l_forward=1;
    delay_ms(100);
    }
  else if(l_forward==1 && r_forward==0) { // When spining left before
    while(r_speed>0) r_speed_aim=0;
    r_forward=1;
    delay_ms(100);
    }
  if(l_forward==0 && r_forward==0) {     // When going reverse before
    while(r_speed>0 && l_speed>0) { l_speed_aim=0; r_speed_aim=0; }
    l_forward=1; r_forward=1;
    delay_ms(100);
    }
  else { l_speed_aim=0; r_speed_aim=0; }
  
  if(l_speed==0 && r_speed==0) {         // Stop!! (Timer interrupt disable)
    disable_interrupts(int_rtcc);
    disable_interrupts(int_timer1);
    }
}

void back_off(void)
{
  if(l_forward==0 && r_forward==1) {      // When spining right before
    while(r_speed>0) r_speed_aim=0;
    r_forward=0;
    delay_ms(100);
    }
  else if(l_forward==1 && r_forward==0) { // When spining left before
    while(l_speed>0) l_speed_aim=0;
    l_forward=0;
    delay_ms(100);
    }
  if(l_forward==1 && r_forward==1) {     // When going forward before
    while(r_speed>0 || l_speed>0) { l_speed_aim=0; r_speed_aim=0; }
    l_forward=0; r_forward=0;
    delay_ms(100);
    }
  else { l_speed_aim=125; r_speed_aim=125; }

}