esp32读取倾角传感器,控制伺服电机使其保持平衡的代码

admin 5月前 387



#include <Arduino.h>

//#include <WiFi.h>

#include <AsyncTCP.h>

#include "SPIFFS.h"

#include <Wire.h>

#include <ESPAsyncWebSrv.h>

#include <Arduino_JSON.h>

#include <Ticker.h>

#include <WiFiAP.h>

//#include <HardwareSerial.h>

#include <SoftwareSerial.h>

//SoftwareSerial rs485(10, 11);

//HardwareSerial rs485(1);

SoftwareSerial rs485(6, 7);  // 485通信使用的软件串口对象


const char *ssid = "ESP32";

const char *password = "12345678";

Ticker tickerSetFlag;

// Create AsyncWebServer object on port 80

AsyncWebServer server(80);

// Create a WebSocket object

AsyncWebSocket ws("/ws");

//设置初始值及变量

String message = "";

float roll = 0;

String dianji = "0";

String sliderValue3 = "0";

String sliderValue4 = "0";

int timeto = 0;

volatile bool task1Flag = false;

volatile bool task2Flag = false;

volatile bool task3Flag = false;

//Json Variable to Hold Slider Values

JSONVar sliderValues;

//Get Slider Values  获取滑块值  要传递的值

String getSliderValues() {

  sliderValues["sliderValue1"] = String(roll);

  sliderValues["sliderValue2"] = String(timeto);

  sliderValues["sliderValue3"] = String(sliderValue3);

  sliderValues["sliderValue4"] = String(sliderValue4);

  String jsonString = JSON.stringify(sliderValues);

  return jsonString;

}


最新回复 (3)
  • admin 4月前
    2
    后边是 加了 限位开关的。
  • admin 3月前
    3
    #include <Arduino.h>
    //#include <WiFi.h>
    #include <AsyncTCP.h>
    #include "SPIFFS.h"
    #include <Wire.h>
    #include <ESPAsyncWebSrv.h>
    #include <Arduino_JSON.h>
    #include <Ticker.h>
    #include <WiFiAP.h>
    //#include <HardwareSerial.h>
    #include <SoftwareSerial.h>
    SoftwareSerial rs485(6, 7);  // 485通信使用的软件串口对象
    //const char *ssid = "CMCC4444";
    //const char *password = "3333321";
    const char *ssid = "ESP32";
    const char *password = "12345678";
    Ticker tickerSetFlag;
    // Create AsyncWebServer object on port 80
    AsyncWebServer server(80);
    // Create a WebSocket object
    AsyncWebSocket ws("/ws");
    //设置初始值及变量
    String message = "";
    float roll = 0;
    float jiaodu = 0;
    String dianji = "0";
    String sliderValue3 = "1";
    String sliderValue4 = "0";
    int timeto = 0;
    volatile bool task1Flag = true;
    volatile bool task2Flag = false;
    volatile bool task3Flag = false;
    volatile bool upflag = false;
    volatile bool downflag = false;
    static bool hasExecuted = false;
    //Json Variable to Hold Slider Values
    JSONVar sliderValues;
    //Get Slider Values  获取滑块值  要传递的值
    String getSliderValues() {
      sliderValues["sliderValue1"] = String(roll);
      sliderValues["sliderValue2"] = String(timeto);
      sliderValues["sliderValue3"] = String(sliderValue3);
      sliderValues["sliderValue4"] = String(sliderValue4);
      String jsonString = JSON.stringify(sliderValues);
      return jsonString;
    }
    //Initialize SPIFFS 初始化 SPIFFS
    void initFS() {
      if (!SPIFFS.begin()) {
        Serial.println("An error has occurred while mounting SPIFFS");
      } else {
        Serial.println("SPIFFS mounted successfully");
      }
    }
    // Initialize ap
    void initWifiAp() {
      if (!WiFi.softAP(ssid, password)) {
        log_e("Soft AP creation failed.");
        while (1)
          ;
      }
      IPAddress myIP = WiFi.softAPIP();
      Serial.print("AP IP address: ");
      Serial.println(myIP);
    }
    // Initialize WiFi
    void initWiFi() {
      WiFi.mode(WIFI_STA);
      WiFi.begin(ssid, password);
      Serial.print("Connecting to WiFi ..");
      while (WiFi.status() != WL_CONNECTED) {
        Serial.print('.');
        delay(1000);
      }
      Serial.println(WiFi.localIP());
    }
    //向客户端发消息的函数
    void notifyClients(String sliderValues) {
      ws.textAll(sliderValues);
    }
    //初始化ws
    void initWebSocket() {
      ws.onEvent(onEvent);
      server.addHandler(&ws);
    }
    //ws 有连接时的信息
    void onEvent(AsyncWebSocket *server, AsyncWebSocketClient *client, AwsEventType type, void *arg, uint8_t *data, size_t len) {
      switch (type) {
        case WS_EVT_CONNECT:
          Serial.printf("WebSocket client #%u connected from %s\n", client->id(), client->remoteIP().toString().c_str());
          break;
        case WS_EVT_DISCONNECT:
          Serial.printf("WebSocket client #%u disconnected\n", client->id());
          break;
        case WS_EVT_DATA:
          handleWebSocketMessage(arg, data, len);
          break;
        case WS_EVT_PONG:
        case WS_EVT_ERROR:
          break;
      }
    }
    //这段代码是一个处理WebSocket消息的函数
    void handleWebSocketMessage(void *arg, uint8_t *data, size_t len) {
      AwsFrameInfo *info = (AwsFrameInfo *)arg;
      if (info->final && info->index == 0 && info->len == len && info->opcode == WS_TEXT) {
        data[len] = 0;
        message = (char *)data;
        Serial.println(message);
        /*
            if (message.indexOf("1s") >= 0)
            {
              roll = message.substring(2);
              notifyClients(getSliderValues());
            }
            if (message.indexOf("2s") >= 0)
            {
              dianji = message.substring(2);
              notifyClients(getSliderValues());
            }
        */
        //手动选择
        if (message.indexOf("3s") >= 0) {
          sliderValue3 = message.substring(2);
          int num = sliderValue3.toInt();
          if (num == 1) {
            task2Flag = false;
          }
          if (num == 0) {
            task2Flag = true;
          }
          Serial.println(sliderValue3);
          notifyClients(getSliderValues());
        }
        //
        if (message.indexOf("4s") >= 0) {
          if (timeto == 0) {
            sliderValue4 = message.substring(2);
            Serial.println(sliderValue4);
            timeto = sliderValue4.toInt();
            notifyClients(getSliderValues());
            if (timeto > 0) {
              post485(200);
              delay(50);
              post485(200);
            }
            if (timeto < 0) {
              post485(-200);
              delay(50);
              post485(-200);
            }
          } else {
            sliderValue4 = "0";
            Serial.println(sliderValue4);
            notifyClients(getSliderValues());
          }
        }
        if (strcmp((char *)data, "getValues") == 0) {
          notifyClients(getSliderValues());
        }
      }
    }
    unsigned int calculateCRC(byte *data, unsigned int length) {
      unsigned int crc = 0xFFFF;
      for (unsigned int i = 0; i < length; i++) {
        crc ^= data[i];
        for (unsigned int j = 0; j < 8; j++) {
          if ((crc & 0x0001) != 0) {
            crc >>= 1;
            crc ^= 0xA001;
          } else {
            crc >>= 1;
          }
        }
      }
      return crc;
    }
    void setFlag() {
      //task1Flag = true;
      if (task2Flag) {
        timeto = 0;
      } else {
        if (timeto > 0) {
          timeto--;
        }
        if (timeto < 0) {
          timeto++;
        }
        if (timeto == 0) {
          timeto = 0;
        }
      }
      //Serial.println(timeto);
      //task2Flag = true;
      //  Serial.println("发送数据到ws");
      notifyClients(getSliderValues());
    }
    void post485(int pwm) {
      char a[] = { 0x01, 0x06, 0x00, 0x00, 0x00, 0x01, 0x48, 0x0A };
      rs485.write(a, 8);
      //发送启动电机
      delay(50);
      byte response[8];
      response[0] = 0x01;
      response[1] = 0x06;
      response[2] = 0x00;
      response[3] = 0x06;
      response[4] = (pwm >> 8) & 0xFF;
      response[5] = pwm & 0xFF;
      // 计算CRC校验
      unsigned int crc = calculateCRC(response, 6);
      response[6] = crc & 0xFF;         // CRC低字节
      response[7] = (crc >> 8) & 0xFF;  // CRC高字节
      // 发送回复数据
      rs485.write(response, sizeof(response));
      //Serial.println(pwm);
    }
    void up() {
      upflag = true;
      stopRun();
    }
    void down() {
      downflag = true;
      stopRun();
    }
    void stopRun() {
      char a[] = { 0x01, 0x06, 0x00, 0x00, 0x00, 0x00, 0x89, 0xCA };
      rs485.write(a, 8);
      //digitalWrite(5, HIGH);
      // digitalWrite(5, LOW); // 点亮LE
    }
    void setup() {
      Serial.begin(115200);
      rs485.begin(9600);
      Serial.print("初始化倾角传感器");
      //初始化 倾角传感器
      char e[] = { 0x50, 0x06, 0x00, 0x01, 0x00, 0x00, 0xd5, 0x8b };
      rs485.write(e, 8);
      pinMode(3, INPUT);
      pinMode(4, INPUT);
      pinMode(5, OUTPUT);
      attachInterrupt(digitalPinToInterrupt(3), up, RISING);
      attachInterrupt(digitalPinToInterrupt(4), down, RISING);
      // initWiFi();
      initWifiAp();
      initFS();
      tickerSetFlag.attach(1, setFlag);
      initWebSocket();
      // Web Server Root URL
      server.on("/", HTTP_GET, [](AsyncWebServerRequest *request) {
        request->send(SPIFFS, "/index.html", "text/html");
      });
      server.serveStatic("/", SPIFFS, "/");
      // Start server
      server.begin();
    }
    void loop() {
      /*
      if (!hasExecuted) {
        post485(-3000);
        Serial.println("开始缩回");
        //缩回,直到顶到限位开关,停止;并反方向慢速继续行走4秒,
        while (1) {
          if (upflag || downflag) {
            delay(100);
            stopRun();
            upflag = false;
            downflag = false;
            Serial.println("已到限位");
            break;  // 当 i 等于 5 时跳出循环
          }
        }
        post485(1500);
        delay(4000);
        Serial.println("已到中间");
        stopRun();
        delay(150);
        stopRun();
        // rs485.write(a, 8);
        hasExecuted = true;
      }
    */
      delay(50);
      // put your main code here, to run repeatedly:
      char a[] = { 0x50, 0x03, 0x00, 0x3d, 0x00, 0x01, 0x18, 0x47 };
      rs485.write(a, 8);
      receive485();
      //roll = roll-0.05; //调试用
      delay(250);
      Serial.print("roll:");
      Serial.println(roll);
      if (task2Flag) {
        // Serial.println("自动调节");
        if (roll > 3 || roll < -3) {
          // Serial.print("超出偏差,需要自动调试:");
          //int output = pidCompute(roll, 0, 50.0, 5.0, 1.0);
          int output = roll * 350;
          Serial.print("pid output:");
          Serial.println(output);
          if (output < 0) {
            if (!upflag) {
              post485(output);
              //delay(250);
              downflag = false;
            } else {
              Serial.println("position limit-up");
              delay(150);
            }
          }
          if (output > 0) {
            if (!downflag) {
              post485(output);
              // delay(250);
              upflag = false;
            } else {
              Serial.println("position limit-down");
              delay(150);
            }
          }
        } 
      } else {
        // Serial.println("手动调节");
        if (timeto == 0) {
          stopRun();
          delay(300);
        }
      }
      ws.cleanupClients();
    }
    //接收数据并处理
    void receive485() {
      static byte response[8];  // 存储接收到的数据
      static int length = 0;    // 接收到的数据长度
      while (rs485.available()) {
        byte data = rs485.read();
        // Serial.println(data);
        if (length == 0 && data != 0x50) {
          continue;  // 如果未收到以0x50开头的数据,则继续等待
        }
        response[length] = data;
        length++;
        if (length >= 5) {
          if (response[0] == 0x50 && response[1] == 0x03 && response[2] == 0x02) {
            // if (response[0] == 0x50) {
            int combinedValue = (response[3] << 8) | response[4];                // 合并高8位和低8位为一个整数
            float result = static_cast<float>(combinedValue) / 32768.0 * 180.0;  // 计算结果
            if (result > 180) {
              roll = result - 360;
              //  Serial.print("目前角度:");
              //  Serial.println(roll);
            } else {
              roll = result;
              //  Serial.print("目前角度:");
              //  Serial.println(roll);
            }
            jiaodu = result - 180;
          }
          // 重置数据缓冲区和长度
          length = 0;
        }
      }
    }
    int pidCompute(double input, double setpoint, double kp, double ki, double kd) {
      static double lastError = 0;
      static double integral = 0;
      double error = setpoint - input;
      double pTerm = kp * error;
      integral += error;
      double iTerm = ki * integral;
      double dTerm = kd * (error - lastError);
      lastError = error;
      double output = pTerm + iTerm + dTerm;
      // 确定输出的符号并取整
      int intOutput = (output >= 0) ? (int)round(output) : (int)round(output);
      return intOutput;
    }


  • admin 3月前
    4


    完整代码

返回