FireBeetle 2 ESP32-S3+自制工业机器人无线控制手柄(二)--软件篇
接上文，为了设计一个工业机器人无线控制器，硬件部分已经准备完毕。接下来是软件部分。整体思路是使用FireBeetle 2 ESP32-S3作为主控，采集MPU6050陀螺仪数据，然后通过websocket的通讯方式，将采集的数据发送给PC上位机。上位机再将数据分发给不同类型机器人，控制其运动。同时FireBeetle 2 ESP32-S3再将采集的数据，通过IIC的屏幕，显示在小屏幕上。系统框架图如下： 首先，设计采集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 internallyif (!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 secondsdisplay.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的数据，这样就会过滤掉因为灵敏和漂移产生的无效数据。如下图：
接下来实现websocket传输数据到上位机，同样使用官方库#include <WebSocketsServer_Generic.h>，测试代码如下：/****************************************************************************************************************************ESP32_WebSocketServer.inoFor ESP32 Based on and modified from WebSockets libarary https://github.com/Links2004/arduinoWebSocketsto support other boards such asSAMD21, SAMD51, Adafruit's nRF52 boards, etc. Built by Khoi Hoang https://github.com/khoih-prog/WebSockets_GenericLicensed under MIT license Originally Created on: 22.05.2015Original 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;WebSocketsServerwebSocket = 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 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: ", (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, ip, ip, ip, 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 URLSerial.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_OBJECTpublic: 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(){ }
上位机是我做的一个综合性的程序，将这个功能加入到其中了，只展示无线控制器这部分，通讯测试没有问题，就开始把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 8080WiFiMulti 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 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: ", (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, ip, ip, ip, 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 internallyif (!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 secondsdisplay.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 URLSerial.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);}主循环中加了，如果接收到客户端发送的字符，就可以循环发送陀螺仪的数据到上位机。
由于上位机控制机械臂是公司项目，不方便公开，所以暂时不展示了。后面如果有机会，再开源。下面是视频展示https://v.youku.com/v_show/id_XNjAxNzYyMTI2OA==.html
页:
[1]