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

news2024/9/23 13:22:44

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

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）

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

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

  }

}

（3）在待救援定位装置的控制板中烧录以下程序（human.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中，将从模块分别摆开，观察显示屏数据；

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

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

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

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

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/67463.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

深度解读汽车域控制器

深度解读汽车域控制器

已剪辑自: https://mp.weixin.qq.com/s?__bizMzg4NjIxODk4Mg&mid2247496089&idx1&sndb5c08f97342decfabc9ce985ec75aaa&chksmcf9fb810f8e83106994f2f2b9ca0387eaca7543d36b1673d4fc00bdfe07fbc5099322d41a702&scene21#wechat_redirect * * 过去十多年的…
阅读更多...
华为机试-字符串合并处理

华为机试-字符串合并处理

描述 按照指定规则对输入的字符串进行处理。 详细描述 第一步&#xff1a;将输入的两个字符串str1和str2进行前后合并。如给定字符串 “dec” 和字符串 “fab” &#xff0c; 合并后生成的字符串为 “decfab” 第二步&#xff1a;对合并后的字符串进行排序&#xff0c;要求…
阅读更多...
拖拽式网页制作工具

拖拽式网页制作工具

拖拽式网页制作工具是什么&#xff0c;有什么优势&#xff0c;怎么使用&#xff1f; 拖拽式网页制作工具是什么: 它是一款可以帮助企业、机构、个体户快速搭建网站的一款工具&#xff0c;通过简单易上手的操作&#xff0c;免除学习代码、学习设计等繁琐步骤&#xff0c;利用工…
阅读更多...
2022.12.2Treats for the Cows POJ - 3186(区间dp

2022.12.2Treats for the Cows POJ - 3186(区间dp

原题链接&#xff1a;传送门 FJ has purchased N (1 < N < 2000) yummy treats for the cows who get money for giving vast amounts of milk. FJ sells one treat per day and wants to maximize the money he receives over a given period time. The treats are inte…
阅读更多...
请不要直接拆除或更换旧光纤！Softing为您提供光纤以太网网络解决方案

请不要直接拆除或更换旧光纤！Softing为您提供光纤以太网网络解决方案

Softing的Phoenix Digital光纤以太网网络冗余模块与传统光纤相兼容。通过使用Phoenix Digital模块&#xff0c;用户无需更换传统光纤和远程I/O设备。 升级工业网络是一项复杂的工作&#xff0c;用户需要耗费大量的时间、成本和其他资源来确保新建网络可以满足系统应用中的所有要…
阅读更多...
Excel - 插入空白行

Excel - 插入空白行

简单的插入一个空白行&#xff0c;只需选中一行&#xff0c;右键&#xff0c;选择插入一行即可。 如果要一次插入多个空行&#xff0c;可以按住Ctrl键&#xff0c;然后逐个选中想要插入的行&#xff0c;然后执行插入操作&#xff0c;如下所示&#xff1a; 选中张三、王五、田七…
阅读更多...
二叉树的基本运算

二叉树的基本运算

二叉树的基本运算 上一讲我们已经讲了创建二叉树,所以这一讲,我们来说一下二叉树的基本运算方法,为以后二叉树的运用打下基础: (1)查找节点FindNode(*b,x): 在二叉树b中寻找data域值为x的节点,并返回指向该节点的指针 (2)找孩子节点LchildNode和Rchild-Node: 分别求二叉树中节…
阅读更多...
逆傅里叶变IFFT原始信号恢复方法研究-附Matlab代码

逆傅里叶变IFFT原始信号恢复方法研究-附Matlab代码

一、原始信号模拟 现实中&#xff0c;由于得到的信号都是实数序列。设有实数序列x(n)&#xff0c;如下图所示 图1 原始信号 对应Matlab如下&#xff1a; %% 矩形波 N33; % 设置N长 xzeros(1,N); % 构成矩形波形 x(7:27)1; figur…
阅读更多...
JavaSe-JAVA的多态

JavaSe-JAVA的多态

用一句话概括就是&#xff1a;事物在运行过程中存在不同的状态。先以教科书般举例说明,下文再举一个花木兰替父从军的例子帮助大家理解. 多态的存在有三个前提: 1.要有继承关系 2.子类要重写父类的方法 3.父类引用指向子类对, 但是其中又有很多细节需要注意。首先我们定义两…
阅读更多...
java代码审计

java代码审计

java代码审计 SQL注入分析 1、JDBC 1、1&#xff08;statement&#xff09; public String jdbcVul(String id) {StringBuilder result new StringBuilder();try {Class.forName("com.mysql.cj.jdbc.Driver");Connection conn DriverManager.getConnection(db_u…
阅读更多...
Qt OpenGL（二十六）——Qt OpenGL 核心模式-旋转的彩色三角形

Qt OpenGL（二十六）——Qt OpenGL 核心模式-旋转的彩色三角形

Qt OpenGL（二十六）——Qt OpenGL 核心模式-旋转的彩色三角形 上篇文章我们把三角形上色成了，彩色的三角形： 离我们想要绘制的图形，旋转的三角形，又近了一步，本篇文章就让这个彩色的三角形旋转起来。不过从上篇开始使用的代码就是Qt封装的类了，不过也是核心模式的，步…
阅读更多...
【Qt】从开源项目QCAD中学习如何增强QLineEdit控件

【Qt】从开源项目QCAD中学习如何增强QLineEdit控件

1 背景 Qt5 中有个很基本的单行输入框控件&#xff0c;就是 QLineEdit&#xff0c;类似于HTML中的input标签。 在QCAD开源项目中&#xff0c;其主界面中有个供用户输入绘图命令的单行输入框控件&#xff0c;其可以实现类似 Linux 终端的简单效果&#xff1a; 上键显示最近的…
阅读更多...
基于jsp+ssm的家政服务系统-计算机毕业设计

基于jsp+ssm的家政服务系统-计算机毕业设计

项目介绍 作为一个家政服务系统&#xff0c;它就面对着许多用户的操作&#xff0c;而这些用户对系统的操作应该有所不同&#xff0c;所以我们在设计时必须要对不同的用户设立不同的的权限在本系统中&#xff0c;我考虑了3种权限&#xff08;管理员&#xff0c;雇主&#xff0c…
阅读更多...
盘点JDK中基于CAS实现的原子类

盘点JDK中基于CAS实现的原子类

前言 JDK中提供了一系列的基于CAS实现的原子类&#xff0c;CAS 的全称是Compare-And-Swap&#xff0c;底层是lock cmpxchg指令&#xff0c;可以在单核和多核 CPU 下都能够保证比较交换的原子性。所以说&#xff0c;这些原子类都是线程安全的&#xff0c;而且是无锁并发&#x…
阅读更多...
国内设计师经常逛的5个素材网站

国内设计师经常逛的5个素材网站

设计师必备的5个设计素材网站&#xff0c;免费下载&#xff0c;还可商用&#xff0c;再也不用担心侵权了。1、菜鸟图库 https://www.sucai999.com/?vNTYwNDUx菜鸟图库是一个素材量非常丰富的网站&#xff0c;网站聚合了平面、UI、淘宝电商、高清背景图、图片、插画等高质量素材…
阅读更多...
六 游戏基础知识和SHAPE

六 游戏基础知识和SHAPE

显示对象 在Egret渲染架构设计中&#xff0c;我们将能够参与渲染的对象抽象为显示对象 Egret引擎中所有的显示对象类型表格&#xff1a; 坐标系 egret游戏中的的坐标原点位于舞台的左上角 显示列表与容器类 所有的容器全部继承于 DisplayObjectContainer类 DisplayObje…
阅读更多...
世界杯主题系列-用Scratch制作足球比赛小游戏，源码分享啦

世界杯主题系列-用Scratch制作足球比赛小游戏，源码分享啦

目录 一&#xff1a;两个小游戏的预览图效果图&#xff1a; 二&#xff1a;简单版双人足球赛源码&#xff1a; 三&#xff1a;复杂多人控制版足球赛源码 先来动态gif效果图&#xff1a; 四年一度的世界杯正在卡塔尔火热进行中&#xff01; 在绿茵场内&#xff0c;有胜利的…
阅读更多...
Reids实战——分布式锁优化（Lua脚本）

Reids实战——分布式锁优化（Lua脚本）

1 基于Redis分布式锁的问题 先来看看之前分布式锁的实现。 这个基于Redis的分布式锁仍然有着一个问题&#xff0c;那就是误删锁的问题。 简单的来说&#xff0c;就是当第一个线程&#xff0c;也就是线程1&#xff0c;拿到锁后&#xff0c;但由于本身业务复杂&#xff0c;而导…
阅读更多...
gvim写verilog环境搭建——将文本编辑器客制化定义为你自己的IDE

gvim写verilog环境搭建——将文本编辑器客制化定义为你自己的IDE

本文将详细描述将vim变成写Verilog代码的IDE客制化的实现过程&#xff0c;同时最后提供自己的环境。实现的过程中有使用自己写的&#xff0c;也有参考借鉴其他同学进行一定的修改&#xff0c;也有直接使用插件实现的相关功能。对应的功能实现部分均会给出相应的参考与插件地址等…
阅读更多...
基于jsp+ssm的网上图书商城-计算机毕业设计

基于jsp+ssm的网上图书商城-计算机毕业设计

项目介绍 本系统的用户群体分为管理员、会员两类。不同的身份拥有不同的职责和权限。管理员的职能包括&#xff1a;图书管理、会员管理、订单信息审核、个人信息维护等 该网上书城系统应该具备有图书添加、图书信息修改、用户浏览图书、用户搜索图书、图书购买和订购等功能&a…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023