【无标题】

#include <iostream>
#include <vector>
#include <deque>
#include <numeric> // for std::accumulate
#include <algorithm> // for std::copy_if

enum Dir {
    LEFT,
    RIGHT,
    UP,
    DOWN
};

struct Compen {
    long lPadIndex;
    unsigned short usArmId; // 总共两个机械臂,0或1
    Dir dir;
    double X, Y, T; // 偏差值
    double dCompensation; // 补偿值
};

// 辅助函数,用于计算指定机械臂ID的最近n个补偿值和偏差值的平均值
// 如果按方向补偿开关关闭,则忽略方向参数
double calculateCompensation(const std::vector<Compen>& data, unsigned short armId, Dir dir, int n, bool useDirection) {
    double sum = 0.0;
    int count = 0;
    std::deque<Compen> filteredData;

    // 如果使用方向补偿,则只考虑相同方向的数据
    if (useDirection) {
        for (const auto& compen : data) {
            if (compen.usArmId == armId && compen.dir == dir && count < n) {
                filteredData.push_back(compen);
                count++;
            }
        }
    } else {
        // 如果不使用方向补偿,则考虑最近n个数据(不考虑方向)
        if (data.size() >= n) {
            filteredData.assign(data.end() - n, data.end());
            count = n;
        } else {
            filteredData.assign(data.begin(), data.end());
            count = data.size();
        }
    }

    // 计算平均值
    for (const auto& compen : filteredData) {
        sum += (compen.dCompensation + compen.X + compen.Y + compen.T);
    }

    return count == 0 ? 0.0 : sum / count;
}

int main() {
    int n = 5; // 滑动平均的窗口大小
    std::vector<Compen> WorkingData[2]; // 存储两个机械臂的数据
    bool useDirectionCompensation = true; // 是否按方向补偿的开关

    // 示例数据添加(在实际应用中,这些数据将来自机械臂的传感器或作业记录)
    // ... 添加数据到WorkingData中

    // 假设我们现在要为机械臂0进行作业
    unsigned short armId = 0;
    Dir currentDir = LEFT;

    // 计算补偿值
    double newCompensation = calculateCompensation(WorkingData[armId], armId, currentDir, n, useDirectionCompensation);

    // 假设我们有一个函数来移动机械臂到指定位置(位置+补偿值)
    // moveArmToPosition(armId, targetPosition + newCompensation);

    // 假设作业后我们得到了新的偏差值
    double newX = 0.15, newY = 0.25, newT = 0.35;

    // 创建一个新的Compen对象来存储这次作业的数据
    Compen newEntry = {WorkingData[armId].size() + 1, armId, currentDir, newX, newY, newT, newCompensation};

    // 将新数据添加到对应机械臂的数据集中
    WorkingData[armId].push_back(newEntry);

    // 输出新添加的补偿值和数据,用于验证
    std::cout << "New Compensation for Arm " << armId << " (Use Direction: " << (useDirectionCompensation ? "Yes" : "No") << "): " << newCompensation << std::endl;
    std::cout << "New Entry: " << newEntry.lPadIndex << ", " << newEntry.usArmId << ", " 
              << (newEntry.dir == LEFT ? "LEFT" : (newEntry.dir == RIGHT ? "RIGHT" : (newEntry.dir == UP ? "UP" : "DOWN"))) 
              << ", " << newEntry.X << ", " << newEntry.Y << ", " << newEntry.T << ", " << newEntry.dCompensation << std::endl;

    // 更改补偿开关并重新计算补偿值(可选,仅用于演示)
    useDirectionCompensation = false;
    newCompensation = calculateCompensation(WorkingData[armId], armId, currentDir, n, useDirectionCompensation);
    std::cout << "New Compensation for Arm " << armId << " (Use Direction: " << (useDirectionCompensation ? "Yes" : "No") << "): " << newCompensation << std::endl;

    return 0;
}

#include <iostream>
#include <vector>
#include <deque>
#include <numeric>
#include <algorithm>
#include <cassert>

enum Dir {
    LEFT,
    RIGHT,
    UP,
    DOWN
};

struct Compen {
    long lPadIndex;
    unsigned short usArmId;
    Dir dir;
    double X, Y, T;
    double dCompensation;
};

class CompensationCalculator {
public:
    CompensationCalculator(int windowSize, bool useDirectionCompensation)
        : n(windowSize), useDirectionCompensation(useDirectionCompensation) {}

    double calculateCompensation(const std::vector<Compen>& data, unsigned short armId, Dir dir) {
        if (useDirectionCompensation) {
            return calculateWithDirection(data, armId, dir);
        } else {
            return calculateWithoutDirection(data);
        }
    }

    void addData(unsigned short armId, const Compen& newEntry) {
        workingData[armId].push_back(newEntry);
    }

private:
    int n; // 滑动平均窗口大小
    bool useDirectionCompensation;
    std::vector<Compen> workingData[2]; // 存储两个机械臂的数据

    double calculateWithDirection(const std::vector<Compen>& data, unsigned short armId, Dir dir) {
        double sum = 0.0;
        int count = 0;
        for (const auto& compen : data) {
            if (compen.usArmId == armId && compen.dir == dir && count < n) {
                sum += (compen.dCompensation + compen.X + compen.Y + compen.T);
                count++;
            }
        }
        return count == 0 ? 0.0 : sum / count;
    }

    double calculateWithoutDirection(const std::vector<Compen>& data) {
        double sum = 0.0;
        int count = std::min(n, static_cast<int>(data.size()));
        for (int i = 0; i < count; ++i) {
            sum += (data[data.size() - count + i].dCompensation +
                    data[data.size() - count + i].X +
                    data[data.size() - count + i].Y +
                    data[data.size() - count + i].T);
        }
        return count == 0 ? 0.0 : sum / count;
    }
};

int main() {
    CompensationCalculator calculator(5, true); // 设置窗口大小为5,启用方向补偿

    // 假设有一些数据
    std::vector<Compen> arm0Data = {
        {1, 0, LEFT, 0.1, 0.2, 0.3, 0.4},
        {2, 0, LEFT, 0.2, 0.3, 0.4, 0.5},
        {3, 0, LEFT, 0.3, 0.4, 0.5, 0.6}
    };

    // 为机械臂0添加数据
    for (const auto& data : arm0Data) {
        calculator.addData(0, data);
    }

    // 计算补偿值
    unsigned short armId = 0;
    Dir currentDir = LEFT;
    double newCompensation = calculator.calculateCompensation(calculator.workingData[armId], armId, currentDir);

    // 输出新补偿值
    std::cout << "New Compensation for Arm " << armId << ": " << newCompensation << std::endl;

    // 添加新的补偿数据
    Compen newEntry = {4, armId, currentDir, 0.4, 0.5, 0.6, newCompensation};
    calculator.addData(armId, newEntry);

    // 输出新的补偿值和数据
    std::cout << "New Entry: " << newEntry.lPadIndex << ", " << newEntry.usArmId << ", "
              << (newEntry.dir == LEFT ? "LEFT" : "RIGHT") << ", " << newEntry.X << ", "
              << newEntry.Y << ", " << newEntry.T << ", " << newEntry.dCompensation << std::endl;

    return 0;
}
#include <iostream>
#include <vector>
#include <deque>
#include <numeric>
#include <algorithm>
#include <cassert>

enum Dir {
    LEFT,
    RIGHT,
    UP,
    DOWN
};

struct Compen {
    long lPadIndex;
    unsigned short usArmId;
    Dir dir;
    double X, Y, T;
    double dCompensation;
};

class CompensationCalculator {
public:
    CompensationCalculator(int windowSize, bool useDirectionCompensation)
        : n(windowSize), useDirectionCompensation(useDirectionCompensation) {
        // 为每个方向初始化一个 deque 存储补偿数据
        for (int i = 0; i < 4; ++i) {
            directionData[i] = std::deque<Compen>();
        }
    }

    double calculateCompensation(unsigned short armId, Dir dir) {
        if (useDirectionCompensation) {
            // 如果该方向的数据量小于n,则不按方向补偿
            if (directionData[dir].size() < n) {
                std::cout << "Not enough data for direction compensation, falling back to global compensation." << std::endl;
                return calculateWithoutDirection(armId); // 方向数据不足,转为全局补偿
            }
            return calculateWithDirection(armId, dir);
        } else {
            return calculateWithoutDirection(armId);
        }
    }

    void addData(unsigned short armId, const Compen& newEntry) {
        // 按方向将数据添加到对应的 deque
        directionData[newEntry.dir].push_back(newEntry);
        // 如果该方向的数据超过窗口大小,删除最旧的元素
        if (directionData[newEntry.dir].size() > n) {
            directionData[newEntry.dir].pop_front();
        }
    }

private:
    int n; // 滑动平均窗口大小
    bool useDirectionCompensation;
    std::vector<Compen> workingData[2]; // 存储两个机械臂的数据
    std::deque<Compen> directionData[4]; // 每个方向对应一个 deque

    double calculateWithDirection(unsigned short armId, Dir dir) {
        double sum = 0.0;
        int count = directionData[dir].size();
        for (const auto& compen : directionData[dir]) {
            if (compen.usArmId == armId) {
                sum += (compen.dCompensation + compen.X + compen.Y + compen.T);
            }
        }
        return count == 0 ? 0.0 : sum / count;
    }

    double calculateWithoutDirection(unsigned short armId) {
        double sum = 0.0;
        const auto& data = workingData[armId];
        int count = std::min(n, static_cast<int>(data.size()));
        if (count < n) {
            return 0.0; // 如果数据不足,补偿值为0
        }
        for (int i = 0; i < count; ++i) {
            sum += (data[data.size() - count + i].dCompensation +
                    data[data.size() - count + i].X +
                    data[data.size() - count + i].Y +
                    data[data.size() - count + i].T);
        }
        return count == 0 ? 0.0 : sum / count;
    }
};

int main() {
    CompensationCalculator calculator(5, true); // 设置窗口大小为5,启用方向补偿

    // 假设有一些数据
    std::vector<Compen> arm0Data = {
        {1, 0, LEFT, 0.1, 0.2, 0.3, 0.4},
        {2, 0, LEFT, 0.2, 0.3, 0.4, 0.5},
        {3, 0, LEFT, 0.3, 0.4, 0.5, 0.6}
    };

    // 为机械臂0添加数据
    for (const auto& data : arm0Data) {
        calculator.addData(0, data);
    }

    // 计算补偿值
    unsigned short armId = 0;
    Dir currentDir = LEFT;
    double newCompensation = calculator.calculateCompensation(armId, currentDir);

    // 输出新补偿值
    std::cout << "New Compensation for Arm " << armId << ": " << newCompensation << std::endl;

    // 添加新的补偿数据
    Compen newEntry = {4, armId, currentDir, 0.4, 0.5, 0.6, newCompensation};
    calculator.addData(armId, newEntry);

    // 输出新的补偿值和数据
    std::cout << "New Entry: " << newEntry.lPadIndex << ", " << newEntry.usArmId << ", "
              << (newEntry.dir == LEFT ? "LEFT" : "RIGHT") << ", " << newEntry.X << ", "
              << newEntry.Y << ", " << newEntry.T << ", " << newEntry.dCompensation << std::endl;

    return 0;
}
#include <iostream>
#include <vector>
#include <deque>
#include <numeric>
#include <algorithm>
#include <cassert>

enum Dir {
    LEFT,
    RIGHT,
    UP,
    DOWN
};

struct Compen {
    long lPadIndex;
    unsigned short usArmId;
    Dir dir;
    double X, Y, T;
    double dCompensation;
};

class CompensationCalculator {
public:
    CompensationCalculator(int windowSize, bool useDirectionCompensation)
        : n(windowSize), useDirectionCompensation(useDirectionCompensation) {
        // 为每个方向初始化一个 deque 存储补偿数据
        for (int i = 0; i < 4; ++i) {
            directionData[i] = std::deque<Compen>();
        }
    }

    double calculateCompensation(unsigned short armId, Dir dir) {
        if (useDirectionCompensation) {
            // 如果该方向的数据量小于n,则不按方向补偿
            if (directionData[dir].size() < n) {
                std::cout << "Not enough data for direction compensation, falling back to global compensation." << std::endl;
                return calculateWithoutDirection(armId); // 方向数据不足,转为全局补偿
            }
            return calculateWithDirection(armId, dir);
        } else {
            return calculateWithoutDirection(armId);
        }
    }

    void addData(unsigned short armId, const Compen& newEntry) {
        // 按方向将数据添加到对应的 deque
        directionData[newEntry.dir].push_back(newEntry);
        // 如果该方向的数据超过窗口大小,删除最旧的元素
        if (directionData[newEntry.dir].size() > n) {
            directionData[newEntry.dir].pop_front();
        }

        // 同时将数据添加到全局数据集中
        workingData[armId].push_back(newEntry);
        // 如果全局数据量超过 n 的限制,可以选择保留更多数据,或保持 n 条最新数据
    }

private:
    int n; // 滑动平均窗口大小
    bool useDirectionCompensation;
    std::vector<Compen> workingData[2]; // 存储两个机械臂的全局数据
    std::deque<Compen> directionData[4]; // 每个方向对应一个 deque

    double calculateWithDirection(unsigned short armId, Dir dir) {
        double sum = 0.0;
        int count = directionData[dir].size();
        for (const auto& compen : directionData[dir]) {
            if (compen.usArmId == armId) {
                sum += (compen.dCompensation + compen.X + compen.Y + compen.T);
            }
        }
        return count == 0 ? 0.0 : sum / count;
    }

    double calculateWithoutDirection(unsigned short armId) {
        double sum = 0.0;
        const auto& data = workingData[armId];
        int count = std::min(n, static_cast<int>(data.size()));
        if (count < n) {
            return 0.0; // 如果全局数据不足 n 条,返回 0
        }
        for (int i = 0; i < count; ++i) {
            sum += (data[data.size() - count + i].dCompensation +
                    data[data.size() - count + i].X +
                    data[data.size() - count + i].Y +
                    data[data.size() - count + i].T);
        }
        return count == 0 ? 0.0 : sum / count;
    }
};

int main() {
    CompensationCalculator calculator(5, true); // 设置窗口大小为5,启用方向补偿

    // 假设有一些数据
    std::vector<Compen> arm0Data = {
        {1, 0, LEFT, 0.1, 0.2, 0.3, 0.4},
        {2, 0, LEFT, 0.2, 0.3, 0.4, 0.5},
        {3, 0, LEFT, 0.3, 0.4, 0.5, 0.6}
    };

    // 为机械臂0添加数据
    for (const auto& data : arm0Data) {
        calculator.addData(0, data);
    }

    // 计算补偿值
    unsigned short armId = 0;
    Dir currentDir = LEFT;
    double newCompensation = calculator.calculateCompensation(armId, currentDir);

    // 输出新补偿值
    std::cout << "New Compensation for Arm " << armId << ": " << newCompensation << std::endl;

    // 添加新的补偿数据
    Compen newEntry = {4, armId, currentDir, 0.4, 0.5, 0.6, newCompensation};
    calculator.addData(armId, newEntry);

    // 输出新的补偿值和数据
    std::cout << "New Entry: " << newEntry.lPadIndex << ", " << newEntry.usArmId << ", "
              << (newEntry.dir == LEFT ? "LEFT" : "RIGHT") << ", " << newEntry.X << ", "
              << newEntry.Y <<
#include <iostream>
#include <vector>
#include <deque>
#include <numeric>
#include <algorithm>
#include <cassert>

enum Dir {
    LEFT,
    RIGHT,
    UP,
    DOWN
};

struct Compen {
    long lPadIndex;
    unsigned short usArmId;
    Dir dir;
    double X, Y, T;
    double dCompensation;
};

class CompensationCalculator {
public:
    CompensationCalculator(int windowSize, bool useDirectionCompensation)
        : n(windowSize), useDirectionCompensation(useDirectionCompensation) {
        // 为每个方向初始化一个 deque 存储补偿数据
        for (int i = 0; i < 4; ++i) {
            directionData[i] = std::deque<Compen>();
        }
    }

    double calculateCompensation(unsigned short armId, Dir dir) {
        if (useDirectionCompensation) {
            // 如果该方向的数据量小于n,则不按方向补偿
            if (directionData[dir].size() < n) {
                std::cout << "Not enough data for direction compensation, falling back to global compensation." << std::endl;
                return calculateWithoutDirection(armId); // 方向数据不足,转为全局补偿
            }
            return calculateWithDirection(armId, dir);
        } else {
            return calculateWithoutDirection(armId);
        }
    }

    void addData(unsigned short armId, const Compen& newEntry) {
        // 按方向将数据添加到对应的 deque
        directionData[newEntry.dir].push_back(newEntry);
        // 如果该方向的数据超过窗口大小,删除最旧的元素
        if (directionData[newEntry.dir].size() > n) {
            directionData[newEntry.dir].pop_front();
        }
    }

private:
    int n; // 滑动平均窗口大小
    bool useDirectionCompensation;
    std::vector<Compen> workingData[2]; // 存储两个机械臂的数据
    std::deque<Compen> directionData[4]; // 每个方向对应一个 deque

    double calculateWithDirection(unsigned short armId, Dir dir) {
        double sum = 0.0;
        int count = directionData[dir].size();
        for (const auto& compen : directionData[dir]) {
            if (compen.usArmId == armId) {
                sum += (compen.dCompensation + compen.X + compen.Y + compen.T);
            }
        }
        return count == 0 ? 0.0 : sum / count;
    }

    double calculateWithoutDirection(unsigned short armId) {
        double sum = 0.0;
        const auto& data = workingData[armId];
        int count = std::min(n, static_cast<int>(data.size()));
        if (count < n) {
            return 0.0; // 如果数据不足,补偿值为0
        }
        for (int i = 0; i < count; ++i) {
            sum += (data[data.size() - count + i].dCompensation +
                    data[data.size() - count + i].X +
                    data[data.size() - count + i].Y +
                    data[data.size() - count + i].T);
        }
        return count == 0 ? 0.0 : sum / count;
    }
};

int main() {
    CompensationCalculator calculator(5, true); // 设置窗口大小为5,启用方向补偿

    // 假设有一些数据
    std::vector<Compen> arm0Data = {
        {1, 0, LEFT, 0.1, 0.2, 0.3, 0.4},
        {2, 0, LEFT, 0.2, 0.3, 0.4, 0.5},
        {3, 0, LEFT, 0.3, 0.4, 0.5, 0.6}
    };

    // 为机械臂0添加数据
    for (const auto& data : arm0Data) {
        calculator.addData(0, data);
    }

    // 计算补偿值
    unsigned short armId = 0;
    Dir currentDir = LEFT;
    double newCompensation = calculator.calculateCompensation(armId, currentDir);

    // 输出新补偿值
    std::cout << "New Compensation for Arm " << armId << ": " << newCompensation << std::endl;

    // 添加新的补偿数据
    Compen newEntry = {4, armId, currentDir, 0.4, 0.5, 0.6, newCompensation};
    calculator.addData(armId, newEntry);

    // 输出新的补偿值和数据
    std::cout << "New Entry: " << newEntry.lPadIndex << ", " << newEntry.usArmId << ", "
              << (newEntry.dir == LEFT ? "LEFT" : "RIGHT") << ", " << newEntry.X << ", "
              << newEntry.Y << ", " << newEntry.T << ", " << newEntry.dCompensation << std::endl;

    return 0;
}

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值