补偿方案arm

#include
#include
#include

class Compensator {
private:
std::deque x_deque, y_deque, t_deque; // 偏差队列
int window_size; // 滑动平均的窗口大小

public:
// 构造函数
Compensator(int window_size = 10) : window_size(window_size) {}

// 更新偏差值并计算补偿
void update(double x, double y, double t) {
    // 更新x的滑动平均
    if (x_deque.size() >= window_size) {
        x_deque.pop_front();  // 弹出最旧的偏差值
    }
    x_deque.push_back(x);

    // 更新y的滑动平均
    if (y_deque.size() >= window_size) {
        y_deque.pop_front();
    }
    y_deque.push_back(y);

    // 更新t的滑动平均
    if (t_deque.size() >= window_size) {
        t_deque.pop_front();
    }
    t_deque.push_back(t);
}

// 计算滑动平均补偿
double calculateCompensation(const std::deque<double>& deque) {
    if (deque.empty()) return 0.0;
    double sum = std::accumulate(deque.begin(), deque.end(), 0.0);
    return sum / deque.size();  // 计算平均值
}

// 获取当前的补偿值
void getCompensation() {
    double x_comp = calculateCompensation(x_deque);
    double y_comp = calculateCompensation(y_deque);
    double t_comp = calculateCompensation(t_deque);

    std::cout << "Compensation: x = " << x_comp << ", y = " << y_comp << ", t = " << t_comp << std::endl;
}

};

int main() {
// 创建补偿器对象，窗口大小为10
Compensator compensator(10);

// 模拟更新机械臂的放置偏差（x, y, t）
compensator.update(1.0, 2.0, 0.5);
compensator.update(1.2, 1.8, 0.6);
compensator.update(1.1, 2.1, 0.4);
compensator.update(0.9, 2.2, 0.3);
compensator.update(1.0, 2.0, 0.5);
compensator.update(1.3, 1.7, 0.4);
compensator.update(1.2, 1.9, 0.6);
compensator.update(1.0, 2.1, 0.5);
compensator.update(0.8, 2.3, 0.2);
compensator.update(1.1, 2.0, 0.4);

// 获取补偿值
compensator.getCompensation();

return 0;

}

Compensation: x = 1.05, y = 2.01, t = 0.43