Arduino通过信号强度值控制云台舵机

想做一个天线自动跟踪装置（跟踪无人机），有双舵机组成云台，上面搭载天线！（通信接收机问题不用考虑，无人机飞控不用考虑）
打算用接收信号强度来舵机控制，即Arduino从接收机获得天线接收的信号强度（RSSI)
每个一定周期（暂定2S）接受到RSSI，由Arduino控制舵机使天线始终对准飞行目标（动态跟踪），相当于步进跟踪，极大值跟踪。
目前，我能从接收机获得信号强度值，函数如下：
String comdata = "";  //用于接收通信接收机发来的信息，其中包括signal=-xx，为两位数信号
int numdata[6] = {
   0};
int mark = 0;
void setup()
{
   Serial.begin(9600);
   while (!Serial);
}

void loop()
{
   int j = 0;
   while (Serial.available() > 0)
   {
     comdata += char(Serial.read());
     delay(2);
     mark = 1;
   }
   if(mark == 1)
   {
     int sIndex = comdata.indexOf("signal=-");//对signal信息定位
    if(sIndex==-1){
       return;
     }
     String rssi=comdata.substring(sIndex+8,sIndex+10);
     Serial.println(rssi.toInt());
   }
   comdata = String("");
   mark = 0;Serial.flush();
}
接下来需要Arduino利用RSSI的值控制舵机，每接收到一个新的RSSI值与上一次的进行比较，控制舵机进行运动，保持跟踪最大值，算法怎么写啊，给点思路。

不知道适不适合你的应该,帮你找到一个,你看看先~~
http://wenku.baidu.com/link?url= ... eylYdPBtD9r-3YFWTuG