如何用蓝牙实现无线定位（三）--本地定位显示

1. 被定位目标

本项目设计有两个定位装置，一个用于固定目标，一个用于可移动设备。在定位系统的帮助下，我们可以操作可移动设备向固定目标移动。假设这是一个救援场景的话，我们就可以把固定的目标看作等待救援的SEO靠我人或物，把可移动的设备看作前来救援的机器人。

2. 构建待救援者的定位设备

定位设备主要由1个Arduino Mega2560控制板、1个Bigfish扩展板、2个BLE4.0模块构成。在本实验中，我们为SEO靠我目标安装1个OLED显示屏，以便在调试时查看坐标值。

两个BLE4.0模块中的一个用于接收3个信号塔的信号（主），在控制板中计算后生成坐标信息。坐标信息可以在OLED显示屏上显示出来，并可以通过另一个BSEO靠我LE4.0模块发送出去（从）。从BLE4.0模块在本功能中暂时用不上，后续的远程定位功能将会用到。

       按照上面的针脚使用杜邦线将待救援设备的主蓝牙和从蓝牙连接到主控板上，注意错误的连接会导致模块损坏。

3.SEO靠我 目标的BLE4.0a蓝牙模块配置。

       目标的主蓝牙需要接收三个信号发射塔的信号，从而通过三点定位的算法计算出自己当前的位置信息，并发送显示。

使用下面的命令，配置目标的主蓝牙：

AT+RENEW    //SEO靠我恢复默认设置

AT -- OK//测试模块正常

AT+ADDR? -- MAC//查询模块MAC地址

AT+BAUD0 -- 9600//设置波特率为9600

AT+CLEAR -- OK//清除设备配对信息SEO靠我

AT+IMME1 -- OK    //设置模块工作类型：上电等待触发

AT+ROLE1 -- OK        //设置主从模式：主设备

AT+MODE1 -- OK//设置模块工作模式：远控模式

各设SEO靠我备主从蓝牙的MAC地址（以下地址可根据自己的BLE4.0a蓝牙模块的实际MAC地址进行修改，详细参考：蓝牙配置说明.txt）

信号塔MAC地址：

0：D8A98B788750 （从）

1：D8A98B788SEO靠我732 （从）

2：380B3CFFC5B0 （从）

目标1设备地址：

B：F83002253178 （从）

主：D8A98B788758 （主）

4. 坐标位置的本地显示

将OLED模块按如图所示的方法连接在BSEO靠我igfish扩展板上

 按照上面的针脚使用杜邦线将OLED显示屏连接到主控板上，注意错误的连接会导致模块损坏。

5. 位置的获取与发送

烧录以下程序（human_rssi.ino），移动目标的位置，可观察到OSEO靠我LED模块显示坐标值的变化。

注意：该例程仅作调试使用，后期构建完整系统时会使用新的程序。

/*------------------------------------------------------SEO靠我------------------------------

版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All RightsSEO靠我 Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

https://opensource.orgSEO靠我/licenses/MIT

           by 机器谱 2022-5-30 https://www.robotway.com/

---------------------------------------------SEO靠我----------------------------------------*/

//蓝牙4.0无线连接定位实验：待救援主控接收例程

//配置模块：(模块在配置时与正常工作时TX、RX线序不同，请注意SEO靠我)

//第一步：使用AT指令将所使用的四个模块其中一个设置为主模式，其余三个为从模式；所有模块波特率全部为默认的9600，并记录三个从模块的地址(AT+ADDR?);

//第二步：将主模块在未连接时清除之SEO靠我前的配对信息(AT+CLEAR),再设置其工作类型为类型1(AT+IMME即上后处于等待状态，收到AT+START,AT+DISC,AT+CONNL等指令后开始工作）；

//开始工作：

//第三步：将所有SEO靠我模块上电(主模块使用杜邦线接在MEGA2560上，从模块只给两个电源引脚提供3.3或5V电源即可);

//第四步：将本程序下载进MEGA2560中，将从模块分别摆开，观察显示屏数据；

//按救援机器人的场SEO靠我地在信号塔位置放置3个从模块，主模块放置在中间区域；

//本实验使用的3个从模块地址分别为：

/*********从设备地址********/

// "D8A98B788750",

// "D8A98B788SEO靠我732",

// "380B3CFFC5B0"

/*********从设备地址********/

/**********头文件***************/

#include <Arduino.h>

#incSEO靠我lude <Wire.h>

#include <MultiLCD.h>

#include <RssiPositionComputer.h>

/***********宏定义**************/

#deSEO靠我fine DEBUG_SERIAL Serial //打印信息串口

#define CON_SERIAL Serial1 //蓝牙通信串口

#define SEND_SERIAL Serial2 //数据SEO靠我发送串口

#define CMD_CON "AT+CON"

#define CMD_DIS_CON "AT"

#define CMD_GET_RSSI "AT+RSSI?"

LCD_SSD1306 lcd;

RSEO靠我ssiPositionComputer myPositionComputer;

Point2D master_point;

//基站数量

#define SLAVENUMBER 3

//基站地址

String SEO靠我BLUETOOTHADDRESS[3] = {

"D8A98B788750",

"D8A98B788732",

"380B3CFFC5B0"

};

//位置发送蓝牙地址

// D8A98B788758

StringSEO靠我 search_result_string[SLAVENUMBER] = {""};

String rssi[SLAVENUMBER] = {""};

float distance[SLAVENUMBERSEO靠我] = {};

void setup()

{

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.begin(9600);

#endif

CON_SERIAL.begin(9600);

SSEO靠我END_SERIAL.begin(9600);

delay(1000);

lcd.begin();

lcd.clear();

lcd.setCursor(25, 3);

lcd.print("Hello WorSEO靠我ld!");

delay(1000);

init_ble();

}

void loop()

{

read_ble(BLUETOOTHADDRESS);

to_axis(distance, &master_pointSEO靠我);

}

//读取串口

String serial_read(int _len)

{

String data = "";

int len = 0;

unsigned long t = millis() + 500;SEO靠我

while(1)

{

while(CON_SERIAL.available())

{

char c = CON_SERIAL.read();

data += c; len++;

}

if(len == _len)

{SEO靠我

Serial.println("the len");

Serial.println(data);

break;

}

if(millis() > t)

{

Serial.println("the t");

SeriaSEO靠我l.println(data);

break;

}

}

#if defined(DEBUG_SERIAL)

//DEBUG_SERIAL.println(data);

#endif

return data;

}

//初SEO靠我始化

void init_ble()

{

CON_SERIAL.print(CMD_DIS_CON);

delay(100);

serial_read(2);

}

//获取设备 RSSI

void read_ble(SEO靠我String * address)

{

for(int i=0;i<SLAVENUMBER;i++)

{

CON_SERIAL.print(CMD_DIS_CON);

delay(100);

serial_reaSEO靠我d(2);

CON_SERIAL.print(CMD_CON + address[i]);

serial_read(8);

delay(500);

CON_SERIAL.print(CMD_GET_RSSI)SEO靠我;

String rssi_str = serial_read(10);

String _rssi = rssi_str.substring(7, rssi_str.length());

//rssi

rssSEO靠我i[i] = _rssi;

//distance

distance[i] = rssiToDistance(rssi[i].toFloat());

#if defined(DEBUG_SERIAL)

DEBUSEO靠我G_SERIAL.println("BLE_" + String(i) + ": " + rssi[i]);

//DEBUG_SERIAL.println("BLE_" + String(i) + ":SEO靠我 " + distance[i]);

#endif

//delay(800);

}

}

//计算距离

float rssiToDistance(float rssi)

{

float dis = 0;

//dis = SEO靠我pow(10.0,((abs(rssi)-56)/10.0/1.05));

dis = pow(10.0,((abs(rssi)-56)/5.0/1.65));

return dis;

}

//转换为2d坐标SEO靠我x,y

void to_axis(float * dis, Point2D* actual_master_point)

{

//myPositionComputer.distanceToPoint(*disSEO靠我,*(dis+1),*(dis+2),actual_master_point);

myPositionComputer.distanceToPoint(*dis,*(dis+1),random(0,77SEO靠我),actual_master_point);

int x = master_point.x*100;

int y = master_point.y*100;

char point[100];

sprintfSEO靠我(point, "[ax:%3d,ay:%3d]\n",abs(x),abs(y));

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.println(point);

lcd.SEO靠我clear();

lcd.setCursor(10, 1);

lcd.print("a: human");

lcd.setCursor(25, 5);

lcd.print(point);

#endif

SEND_SEO靠我SERIAL.print(point);

}

6. 待救援定位装置的摆放

使用下图场地进行构建，在信号塔的位置分别放置三个信号发射塔，将待救援定位装置尽量放置在三个信号塔连线构成的三角形内。依次开启3个信号SEO靠我塔、目标的电源，观察目标的OLED屏幕上显示的坐标位置值（注：目标的电源必须最后打开）。

定位效果如下图所示：

7. 资料内容

① 函数库

② 目标定位设备调试例程

③ 蓝牙配置说明.txt 

资料内容下载请参考如SEO靠我何用蓝牙实现无线定位

---------------------------------------------------未完待续-----------------------------------SEO靠我----------------