Phase 5: PIC PROGRAM

This program will help to control the DC motors using the inputs from the BeagleBone Black.
Compiler : PICC
uC: PIC16f877a

#define FB_GRIP_C pin_A0

#define FB_GRIP_O pin_A1

#define FB_ARM_UP pin_A2

#define FB_ARM_DN pin_A3

#define L0 pin_B7

#define L1 pin_B6

#define L2 pin_B5

#define L3 pin_B4

#define ARM_0 pin_D2

#define ARM_1 pin_D3

#define GRIP_0 pin_C4

#define GRIP_1 pin_C5

#define LMOTOR_0 pin_C6

#define LMOTOR_1 pin_C7

#define RMOTOR_0 pin_D4

#define RMOTOR_1 pin_D5

void arm_up(int i)

{

   while (input(FB_ARM_UP)==0)

   {

      output_high(ARM_0);

      output_low(ARM_1);

      delay_ms(500);

      output_low(ARM_0);

      output_low(ARM_1);

      //delay_ms(500);

   }

  return;

}

void arm_dn(int i)

{

   while (input(FB_ARM_DN)==0)

   {

      output_low(ARM_0);

      output_high(ARM_1);

      delay_ms(500);

      output_low(ARM_0);

      output_low(ARM_1);

      //delay_ms(500);

   }

  return;

}

void grip_o(int i)

{  i=1;

   while (i<=7)

   {

      output_high(GRIP_0);

      output_low(GRIP_1);

      delay_ms(500);

      output_low(GRIP_0);

      output_low(GRIP_1);

      //delay_ms(500);

      i++;

   }

  return;

}

void grip_c(int i)

{  i=1;

   while (i<=7)

   {

      output_low(GRIP_0);

      output_high(GRIP_1);

      delay_ms(500);

      output_low(GRIP_0);

      output_low(GRIP_1);

      //delay_ms(500);

      i++;

   }

  return;

}

void move_fd(int i)

{

   output_low(LMOTOR_0);

   output_high(LMOTOR_1);

   output_high(RMOTOR_0);

   output_low(RMOTOR_1);

   delay_ms(200);

   output_low(LMOTOR_0);

   output_low(LMOTOR_1);

   output_low(RMOTOR_0);

   output_low(RMOTOR_1);

   delay_ms(300);

}

void move_bd(int i)

{

   output_high(LMOTOR_0);

   output_low(LMOTOR_1);

   output_low(RMOTOR_0);

   output_high(RMOTOR_1);

   delay_ms(200);

   output_low(LMOTOR_0);

   output_low(LMOTOR_1);

   output_low(RMOTOR_0);

   output_low(RMOTOR_1);

   delay_ms(300);

}

void move_rt(int i)

{

   output_low(LMOTOR_0);

   output_high(LMOTOR_1);

   output_low(RMOTOR_0);

   output_high(RMOTOR_1);

   delay_ms(100);

   output_low(LMOTOR_0);

   output_low(LMOTOR_1);

   output_low(RMOTOR_0);

   output_low(RMOTOR_1);

   delay_ms(300);

}

void move_lt(int i)

{

   output_high(LMOTOR_0);

   output_low(LMOTOR_1);

   output_high(RMOTOR_0);

   output_low(RMOTOR_1);

   delay_ms(100);

   output_low(LMOTOR_0);

   output_low(lMOTOR_1);

   output_low(RMOTOR_0);

   output_low(RMOTOR_1);

   delay_ms(300);

}

void tilt_rt(int i)

{

   output_low(LMOTOR_0);

   output_high(LMOTOR_1);

   output_low(RMOTOR_0);

   output_high(RMOTOR_1);

   delay_ms(100);

   output_low(LMOTOR_0);

   output_low(lMOTOR_1);

   output_low(RMOTOR_0);

   output_low(RMOTOR_1);

   delay_ms(300);

}

void tilt_lt(int i)

{

   output_high(LMOTOR_0);

   output_low(LMOTOR_1);

   output_high(RMOTOR_0);

   output_low(RMOTOR_1);

   delay_ms(100);

   output_low(LMOTOR_0);

   output_low(LMOTOR_1);

   output_low(RMOTOR_0);

   output_low(RMOTOR_1);

   delay_ms(300);

}

void main()

{

  

   setup_adc_ports(NO_ANALOGS);

   setup_adc(ADC_OFF);

   setup_psp(PSP_DISABLED);

   setup_spi(SPI_SS_DISABLED);

   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);

   setup_timer_1(T1_DISABLED);

   setup_timer_2(T2_DISABLED,0,1);

   setup_comparator(NC_NC_NC_NC);

   setup_vref(FALSE);

  

   output_c(0x00);

   output_d(0x00);

   delay_ms(1000);

   output_c(0xFF);

   output_d(0xFF);

   delay_ms(10000);

   output_c(0x00);

   output_d(0x00);

   delay_ms(1000);

   arm_up(1);

   grip_o(1);

while(1)

   {

   if (input(L0)==1 && input(L1)==1 &&  input(L2)==1)

      {

      arm_dn(1);

      grip_c(1);

      arm_up(1);

      grip_o(1);

      }

     

   else if (input(L0)==1 && input(L1)==1 &&  input(L2)==0)

      {

      tilt_lt(1);

      }

     

   else if (input(L0)==0 && input(L1)==1 &&  input(L2)==1)

      {

      tilt_rt(1);

      }

     

   else if (input(L0)==1 && input(L1)==0 &&  input(L2)==0)

      {

      move_lt(1);

      }

     

   else if (input(L0)==0 && input(L1)==0 &&  input(L2)==1)

      {

      move_rt(1);

      }

     

   else if (input(L0)==1 && input(L1)==0 &&  input(L2)==1)

      {

      move_bd(1);

      }

     

   else if (input(L0)==0 && input(L1)==1 &&  input(L2)==0)

      {

      move_fd(1);

      }

     

   else if (input(L0)==0 && input(L1)==0 &&  input(L2)==0)

      {

      move_rt(1);

      delay_ms(1000);

      }

   }

}