【mbot试用体验】根据蓝牙RSSI设计的mbot跟随

想了很久，看了一阵子代码，如果要做到蓝牙跟随，是不用修改mbot上面的代码的。我只要控制好mbot的移动就好了。在mbot的头文件中，定义了指令的代码，
比如：
#define IR_BUTTON_POWER     0x45
#define IR_BUTTON_A         0x45
#define IR_BUTTON_B         0x46
#define IR_BUTTON_MENU         0x47
。。。。。
所以总体上来说，我只要写一个安卓端的代码，就可以通过蓝牙控制小车了。还是像之前那样，通过RSSI，去得到相对位置，强度分为【1:128】，如果我这里将强度80作为一个限值，当小车和手机之间的RSSI大于这个值得时候，不控制小车移动，如果RSSI小于这个值得时候，控制小车进行试探性移动，如果移动后，RSSI值继续减小，控制小车向相反方向移动。

在上图中，如果手机在圆心位置， 小车在黑点处，如果只是简单的左右移动，RSSI都是减小。所以，在移动之前，会判断，手机是否有移动，如果没有，采用移动三个点，以确定mbot与手机的相对位置，从而可以朝着手机的方向移动。
下面是我安卓得到信号强度的代码：
private final BroadcastReceiver mReceiver = new BroadcastReceiver() {
        @Override
        public void onReceive(Context context, Intent intent) {
            Log.d("onReceive","OK");
            String action = intent.getAction();
            // When discovery finds a device
            if (BluetoothDevice.ACTION_FOUND.equals(action)) {
                // Get the BluetoothDevice object from the Intent
                BluetoothDevice device = intent.getParcelableExtra(BluetoothDevice.EXTRA_DEVICE);
                if(device.getName().equals("HC-06")==true){
                    int i;
                    short rssi = intent.getExtras().getShort(BluetoothDevice.EXTRA_RSSI);
                    for(i=0;i                         if(mDevicesVector.get(i).equals(device.getAddress()))break;
                    }
                    if(i==mDevicesVector.size()){
                        mDevicesVector.add(device.getAddress());
                    }
                    mRSSIVector[mRSSINum++]=rssi;
                    Log.d("RSSI",device.getName()+"  "+String.valueOf(rssi));  
                }              
            // When discovery is finished, change the Activity title
            } else if (BluetoothAdapter.ACTION_DISCOVERY_FINISHED.equals(action)) {
                setProgressBarIndeterminateVisibility(false);
                if (mDevicesVector.size() != 0) {
                    Message msg = new Message();//消息
                    Bundle bundle = new Bundle();
                    bundle.clear();Log.d("onReceive","1");
                    msg.what = 0x01;//消息类别
                    bundle.putShort("msg",(short) 0);Log.d("onReceive","2");
                    msg.setData(bundle);Log.d("onReceive","3");
                    myHandler.sendMessage(msg);Log.d("onReceive","4");
                }
            }
        }
    };
安卓的代码，我自己也是第一次写，弄起来很头疼，有几个地方出错，依然在调试。希望有做过类似项目的大神，可以不吝赐教。