C++ source for servo motor with PIC 16F877A

int servo1,servo2,servo3;
void servo_1(int servo1);
void servo_2(int servo2);
void servo_3(int servo3);
void initial(void);
void check(void);
void move1(void);
void move2(void);
void stop(void);
void interrupt(void);
void main()
{
 int i,adc;
 ADCON1=0x8e;
 trisb=0;        // PORTB IS OUTPUT
 trisd=0xff;        // PORTD IS INPUT
 tmr0h=0xfc;
 tmr0l=0xee;
 intcon=0xa0;
 t0con=0x87;
 portb=0x00;
 portd=0x00;

 while(1)
 {
 initial:
 servo1=500;             // HOME POSITION
 servo2=500;
 servo3=500;
 delay_ms(5000);
//------checking sensor--------------
 check:
 if (Button(&portd,3,0,1))               // SENSOR1 PORT
    {goto move1;}
    else if (Button(&portd,4,0,1))           // CHECK SENSOR 2
        { if (Button(&portd,5,0,1))
          {goto move2;}
        }
     else
         { goto check;}
 }
//---------move1-------------
move1:
 for(i=0;i<133;i++)
 {
  servo1--;                // SERVO 1, AND 2 ROTATE 20 DEGREES CLOCKWISE
  servo2--;
  servo3++;                // SERVO 3 ROTATE 20 DEGREESS CLOCKWISE
  if (Button(&portd,5,0,1))
    { goto stop; }          // IF GRIPPER SENSOR HIGH, GO TO STOP
  else {}
  delay_ms(10);
  }
 for(i=0;i<67;i++)
 {
  servo1--;                 // SERVO 1 ROTATE 10 DEGREES CLOCKWISE
  servo3++;                 // SERVO 3 ROTATE 10 DEGREES COUNTERCLOCKWISE
  if (Button(&portd,5,0,1))
    { goto stop; }           // IF GRIPPER SENSOR  HIGH, GO TO STOP
  else {}
  delay_ms(10);
  }
 for(i=0;i<267;i++)
 {
  servo1--;                  // SERVO 1 ROTATE 40 DEGREES CLOCKWISE
  if (Button(&portd,5,0,1))
    { goto stop; }           // IF GRIPPER SENSOR  HIGH, GO TO STOP
  else {}
  delay_ms(10);
  }
goto stop;

//---------move2------------------------------
move2:
 for(i=0;i<133;i++)
 {
  servo1++;                // SERVO 1 ROTATE 20 DEGREES COUNTERCLOCKWISE
  servo2--;                // SERVO 2 AND 3 ROTATE 20 DEGREES CLOCKWISE
  servo3++;
  if (Button(&portd,5,0,1))   // IF GRIPPER SENSOR  HIGH, GO TO STOP
    { goto stop; }
  else {}
  delay_ms(10);
  }
 for(i=0;i<67;i++)
 {
  servo1++;                // SERVO 1 ROTATE 10 DEGREES COUNTERCLOCKWISE
  servo3++;                // SERVO 3 ROTATE 10 DEGREES CLOCKWISE
  if (Button(&portd,5,0,1))
    { goto stop; }          // IF GRIPPER SENSOR  HIGH, GO TO STOP
  else {}
  delay_ms(10);
  }
 for(i=0;i<267;i++)
 {
  servo1++;                // SERVO 1 ROTATE 40 DEGREES COUNTERCLOCKWISE
  if (Button(&portd,5,0,1))
    { goto stop; }          // IF GRIPPER SENSOR HIGH, GO TO STOP
  else {}
  delay_ms(10);
  }
goto stop;

stop:     // STOP ALL SERVO MOTORS
delay_ms(5000);
goto initial;
}
//-------interrupt-----------
void interrupt()             // FUNCTION TO GENERATE PWM
{
 if(intcon.tmr0if)
 {
  tmr0h=0xfc;
  tmr0l=0xee;
  intcon=0x20;
  servo_1(servo1);
  portb.f4=0;
  servo_2(servo2);
  portb.f3=0;
  servo_3(servo3);
  portb.f2=0;
  }
}

void servo_1(int servo1)
{
 while(servo1>0)
 {
  portb.f4=1;
  servo1--;
  }
}

void servo_2(int servo2)
{
 while(servo2>0)
 {
  portb.f3=1;
  servo2--;
  }
}

void servo_3(int servo3)
{
 while(servo3>0)
 {
  portb.f2=1;
  servo3--;
  }
}