#include <Arduino.h>
#include <Wire.h>
#include <MPU6050_light.h>
#include <Esp32McpwmMotor.h>
#include <Esp32PcntEncoder.h>

Esp32McpwmMotor motor;        // 创建一个名为motor的对象，用于控制电机
Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器

MPU6050 mpu(Wire);

int64_t last_ticks[2] = {0, 0}; // 用于存储上一次编码器的计数
int64_t delta_ticks[2] = {0, 0}; // 用于存储两次编码器计数之间的差值
int64_t last_update_time = 0;    // 用于存储上一次更新时间
float current_speed[2] = {0, 0}; // 用于存储当前速度

// put function declarations here:
int myFunction(int, int);

#define TRIG 27
#define ECHO 21
#define LED_BUILTIN 2

unsigned long previousMillis = 0; // 用于记录上一次电平切换的时间
const long interval = 500;        // 定时器间隔，单位为毫秒

void setup()
{
  // put your setup code here, to run once:
  int result = myFunction(2, 3);
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(LED_BUILTIN, OUTPUT);

  Wire.begin(18, 19); // SDA, SCL
  byte status = mpu.begin();
  Serial.print(F("MPU6050 status: "));
  Serial.println(status);
  while (status != 0)
  {
    // stop everything if could not connect to MPU6050
  }
  mpu.calcOffsets(true, true); // gyro and accelero
  Serial.println("Done!");
  Serial.println("===========================\n");
  motor.attachMotor(0, 22, 23); // 将电机0连接到引脚22和引脚23
  motor.attachMotor(1, 12, 13); // 将电机1连接到引脚12和引脚13
  Serial.println("Motor Done!");
  // 编码器
  encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接
  encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接
  Serial.println("Encoder Done!");

  // 控制电机
  motor.updateMotorSpeed(0, 100); // 设置电机0的速度为负70
  motor.updateMotorSpeed(1, 0);  // 设置电机1的速度为正70
}

unsigned long timer = 0;
void loop()
{
  // put your main code here, to run repeatedly:
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  double duration = pulseIn(ECHO, HIGH);
  double distance = duration * 0.034 / 2;
  // Serial.printf("Distance: %lf cm\n", distance);
  /*
  mpu.update();
  if ((millis() - timer) > 1000)
  {
    // 温度
    Serial.printf("Temp: %lf\n", mpu.getTemp());
    // 加速度计数据
    Serial.printf("Accel: %lf %lf %lf\n", mpu.getAccX(), mpu.getAccY(), mpu.getAccZ());
    // 加速度计角度
    Serial.printf("Angle: %lf %lf %lf\n", mpu.getAngleX(), mpu.getAngleY(), mpu.getAngleZ());
    // 陀螺仪数据
    Serial.printf("Gyro: %lf %lf %lf\n", mpu.getGyroX(), mpu.getGyroY(), mpu.getGyroZ());
    // 综合角度
    Serial.printf("Angle: %lf %lf %lf\n", mpu.getAngleX(), mpu.getAngleY(), mpu.getAngleZ());
    Serial.println("===========================\n");
    timer = millis();
  }
  */
  // 获取当前时间
  unsigned long currentMillis = millis();

  // 检查是否达到定时器间隔
  if (currentMillis - previousMillis >= interval)
  {
    // 记录当前时间
    previousMillis = currentMillis;
    // 切换2号脚的电平
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  }

  delay(10);
  // 读取编码器的计数器数值
  int64_t dt = millis() - last_update_time; // 计算时间差
  delta_ticks[0] = encoders[0].getTicks() - last_ticks[0]; // 计算第一个编码器的差值
  delta_ticks[1] = encoders[1].getTicks() - last_ticks[1]; // 计算第二个编码器的差值
  current_speed[0] = delta_ticks[0]*0.105805 / (float)dt; // 计算第一个编码器的速度
  current_speed[1] = delta_ticks[1]*0.105805 / (float)dt; // 计算第二个编码器的速度

  //下次正常计算
  last_ticks[0] = encoders[0].getTicks(); // 保存第一个编码器的计数
  last_ticks[1] = encoders[1].getTicks(); // 保存第二个编码器的计数
  last_update_time = millis(); // 保存当前时间
  // 读取并打印两个编码器的计数器数值 一圈1930个脉冲 直径65mm  一个脉冲前进=0.105805mm
  Serial.printf("speed1=%lf,speed2=%lf\n", current_speed[0], current_speed[1]);
  //Serial.printf("tick1=%d,tick2=%d\n", encoders[0].getTicks(), encoders[1].getTicks());
}

// put function definitions here:
int myFunction(int x, int y)
{
  return x + y;
}

Lib

PidController.cpp

#include "PidController.h"
#include <Arduino.h>

PidController::PidController() = default;
// 默认构造函数
PidController::PidController(float kp, float ki, float kd) {
    _kp = kp;
    _ki = ki;
    _kd = kd;

}
/*
        float update(float current);// 更新PID控制器，提供当前值，返回控制量
        void update_target(float target); // 更新目标值
        void update_pid(float kp, float ki, float kd); // 更新PID参数
        void reset(); // 重置PID控制器
        void out_limit(float); // 输出限制
*/
float PidController::update(float current){
    error_ = target_ - current; // 计算误差
    error_sum_ += error_; // 累加误差
    if(error_sum_ > intergral_up_) error_sum_ = intergral_up_; // 积分限幅
    if(error_sum_ < -1*intergral_up_) error_sum_ = -1*intergral_up_; // 积分限幅

    derror_ = error_ - prev_error_; // 计算误差变化率
    prev_error_ = error_; // 保存上一次误差
    float output = _kp * error_ + _ki * error_sum_ + _kd * derror_; // 计算输出
    if(output > out_max_) output = out_max_; // 输出限幅
    if(output < out_min_) output = out_min_; // 输出限幅
    return output;
}
void PidController::update_target(float target){
    target_ = target;
}
void PidController::update_pid(float kp, float ki, float kd){
    _kp = kp;
    _ki = ki;
    _kd = kd; 
}
void PidController::reset(){
    error_ = 0;
    error_sum_ = 0;
    derror_ = 0; 
    prev_error_ = 0;
    _kp = 0;
    _ki = 0;
    _kd = 0;
    intergral_up_ = 2500; 
    out_max_ = 0;
    out_min_ = 0;
}
void PidController::out_limit(float out_min, float out_max){
    out_max_ = out_max;
    out_min_ = out_min; 
}

PidController.h

#ifndef _PID_CONTROLLER_H_
#define _PID_CONTROLLER_H_

class PidController{
    public:
        PidController() = default;
        PidController(float kp, float ki, float kd);

    private:
        // pid 控制器参数
        float target_;// 目标值
        float out_min_;// 输出最小值
        float out_max_;// 输出最大值
        float _kp; // 比例系数
        float _ki; // 积分系数
        float _kd; // 微分系数
        float intergral_up_ = 2500; // 积分上限
        //pid 中间
        float error_; // 误差
        float error_sum_; // 误差和
        float derror_; // 误差差分
        float prev_error_; // 上次误差
        

    public:
        float update(float current);// 更新PID控制器，提供当前值，返回控制量
        void update_target(float target); // 更新目标值
        void update_pid(float kp, float ki, float kd); // 更新PID参数
        void reset(); // 重置PID控制器
        void out_limit(float out_min, float out_max); // 输出限制
        
};


#endif
// _PID_CONTROLLER_H_;