如何用蓝牙实现无线定位（五）--双定位显示

1. 机器人定位装置的构建

按照上面的针脚使用杜邦线将救援机器人定位装置的主从蓝牙连接到主控板上，注意错误的连接会导致模块损坏【参考视频】

2. 机器人位置的获取与发送

(1) 在机器人定位装置的控制板中烧录以下程序（robot.ino）

/*------------------------------------------------------------------------------------

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

https://opensource.org/licenses/MIT

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

-------------------------------------------------------------------------------------*/

//救援机器人定位装置例程

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

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

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

//开始工作：

//第三步：将所有模块上电(主从模块按照上面介绍的连接方式连接即可);

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

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

// "D8A98B788750",

// "D8A98B788732",

// "380B3CFFC5B0"

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

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

#include <Arduino.h>

#include <Wire.h>

#include <MultiLCD.h>

#include <RssiPositionComputer.h>

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

#define DEBUG_SERIAL Serial //打印信息串口

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

#define SEND_SERIAL Serial2 //数据发送串口

#define CMD_CON "AT+CON"

#define CMD_DIS_CON "AT"

#define CMD_GET_RSSI "AT+RSSI?"

RssiPositionComputer myPositionComputer;

Point2D master_point; //基站数量

#define SLAVENUMBER 3 //基站地址

String BLUETOOTHADDRESS[3] = {

"D8A98B788750",

"D8A98B788732",

"380B3CFFC5B0"

};

//位置发送蓝牙地址

// F83002253178

String search_result_string[SLAVENUMBER] = {""};

String rssi[SLAVENUMBER] = {""};

float distance[SLAVENUMBER] = {};

void setup()

{

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.begin(9600);

#endif

CON_SERIAL.begin(9600);

SEND_SERIAL.begin(9600);

delay(1000);

init_ble();

}

void loop()

{

read_ble(BLUETOOTHADDRESS);

to_axis(distance, &master_point);

} //读取串口

String serial_read(int _len)

{

String data = "";

int len = 0;

unsigned long t = millis() + 1000;

while(1)

{

while(CON_SERIAL.available())

{

char c = CON_SERIAL.read();

data += c; len++;

}

if(len == _len)

{

break;

}

if(millis() > t)

break;

}

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.print("Serialread_data=");

DEBUG_SERIAL.println(data);

#endif

return data;

}

//初始化

void init_ble()

{

CON_SERIAL.print(CMD_DIS_CON);

delay(100);

serial_read(2);

}

//获取设备1 RSSI

void read_ble(String * address)

{

DEBUG_SERIAL.println("-----------------Start------------------");

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

{

CON_SERIAL.print(CMD_DIS_CON);

delay(500);

serial_read(2);

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

serial_read(8);

delay(800);

CON_SERIAL.print(CMD_GET_RSSI);

String rssi_str = serial_read(10);

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

//rssi

rssi[i] = _rssi;

//distance

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

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.println("BLE_" + String(i) + ": " + rssi[i]);

DEBUG_SERIAL.println("BLE_" + String(i) + ": " + distance[i]);

#endif

//delay(800);

}

DEBUG_SERIAL.println("------------------End------------------");

DEBUG_SERIAL.println();

}

//计算距离

float rssiToDistance(float rssi)

{

float dis = 0;

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

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

return dis;

}

//转换为2d坐标x,y

void to_axis(float * dis, Point2D* actual_master_point)

{

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

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

int x = master_point.x*100;

int y = master_point.y*100;

char point[100];

sprintf(point, "[bx:%3d,by:%3d]\n",abs(x),abs(y));

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.println(point);

#endif

SEND_SERIAL.print(point);

}

（2）在中控台的主控板中烧录以下程序（master.ino）

/*------------------------------------------------------------------------------------

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

https://opensource.org/licenses/MIT

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

-------------------------------------------------------------------------------------*/

//救援指挥中心例程

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

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

//开始工作：

//第三步：将所有模块上电(主模块参考控制中心的连接进行接线，待救援设备从模块参考相应的连接);

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

#include <Arduino.h>

#include <Wire.h>

#include <MultiLCD.h>

#define DATA_SERIAL Serial //信息打印串口

#define HUMAN_SERIAL Serial1 //待救援数据

#define ROBOT_SERIAL Serial2 //救援机器人数据

#define CMD_CON "AT+CON"

#define CMD_DIS_CON "AT"

#define CMD_START "AT+START"

LCD_SSD1306 lcd;

String human_address = "F83002253650"; //待救援设备地址

String robot_address = "F83002253178"; //救援机器人设备地址

String human_point = "[ax: 0 , ay: 0]";

String robot_point = "[bx: 0 , by: 0]";

void setup() {

DATA_SERIAL.begin(9600);

HUMAN_SERIAL.begin(9600);

ROBOT_SERIAL.begin(9600);

lcd.begin();

master_init();

}

void loop() {

read_human_point();

read_robot_point();

point_display();

}

void connect_ble(){

//连接待救援设备地址

// HUMAN_SERIAL.print(CMD_START);delay(1000);

HUMAN_SERIAL.print(CMD_DIS_CON);delay(200);

HUMAN_SERIAL.print(CMD_CON + human_address);

//连接救援机器人设备地址

ROBOT_SERIAL.print(CMD_DIS_CON);delay(200);

ROBOT_SERIAL.print(CMD_CON + robot_address);

}

//读取待救援位置

void read_human_point(){

while(1){

String str;

while(HUMAN_SERIAL.available() > 0){

str = HUMAN_SERIAL.readStringUntil('\n');

str.trim();

}

if(!str.equals("")){

Serial.println(str);

human_point = str;

break;

}

//读取救援机器人位置

void read_robot_point(){

while(1){

String str;

while(ROBOT_SERIAL.available() > 0){

str = ROBOT_SERIAL.readStringUntil('\n');

str.trim();

}

if(!str.equals("")){

Serial.println(str);

robot_point = str;

break;

}

void point_display(){

lcd.clear();

lcd.setCursor(10, 1);

lcd.print("a: human b: robot");

lcd.setCursor(20, 3);

lcd.print(human_point);

lcd.setCursor(20, 5);

lcd.print(robot_point);

}

void master_init(){

lcd.clear();

lcd.setCursor(25, 3);

lcd.print("Hello World!");

delay(1000);

point_display();

connect_ble();

while(true){

if(HUMAN_SERIAL.available() > 0 || ROBOT_SERIAL.available() > 0)

break;

}

（3）在待救援定位装置的控制板中烧录以下程序（human.ino）

/*------------------------------------------------------------------------------------

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

https://opensource.org/licenses/MIT

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

-------------------------------------------------------------------------------------*/

//待救援主控例程

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

//开始工作：

//第三步：将所有模块上电(主从模块按照上面介绍的连接方式连接即可);

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

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

// "7CEC79613E1B",

// "20C38FF2BAA9",

// "7C669D9B281B"

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

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

#include <Arduino.h>

#include <Wire.h>

#include <MultiLCD.h>

#include <RssiPositionComputer.h>

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

//#define DEBUG_SERIAL Serial //打印信息串口

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

#define SEND_SERIAL Serial2 //数据发送串口

#define CMD_CON "AT+CON"

#define CMD_DIS_CON "AT"

#define CMD_GET_RSSI "AT+RSSI?"

RssiPositionComputer myPositionComputer;

Point2D master_point;

//基站数量

#define SLAVENUMBER 3

//基站地址

String BLUETOOTHADDRESS[3] = {

"D8A98B788750",

"D8A98B788732",

"380B3CFFC5B0"

};

//位置发送蓝牙地址

// 20CD397F05B7

String search_result_string[SLAVENUMBER] = {""};

String rssi[SLAVENUMBER] = {""};

float distance[SLAVENUMBER] = {};

void setup() {

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.begin(9600);

#endif

CON_SERIAL.begin(9600);

SEND_SERIAL.begin(9600);

delay(1000);

init_ble();

}

void loop() {

read_ble(BLUETOOTHADDRESS);

to_axis(distance, &master_point);

}

//读取串口

String serial_read(int _len){

String data = "";

int len = 0;

unsigned long t = millis() + 500;

while(1)

{

while(CON_SERIAL.available()){

char c = CON_SERIAL.read();

data += c;

len++;

}

if(len == _len){

break;

}

if(millis() > t)

break;

}

#if defined(DEBUG_SERIAL)

//DEBUG_SERIAL.println(data);

#endif

return data;

}

//初始化

void init_ble(){

CON_SERIAL.print(CMD_DIS_CON);delay(100);

serial_read(2);

}

//获取设备 RSSI

void read_ble(String * address){

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

CON_SERIAL.print(CMD_DIS_CON);delay(100);

serial_read(2);

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

serial_read(8);

delay(500);

CON_SERIAL.print(CMD_GET_RSSI);

String rssi_str = serial_read(10);

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

//rssi

rssi[i] = _rssi;

//distance

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

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.println("BLE_" + String(i) + ": " + rssi[i]);

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

#endif

//delay(800);

}

//计算距离

float rssiToDistance(float rssi){

float dis = 0;

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

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

return dis;

}

//转换为2d坐标x,y

void to_axis(float * dis, Point2D* actual_master_point){

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

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

int x = master_point.x*100;

int y = master_point.y*100;

char point[100];

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

#if defined(DEBUG_SERIAL)

DEBUG_SERIAL.println(point);

#endif

SEND_SERIAL.print(point);

}

（4）把定位装置安装在救援机器人身上

（5）依次开启信号塔、待救援定位装置、机器人定位装置、中控台的电源，中控台的OLED屏幕上即可同时显示待救援者和机器人的坐标值（注：中控台的电源必须最后打开）。

（6）控制救援机器人移动，可看到机器人的定位坐标随着它的移动而变化。

接下来就可以控制机器人向待救援目标前进了。如果再给救援机器人增加一套WiFi视频遥控的功能（请参考斜三角履带机械臂小车），就可以构建出一套完整的机器人救援场景模型了。

3. 资料内容

① 双目标显示-例程

② 蓝牙配置说明.txt

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

---------------------------------------------------完结---------------------------------------------------