设为首页收藏本站
首页 > 论坛 > ESP32

[ESP8266/ESP32]FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇

纪泓丞 | 学徒 | 创造力： | 帖子： | 发消息 | 串个门 | 加好友 | 打招呼
2023-9-6 21:47:32 [显示全部楼层]
1437浏览
0回复
1收藏
查看: 1437|回复: 0

[ESP8266/ESP32] FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇

[复制链接]

接上文，为了设计一个工业机器人无线控制器，硬件部分已经准备完毕。接下来是软件部分。整体思路是使用FireBeetle 2 ESP32-S3作为主控，采集MPU6050陀螺仪数据，然后通过websocket的通讯方式，将采集的数据发送给PC上位机。上位机再将数据分发给不同类型机器人，控制其运动。同时FireBeetle 2 ESP32-S3再将采集的数据，通过IIC的屏幕，显示在小屏幕上。
系统框架图如下：
   FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇图1
首先，设计采集MPU6050传感器数据的模块和屏幕显示模块。使用Adafruit_MPU6050库和Adafruit_SSD1306库，代码如下：
#include <Adafruit_MPU6050.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_Sensor.h>
Adafruit_MPU6050 mpu;
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 32, &Wire);
int X;
int Y;
int Z;
int Rx;
int Ry;
int Rz;
int tempAcc = 5;
int tempAng = 60;
void setup() {
  Serial.begin(115200);
  // while (!Serial);
  Serial.println("MPU6050 OLED demo");
  if (!mpu.begin()) {
    Serial.println("Sensor init failed");
    while (1)
      yield();
  }
  Serial.println("Found a MPU-6050 sensor");
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {  // Address 0x3C for 128x32
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;  // Don't proceed, loop forever
  }
  display.display();
  delay(500);  // Pause for 2 seconds
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setRotation(0);
}
void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  display.clearDisplay();
  display.setCursor(0, 0);
  Serial.print("Accelerometer ");
  Serial.print("X: ");
  X = a.acceleration.x;
  if (abs(X) >= tempAcc) {
    X = a.acceleration.x;
  } else {
    X = 0;
  }
  Serial.print(X);
  Serial.print("  ");
  Serial.print("Y: ");
  Y = a.acceleration.y;
  if (abs(Y) >= tempAcc) {
    Y = a.acceleration.y;
  } else {
    Y = 0;
  }
  Serial.print(Y);
  Serial.print("  ");
  Serial.print("Z: ");
  Z = a.acceleration.z - 9;
  if (abs(Z) >= tempAcc) {
    Z = a.acceleration.z - 9;
  } else {
    Z = 0;
  }
  Serial.print(Z);
  Serial.println(" m/s^2");
  display.println("Accelerometer - m/s^2");
  display.print("X:");
  display.print(X);
  display.print(", ");
  display.print("Y:");
  display.print(Y);
  display.print(", ");
  display.print("Z:");
  display.print(Z);
  display.println("");
  Serial.print("Gyroscope ");
  Serial.print("RX: ");
  Rx = int(g.gyro.x) * (180.0 / PI);
  if (abs(Rx) >= tempAng) {
    Rx = int(g.gyro.x) * (180.0 / PI);
  } else {
    Rx = 0;
  }
  Serial.print(Rx);
  Serial.print("  ");
  Serial.print("RY: ");
  Ry = int(g.gyro.y) * (180.0 / PI);
  if (abs(Ry) >= tempAng) {
    Ry = int(g.gyro.y) * (180.0 / PI);
  } else {
    Ry = 0;
  }
  Serial.print(Ry);
  Serial.print("  ");
  Serial.print("RZ: ");
  Rz = int(g.gyro.z) * (180.0 / PI);
  if (abs(Rz) >= tempAng) {
    Rz = int(g.gyro.z) * (180.0 / PI);
  } else {
    Rz = 0;
  }
  Serial.print(Rz);
  Serial.println(" rps");
  display.println("Gyroscope - dps");
  display.print("Rx: ");
  display.print(Rx);
  display.print(", ");
  display.print("Ry: ");
  display.print(Ry);
  display.print(", ");
  display.print("Rz: ");
  display.print(Rz);
  display.println("");
  display.display();
  delay(100);
}
代码主要通过修改MPU6050的官方例程，这些都有官方程序可以参考，没什么可多说的，唯一不同的是，我的MPU6050读取的数据漂移很厉害，而且传感器很灵敏，为了降低灵敏度，并过滤掉漂移的数据，我设计了将采集的数据只保留了整数部分。然后，再过滤掉采集的加速度数据的绝对值小于temacc和采集的陀螺仪数据的绝对值小于temang的数据，这样就会过滤掉因为灵敏和漂移产生的无效数据。如下图：

FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇图2
FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇图7
FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇图8
接下来实现websocket传输数据到上位机，同样使用官方库#include <WebSocketsServer_Generic.h>，
测试代码如下：
/****************************************************************************************************************************
  ESP32_WebSocketServer.ino
  For ESP32
  Based on and modified from WebSockets libarary https://github.com/Links2004/ArduinoWebSockets
  to support other boards such as  SAMD21, SAMD51, Adafruit's nRF52 boards, etc.
  Built by Khoi Hoang https://github.com/khoih-prog/WebSockets_Generic
  Licensed under MIT license
  Originally Created on: 22.05.2015
  Original Author: Markus Sattler
*****************************************************************************************************************************/
#if !defined(ESP32)
  #error This code is intended to run only on the ESP32 boards ! Please check your Tools->Board setting.
#endif
#define _WEBSOCKETS_LOGLEVEL_     2
#include <WiFi.h>
#include <WiFiMulti.h>
#include <WiFiClientSecure.h>
#include <WebSocketsServer_Generic.h>
#define WS_PORT             8080
WiFiMulti         WiFiMulti;
WebSocketsServer  webSocket = WebSocketsServer(WS_PORT);
void hexdump(const void *mem, const uint32_t& len, const uint8_t& cols = 16)
{
  const uint8_t* src = (const uint8_t*) mem;
  Serial.printf("\n[HEXDUMP] Address: 0x%08X len: 0x%X (%d)", (ptrdiff_t)src, len, len);
  for (uint32_t i = 0; i < len; i++)
  {
    if (i % cols == 0)
    {
      Serial.printf("\n[0x%08X] 0x%08X: ", (ptrdiff_t)src, i);
    }
    Serial.printf("%02X ", *src);
    src++;
  }
  Serial.printf("\n");
}
String messageFromServer = String("Message from Server on ") + String(ARDUINO_BOARD);
void webSocketEvent(const uint8_t& num, const WStype_t& type, uint8_t * payload, const size_t& length)
{
  switch (type)
  {
    case WStype_DISCONNECTED:
      Serial.printf("[%u] Disconnected!\n", num);
      break;
    case WStype_CONNECTED:
    {
      IPAddress ip = webSocket.remoteIP(num);
      Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
      // send message to client
      webSocket.sendTXT(num, "Connected");
    }
    break;
    case WStype_TEXT:
      Serial.printf("[%u] get Text: %s\n", num, payload);
      // send message to client
      webSocket.sendTXT(num, messageFromServer);
      // send data to all connected clients
      // webSocket.broadcastTXT(num, messageFromServer);
      break;
    case WStype_BIN:
      Serial.printf("[%u] get binary length: %u\n", num, length);
      hexdump(payload, length);
      // send message to client
      webSocket.sendBIN(num, payload, length);
      break;
    case WStype_ERROR:
    case WStype_FRAGMENT_TEXT_START:
    case WStype_FRAGMENT_BIN_START:
    case WStype_FRAGMENT:
    case WStype_FRAGMENT_FIN:
      break;
    default:
      break;
  }
}
void setup()
{
  // Serial.begin(921600);
  Serial.begin(115200);
  while (!Serial);
  delay(200);
  Serial.print("\nStarting ESP32_WebSocketServer on ");
  Serial.println(ARDUINO_BOARD);
  Serial.println(WEBSOCKETS_GENERIC_VERSION);
  //Serial.setDebugOutput(true);
  WiFiMulti.addAP("HUAWEI_simo", "simo0309...");
  //WiFi.disconnect();
  while (WiFiMulti.run() != WL_CONNECTED)
  {
    Serial.print(".");
    delay(100);
  }
  Serial.println();
  webSocket.begin();
  webSocket.onEvent(webSocketEvent);
  // server address, port and URL
  Serial.print("WebSockets Server started @ IP address: ");
  Serial.print(WiFi.localIP());
  Serial.print(", port: ");
  Serial.println(WS_PORT);
}
void loop()
{
  webSocket.loop();
}
官方代码，基本上只需要更改自家WIFI的SSID和密码就行了，FireBeetle 2 ESP32-S3作为服务器，PC上位机作为客户端。
上位机我采用Qt编写，使用了QT的<QWebSocket>类。使用时，别忘记了在PRO文件中加上QT+=websockets。
上位机代码如下：
WeSocketClient.h文件：
#ifndef WESOCKETCLIENT_H
#define WESOCKETCLIENT_H
#pragma execution_character_set("utf-8") //允许中文
#include <QObject>
#include <QWebSocket>
class WeSocketClient : public QObject
{
    Q_OBJECT
public:
    explicit WeSocketClient(QObject *parent = nullptr);
    ~WeSocketClient();
    bool connectedstate = false;
    void sendMessage(const QString &message);//发送信息函数
    void open();
    void close();
    void flush();
    QString message;
private slots:
    void onConnected();//连接槽函数
    void onDisconnected();//断开连接槽函数
    void onTextMessageReceived(const QString &message);//接收信息槽函数
private:
    QWebSocket m_webSocket;
signals:
    void SendConnectedState(bool state);
    void SendReceiveMsg(const QString &message);
};
#endif // WESOCKETCLIENT_H
WeSocketClient.cpp文件：
#include "./include/WeSocketClient.h"
#pragma execution_character_set("utf-8") //允许中文
WeSocketClient::WeSocketClient(QObject *parent)
    : QObject{parent}
{
    connect(&m_webSocket, &QWebSocket::connected, this, &WeSocketClient::onConnected);
    connect(&m_webSocket, &QWebSocket::disconnected, this, &WeSocketClient::onDisconnected);
}
void WeSocketClient::sendMessage(const QString &message)
{
    if(connectedstate)
        m_webSocket.sendTextMessage(message);
}
void WeSocketClient::open()
{
    m_webSocket.open(QUrl(tr("ws://192.168.100.147:8080")));
}
void WeSocketClient::close()
{
    m_webSocket.close();
    connectedstate = false;
}
void WeSocketClient::flush()
{
    m_webSocket.flush();
    connectedstate = false;
}
void WeSocketClient::onConnected()
{
   connectedstate = true;
   emit SendConnectedState(true);
      qDebug() <<"jhc";
   connect(&m_webSocket, &QWebSocket::textMessageReceived, this, &WeSocketClient::onTextMessageReceived);
}
void WeSocketClient::onDisconnected()
{
    connectedstate = false;
    emit SendConnectedState(false);
    disconnect(&m_webSocket, &QWebSocket::textMessageReceived, this, &WeSocketClient::onTextMessageReceived);
}
void WeSocketClient::onTextMessageReceived(const QString &message)
{
    this->message = message;
    //std::cout<<message<<std::endl;
    qDebug() <<message;
    emit SendReceiveMsg(message);
}
WeSocketClient::~WeSocketClient()
{
}

FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇图3

FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇图4
上位机是我做的一个综合性的程序，将这个功能加入到其中了，只展示无线控制器这部分，通讯测试没有问题，就开始把websocket程序和前面的mpu6050和屏幕的代码整合到一起了，将我需要的mpu数据发送给上位机。
代码如下：
#include <Adafruit_MPU6050.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_Sensor.h>
#define _WEBSOCKETS_LOGLEVEL_ 2
#include <WiFi.h>
#include <WiFiMulti.h>
#include <WiFiClientSecure.h>
#include <WebSocketsServer_Generic.h>
#define WS_PORT 8080
WiFiMulti WiFiMulti;
WebSocketsServer webSocket = WebSocketsServer(WS_PORT);
Adafruit_MPU6050 mpu;
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 32, &Wire);
int X;
int Y;
int Z;
int Rx;
int Ry;
int Rz;
int tempAcc = 5;
int tempAng = 60;
uint8_t numc = 0;
bool connectsate = false;
void hexdump(const void* mem, const uint32_t& len, const uint8_t& cols = 16) {
  const uint8_t* src = (const uint8_t*)mem;
  Serial.printf("\n[HEXDUMP] Address: 0x%08X len: 0x%X (%d)", (ptrdiff_t)src, len, len);
  for (uint32_t i = 0; i < len; i++) {
    if (i % cols == 0) {
      Serial.printf("\n[0x%08X] 0x%08X: ", (ptrdiff_t)src, i);
    }
    Serial.printf("%02X ", *src);
    src++;
  }
  Serial.printf("\n");
}
String messageFromServer;
void webSocketEvent(const uint8_t& num, const WStype_t& type, uint8_t* payload, const size_t& length) {
  switch (type) {
    case WStype_DISCONNECTED:
      Serial.printf("[%u] Disconnected!\n", num);
      connectsate = false;
      numc = num;
      break;
    case WStype_CONNECTED:
      {
        IPAddress ip = webSocket.remoteIP(num);
        Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
        // send message to client
        webSocket.sendTXT(num, "Connected");
      }
      break;
    case WStype_TEXT:
      Serial.printf("[%u] get Text: %s\n", num, payload);
      // send message to client
      connectsate = true;
      // send data to all connected clients
      // webSocket.broadcastTXT(num, messageFromServer);
      break;
    case WStype_BIN:
      Serial.printf("[%u] get binary length: %u\n", num, length);
      hexdump(payload, length);
      // send message to client
      webSocket.sendBIN(num, payload, length);
      break;
    case WStype_ERROR:
    case WStype_FRAGMENT_TEXT_START:
    case WStype_FRAGMENT_BIN_START:
    case WStype_FRAGMENT:
    case WStype_FRAGMENT_FIN:
      break;
    default:
      break;
  }
}
void setup() {
  Serial.begin(115200);
  // while (!Serial);
  Serial.println("MPU6050 OLED demo");
  if (!mpu.begin()) {
    Serial.println("Sensor init failed");
    while (1)
      yield();
  }
  Serial.println("Found a MPU-6050 sensor");
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {  // Address 0x3C for 128x32
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;  // Don't proceed, loop forever
  }
  display.display();
  delay(500);  // Pause for 2 seconds
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setRotation(0);
  //--------------websocket---------------------------------------------------
  Serial.print("\nStarting ESP32_WebSocketServer on ");
  Serial.println(ARDUINO_BOARD);
  Serial.println(WEBSOCKETS_GENERIC_VERSION);
  display.clearDisplay();
  display.println(ARDUINO_BOARD);
  display.println(WEBSOCKETS_GENERIC_VERSION);
  //Serial.setDebugOutput(true);
  WiFiMulti.addAP("HUAWEI_simo", "simo0309...");
  //WiFi.disconnect();
  while (WiFiMulti.run() != WL_CONNECTED) {
    Serial.print(".");
    display.clearDisplay();
    display.print(".");
    delay(100);
  }
  Serial.println();
  webSocket.begin();
  webSocket.onEvent(webSocketEvent);
  // server address, port and URL
  Serial.print("WebSockets Server started @ IP address: ");
  Serial.print(WiFi.localIP());
  display.clearDisplay();
  display.println(WiFi.localIP());
  Serial.print(", port: ");
  display.println(WS_PORT);
  Serial.println(WS_PORT);
}
void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  display.clearDisplay();
  display.setCursor(0, 0);
  Serial.print("Accelerometer ");
  Serial.print("X: ");
  X = a.acceleration.x;
  if (abs(X) >= tempAcc) {
    X = a.acceleration.x;
  } else {
    X = 0;
  }
  Serial.print(X);
  Serial.print("  ");
  Serial.print("Y: ");
  Y = a.acceleration.y;
  if (abs(Y) >= tempAcc) {
    Y = a.acceleration.y;
  } else {
    Y = 0;
  }
  Serial.print(Y);
  Serial.print("  ");
  Serial.print("Z: ");
  Z = a.acceleration.z - 9;
  if (abs(Z) >= tempAcc) {
    Z = a.acceleration.z - 9;
  } else {
    Z = 0;
  }
  Serial.print(Z);
  Serial.println(" m/s^2");
  display.println("Accelerometer - m/s^2");
  display.print("X:");
  display.print(X);
  display.print(", ");
  display.print("Y:");
  display.print(Y);
  display.print(", ");
  display.print("Z:");
  display.print(Z);
  display.println("");
  Serial.print("Gyroscope ");
  Serial.print("RX: ");
  Rx = int(g.gyro.x) * (180.0 / PI);
  if (abs(Rx) >= tempAng) {
    Rx = int(g.gyro.x) * (180.0 / PI);
  } else {
    Rx = 0;
  }
  Serial.print(Rx);
  Serial.print("  ");
  Serial.print("RY: ");
  Ry = int(g.gyro.y) * (180.0 / PI);
  if (abs(Ry) >= tempAng) {
    Ry = int(g.gyro.y) * (180.0 / PI);
  } else {
    Ry = 0;
  }
  Serial.print(Ry);
  Serial.print("  ");
  Serial.print("RZ: ");
  Rz = int(g.gyro.z) * (180.0 / PI);
  if (abs(Rz) >= tempAng) {
    Rz = int(g.gyro.z) * (180.0 / PI);
  } else {
    Rz = 0;
  }
  Serial.print(Rz);
  Serial.println(" rps");
  display.println("Gyroscope - dps");
  display.print("Rx: ");
  display.print(Rx);
  display.print(", ");
  display.print("Ry: ");
  display.print(Ry);
  display.print(", ");
  display.print("Rz: ");
  display.print(Rz);
  display.println("");
  display.display();
  messageFromServer = String(X) + "," + String(Y) + "," + String(Z) + "," + String(Rx) + "," + String(Ry) + "," + String(Rz);
  if (connectsate) {
    webSocket.sendTXT(numc, messageFromServer);
  }
  webSocket.loop();
  delay(100);
}
主循环中加了，如果接收到客户端发送的字符，就可以循环发送陀螺仪的数据到上位机。

FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇图5
FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇图6
由于上位机控制机械臂是公司项目，不方便公开，所以暂时不展示了。后面如果有机会，再开源。
下面是视频展示


回复

使用道具 举报

高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
撰写新帖
关于楼主

楼主的其它帖子

background 创造力兑换 background 心愿单 background 创客导航
上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2024 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail