#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;
}
【无标题】
于 2025-01-07 14:08:36 首次发布