pid自平衡杆

主函数：

#include <WiFi.h>          //wifi功能需要的库
#include <MPU6050_tockn.h> //mpu6050陀螺仪库
#include "Motor.hpp"

const char *remoteIP = "192.168.4.2";
uint16_t remotePort = 2222;

WiFiUDP Udp; // 声明UDP对象.服务端开启，监听并记录来自客户端的ip,端口

MPU6050 mpu6050(Wire);
Motor motor(7, 10, 0, 1); // 前后平衡控制 空心杯马达引脚

bool sw = false; // 手势感应,倾倒检测

const int ledpin1 = 13, ledpin2 = 12;
int motor_pwm; // 前后平衡控制 马达的PWM

float kp = 16, ki = 0.03, kd = 19.00;
float balanceAngle = 1.55, keepAngle;
float ax, gx, integrate, bias, gy; // 角度，角速度，积分，误差

void detect_sw()
{
  static bool sw1, sw2;

  if (gy > 450)
  {
    sw1 = true;
  }
  if (gy < -450)
  {
    sw1 = false;
  }

  if ((ax > -45) && (ax < 45))
  {
    sw2 = true;
  }
  else
  {
    sw2 = false;
  }

  sw = (sw1 && sw2) ? true : false;
}

void pwm_calculation()
{
  bias = ax - keepAngle;
  integrate += bias;
  integrate = constrain(integrate, -200, 200);
  motor_pwm = kp * bias + ki * integrate + kd * gx;
}

void motor_ctrl()
{
  if (!sw)
  {
    motor_pwm = 0;
  }
  motor.run(motor_pwm);
}

void udp_print_val()
{
  Udp.beginPacket(remoteIP, remotePort); // 准备发送数据到目标IP和目标端口
  String angle_gyro = String(ax) + '\t' + String(gx) + '\t';
  Udp.println(angle_gyro); // 将接收到的数据放入发送的缓冲区
  Udp.endPacket();         // 向目标IP目标端口发送数据
  delay(50);
}

void udp_debug()
{
  if (Udp.parsePacket()) // parsePacket()返回解析到的数据长度，不为0则有数据
  {
    char val = Udp.read(); // 读取1字节数据;
    Serial.println(val);
    if (val == '0')
    {
      kp -= 1;
    }
    if (val == '1')
    {
      kp += 1;
    }
    if (val == '2')
    {
      ki -= 0.01;
    }
    if (val == '3')
    {
      ki += 0.01;
    }
    if (val == '4')
    {
      kd -= 0.1;
    }
    if (val == '5')
    {
      kd += 0.1;
    }
    if (val == 'u')
    {
      keepAngle += 0.1;
    }
    if (val == 'd')
    {
      keepAngle -= 0.1;
    }
    if (val == 'c')
    {
      keepAngle = 15;
    }
    Udp.beginPacket(remoteIP, remotePort); // 准备发送数据到目标IP和目标端口
    String pid_val = String(kp) + '\t' + String(ki) + '\t' + String(kd) + '\t' + String(keepAngle);
    Udp.println(pid_val); // 将接收到的数据放入发送的缓冲区
    Udp.endPacket();      // 向目标IP目标端口发送数据
  }
}

void led_effect()
{
  int ledPWM = abs(motor_pwm);
  if (motor_pwm < 0)
  {
    analogWrite(ledpin1, ledPWM);
  }
  else
  {
    analogWrite(ledpin2, ledPWM);
  }
}

void setup()
{
  Serial.begin(115200);                // 初始化波特率
  WiFi.softAP("ESP32_Udp_server", ""); // 打开ESP32热点，ESP32默认IP是192.168.4.1
  Udp.begin(1111);                     // 启动UDP监听这个端口
  Wire.begin(4, 5);
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  keepAngle = balanceAngle;
  motor.flameout(); // 防止因部分引脚开机启动默认为高电平，导致马达转动
  analogWrite(ledpin1, 125);
  analogWrite(ledpin2, 125);
  delay(500);
}

void loop()
{
  mpu6050.update();

  ax = mpu6050.getAngleX();
  gx = mpu6050.getGyroX();
  gy = mpu6050.getGyroY();

  pwm_calculation();
  detect_sw();
  motor_ctrl();

  led_effect();

  udp_debug();
  // udp_print_val();
}

motor（马达）代码：

#pragma once
#include <Arduino.h>

#define LEDC_FREQ 500 // LEDC频率
#define LEDC_BIT 8    // LEDC分辨率(精度) 0-255

class Motor
{
private:
    int motorPIN1, motorPIN2; // ADC、马达控制引脚
    int channel1, channel2;   // esp32的ledc通道
public:
    Motor(int, int, int, int);
    void run(int);
    void brake();    // 马达静止（高阻）
    void flameout(); // 马达熄火（无阻力）
};

Motor::Motor(int mpin1, int mpin2, int ch1 = 0, int ch2 = 1)
{
    motorPIN1 = mpin1;
    motorPIN2 = mpin2;
    channel1 = ch1;
    channel2 = ch2;
    ledcSetup(channel1, LEDC_FREQ, LEDC_BIT);
    ledcSetup(channel2, LEDC_FREQ, LEDC_BIT);
    ledcAttachPin(motorPIN1, channel1);
    ledcAttachPin(motorPIN2, channel2);
}

// 马达运动PWM控制
void Motor::run(int pwm)
{
    pwm = constrain(pwm, -255, 255);
    if (pwm >= 0)
    {
        ledcWrite(channel1, pwm);
        ledcWrite(channel2, 0);
    }
    else
    {
        ledcWrite(channel1, 0);
        ledcWrite(channel2, -pwm);
    }
}

// 马达静止（高阻）
void Motor::brake()
{
    ledcWrite(channel1, 255);
    ledcWrite(channel2, 255);
}

// 马达熄火（无阻力）
void Motor::flameout()
{
    ledcWrite(channel1, 0);
    ledcWrite(channel2, 0);
}
清单：