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

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

  版权说明：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?"

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

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

  版权说明：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中，将从模块分别摆开，观察显示屏数据；

#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;

  }

}

  版权说明：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中，将从模块分别摆开，观察显示屏数据；

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

//   "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

序号

双目标显示-例程