当前位置：首页 > ds >正文

ROS 2 FishBot PID控制电机代码

ds 2025/5/15 4:44:47

#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 2unsigned 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, SCLbyte 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 acceleroSerial.println("Done!");Serial.println("===========================\n");motor.attachMotor(0, 22, 23); // 将电机0连接到引脚22和引脚23motor.attachMotor(1, 12, 13); // 将电机1连接到引脚12和引脚13Serial.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的速度为负70motor.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.105805mmSerial.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_;

查看全文

http://www.xdnf.cn/news/5648.html