115 lines
3.9 KiB
C++
115 lines
3.9 KiB
C++
//
|
||
// Created by lgv on 2025/8/24.
|
||
//
|
||
|
||
#pragma once
|
||
|
||
#include <utility>
|
||
|
||
#include "devices/abstract_camera.h"
|
||
#include "devices/abstract_dexhand.h"
|
||
#include "devices/abstract_robot.h"
|
||
#include "cmvr/msgs/geometry.pb.h"
|
||
#include "librealsense2/rs.h"
|
||
#include "librealsense2/h/rs_frame.h"
|
||
namespace cmvr {
|
||
namespace ctrl {
|
||
|
||
// 简易版 PID 控制器，带死区、积分限幅、输出限幅和输出斜率限制
|
||
class PID {
|
||
public:
|
||
PID(double kp, double ki, double kd,
|
||
double i_max,
|
||
double output_max_pos, double output_max_neg,
|
||
double delta_max = 0.0) // 输出变化最大值，0 表示不限制
|
||
: kp_(kp), ki_(ki), kd_(kd),
|
||
i_max_(i_max),
|
||
output_max_pos_(output_max_pos),
|
||
output_max_neg_(output_max_neg),
|
||
delta_max_(delta_max),
|
||
prev_error_(0), integral_(0), prev_output_(0) {}
|
||
|
||
double compute(double target, double current, double dt, double deadband = 0.0) {
|
||
double error = target - current;
|
||
|
||
// 死区处理
|
||
if (fabs(error) <= deadband) {
|
||
error = 0.0;
|
||
}
|
||
|
||
// 积分累加限幅
|
||
integral_ += error * dt;
|
||
if (integral_ > i_max_) integral_ = i_max_;
|
||
if (integral_ < -i_max_) integral_ = -i_max_;
|
||
|
||
// 微分
|
||
double derivative = (error - prev_error_) / dt;
|
||
prev_error_ = error;
|
||
|
||
// PID 输出
|
||
double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
|
||
|
||
// 输出限幅
|
||
if (output > output_max_pos_) output = output_max_pos_;
|
||
if (output < -output_max_neg_) output = -output_max_neg_;
|
||
|
||
// 输出斜率限制
|
||
if (delta_max_ > 0.0) {
|
||
double delta = output - prev_output_;
|
||
if (delta > delta_max_) output = prev_output_ + delta_max_;
|
||
else if (delta < -delta_max_) output = prev_output_ - delta_max_;
|
||
}
|
||
|
||
prev_output_ = output;
|
||
return output;
|
||
}
|
||
|
||
private:
|
||
double kp_, ki_, kd_;
|
||
double prev_error_;
|
||
double integral_;
|
||
double i_max_; // 积分限幅
|
||
double output_max_pos_; // 向下按的最大输出
|
||
double output_max_neg_; // 向上抬的最大输出
|
||
double delta_max_; // 输出斜率限制
|
||
double prev_output_;
|
||
};
|
||
|
||
|
||
|
||
|
||
class TouchController {
|
||
public:
|
||
TouchController() {};
|
||
TouchController(std::shared_ptr<device::AbstractRobot> robot,std::shared_ptr<device::AbstractDexHand> hand,std::shared_ptr<device::AbstractCamera> cam)
|
||
:robot_(std::move(robot)),hand_(std::move(hand)),cam_(std::move(cam)),
|
||
pid_(std::make_shared<PID>(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){}
|
||
~TouchController()=default;
|
||
|
||
|
||
bool isArrive(double max_force);
|
||
|
||
void touch(int u,int v ,double max_force);
|
||
|
||
void touch(std::shared_ptr<device::AbstractRobot> robot,const msgs::Pose3d pose,const msgs::Pose3d offset);
|
||
void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force);
|
||
|
||
private:
|
||
std::shared_ptr<device::AbstractRobot> robot_{nullptr};
|
||
std::shared_ptr<device::AbstractDexHand> hand_{nullptr};
|
||
std::shared_ptr<device::AbstractCamera> cam_{nullptr};
|
||
|
||
std::shared_ptr<PID> pid_{nullptr};
|
||
|
||
const double touch_threshold_ = 5.0; // 触控判定阈值
|
||
|
||
|
||
// 从压阻矩阵提取触控点与压力
|
||
bool extractTouch(const std::vector<std::vector<uint16_t>>& matrix,double& force, int& x, int& y);
|
||
|
||
|
||
|
||
};
|
||
}
|
||
}
|