蓝牙rssi定位-5双定位显示

双定位显示 1. 机器人定位装置的构建按照上面的针脚使用杜邦线将救援机器人定位装置的主从蓝牙连接到主控板上，注意错误的连接会导致模块损坏。参考视频（注：此视频链接仅可在手机微信中打开） 2. 机器人位置的获取与发送 (1)在机器人定位装置的控制板中烧录以下程序（robot.ino） /*------------------------------------------------------------------------------------ 版权说明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved. 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?...

继续阅读完整内容

支持我们的网站，请点击查看下方广告

双定位显示

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


{
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


    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);
}