마이크로프로세서 HOT초음파 센서를 이용한 서보모터 컨트롤에 관해서

안녕하세요 반갑습니다.

도무지 방도가 떠오르지 않아 질문을 드립니다.

초음파센서값을 이용해서 다중서보모터를 제어할려고 합니다.

좀 더 간단하게 말을 드리면 초음파센서가 앞에 있는 장애물을 인식하면 서보모터를 제어해서 회피하는 것입니다.

distance = (unsigned)cn_data * Cvelocity / 1000; 
  if(distance<5.0) PORTA=0xf0;
  else if(distance<10.0) PORTA=0x0f;
  else {}
사용중인 소스코드의 일부입니다. distance변수값에 따라서 포트A에 연결된 LED는 작동을 잘합니다.

문제는... "어떻게 해야 다중서보를 제어하는 함수을 실행할 수 있는건가"입니다 ㅠㅠ;;
main()
{
while(1){
move_control()
{
Servo24SetDuty1(2,0,1800);
Servo24SetDuty1(2,1,1200); 
Servo24SetDuty1(2,4,2000);//꼬리
Servo24SetDuty1(1,0,1500);//Base Value
Servo24SetDuty1(1,1,1380);//Base Value front -300
Servo24SetDuty1(1,2,1950);//Base Value
Servo24SetDuty1(1,3,1680);//Base Value
} 
}