Baize_ServoDriver_esp32（ROS+Arduino驱动舵机机械臂，通过串口或WiFi话题通信）（数字孪生：虚拟和现实同步）

介绍

硬件平台

控制板：Baize_ServoDriver_esp32 

舵机机械臂：

通过Baize_ServoDriver_esp32这块舵机驱动板，我们来驱动我们的机器人。

首先，我们通过串口来订阅我们的自定义话题消息。

具体的话题消息以及定义方式，可以参照下面的仓库

https://github.com/Allen953/Leizhuo_UnderWaterHexapodRobot

然后我们就可以使用这个话题消息了，基于这个话题消息，我们用arduino来订阅这个消息，并显示在显示屏上，我发现当只显示一个joint_states数据时，屏幕的关节角度刷新速率正常，非常快，但是当角度多了之后，速度就变的很慢了，我猜是屏幕刷新速度跟不上，不过这个刷法好像也站不住脚，毕竟我用一个数字进行显示实验的时候，刷新速度是非常快的。所以还是比较疑惑。

代码 

1.订阅关节变量消息，并显示在显示屏上

/*
 * rosserial Publisher Example
 * Prints "hello world!"
 */
 

#include 
#include 
#include "Leizhuo_UnderWaterHexapodRobot/joint.h"


#include 
#include        // Hardware-specific library
TFT_eSPI TFT = TFT_eSPI();  // Invoke custom library


void chatterCallback(const std_msgs::String& msg) {
//  TFT.fillScreen(TFT_BLACK);
  TFT.setCursor(0, 30, 4);
  // Set the font colour to be white with a black background
  TFT.setTextColor(TFT_WHITE, TFT_BLACK);

  // We can now plot text on screen using the "print" class
  TFT.println(msg.data); 
}

int m = 0;

void chatterCallbackjoint(const Leizhuo_UnderWaterHexapodRobot::joint& hexapod_joint) {
  
  TFT.fillScreen(TFT_BLACK);
//  for(int i=0;i<2;i++)
//  {
//    for(int j=0;j<3;j++)
//    {
//      TFT.setCursor(j*70, i*20, 4);
//      // Set the font colour to be white with a black background
//      TFT.setTextColor(TFT_WHITE, TFT_BLACK);
//      // We can now plot text on screen using the "print" class
//      TFT.println(hexapod_joint.position[i*3+j]);
//    }
//  }
//      TFT.setCursor(0, 0, 4);
//      // Set the font colour to be white with a black background
//      TFT.setTextColor(TFT_WHITE, TFT_BLACK);
//      // We can now plot text on screen using the "print" class
//      TFT.println(hexapod_joint.position[0]);

      TFT.setCursor(80, 0, 4);
      // Set the font colour to be white with a black background
      TFT.setTextColor(TFT_WHITE, TFT_BLACK);
      // We can now plot text on screen using the "print" class
      TFT.println(hexapod_joint.position[1]);
}

ros::NodeHandle  nh;

std_msgs::String str_msg;
ros::Publisher chatter("gun", &str_msg);
ros::Subscriber subjoint("/hexapod_joint", chatterCallbackjoint);
ros::Subscriber sub("/chatter", chatterCallback);
char hello[13] = "gunnimade!";

void setup()
{
  nh.initNode();
  nh.advertise(chatter);
  nh.subscribe(sub);
  nh.subscribe(subjoint);
  
  TFT.init();
  TFT.setRotation(3);
  TFT.fillScreen(TFT_BLACK);
  TFT.initDMA();
    
  TFT.setCursor(0, 0, 4);
  // Set the font colour to be white with a black background
  TFT.setTextColor(TFT_WHITE, TFT_BLACK);

  // We can now plot text on screen using the "print" class
  TFT.println("Initialised default\n"); 
}

void loop()
{
//    str_msg.data = hello;
//    chatter.publish( &str_msg );

    nh.spinOnce();
//    time_s = millis();  

  delay(10);
}