131 lines
2.9 KiB
C++
Executable File
131 lines
2.9 KiB
C++
Executable File
#include "Arm.h"
|
|
#include <iostream>
|
|
#include <numbers>
|
|
#include <algorithm>
|
|
#include <ranges>
|
|
|
|
Arm::Arm(const std::wstring& comPort) noexcept:
|
|
oSerialBus(comPort),
|
|
joints{
|
|
// bus axis ref len idx min max home parent
|
|
ArmSegment(oSerialBus, Vector3d::Y, Vector3d::Forward, 0.0, 0, -135.0, 45.0, 0.0, nullptr),
|
|
ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 72.0, 1, -38.0, 85.0, 0.0, nullptr),
|
|
ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 97.0, 2, -86.0, 89.0, 0.0, nullptr),
|
|
ArmSegment(oSerialBus, Vector3d::X, Vector3d::Up, 114.0, 3, -85.0, 87.0, 00.0, nullptr),
|
|
ArmSegment(oSerialBus, Vector3d::Y, Vector3d::Forward, 0.0, 4, -85.0, 87.0, 00.0, nullptr),
|
|
ArmSegment(oSerialBus, -Vector3d::X, Vector3d::Up, 56.0, 5, -85.0, 87.0, 0.0, nullptr)
|
|
}
|
|
{
|
|
if (!oSerialBus.isOpen()) {
|
|
std::cerr << "Serial bus is not open! Is the COM port busy?" << std::endl;
|
|
exit(-1);
|
|
}
|
|
|
|
// Assign parents
|
|
ArmSegment* parent = &joints[0];
|
|
for (auto& joint : joints | std::views::drop(1)) {
|
|
joint.setParent(parent);
|
|
parent = &joint;
|
|
}
|
|
}
|
|
|
|
bool Arm::assumeHomePose()
|
|
{
|
|
for (auto& joint : joints) {
|
|
if (!joint.goHome(0.5)) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void Arm::stagePose(
|
|
const std::array<double, NUM_JOINTS>& pose
|
|
) noexcept
|
|
{
|
|
for (std::size_t i = 0; i < NUM_JOINTS; i++) {
|
|
joints[i].stageAngle(pose[i]);
|
|
}
|
|
}
|
|
|
|
void Arm::commitPose() noexcept
|
|
{
|
|
for (auto& joint : joints) {
|
|
if (joint.isStaged()) {
|
|
joint.commitStaging();
|
|
}
|
|
}
|
|
}
|
|
|
|
void Arm::rollbackPose() noexcept
|
|
{
|
|
for (auto& joint : joints) {
|
|
if (joint.isStaged()) {
|
|
joint.rollbackStaging();
|
|
}
|
|
}
|
|
}
|
|
|
|
bool Arm::isMoving() const noexcept
|
|
{
|
|
for (auto& joint : joints) {
|
|
if (joint.isMoving()) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool Arm::checkJointCollisions(std::span<const AxisAlignedBoundingBox> deadzones) const noexcept
|
|
{
|
|
// TODO: Perform this check within the FK loop to prevent redundant operations
|
|
// If any deadzone intersects with the EF pos, fai
|
|
for (const AxisAlignedBoundingBox& aabb : deadzones) {
|
|
|
|
for (auto& joint : joints) {
|
|
if (aabb.doesIntersect(joint.getTargetedGlobalEndpoint())) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool Arm::rollbackJointsBasedOnValidRange() noexcept
|
|
{
|
|
bool allGood = true;
|
|
|
|
for (auto& joint : joints) {
|
|
if (!joint.isPositionInRange(joint.getTargetPosition())) {
|
|
joint.rollbackStaging();
|
|
allGood = false;
|
|
}
|
|
}
|
|
|
|
return allGood;
|
|
}
|
|
|
|
Arm::~Arm()
|
|
{
|
|
oSerialBus.close();
|
|
}
|
|
|
|
bool Arm::assumeHomePoseImmediate()
|
|
{
|
|
for (auto& joint : joints) {
|
|
if (!joint.goHomeImmediate()) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void Arm::update(double frametime)
|
|
{
|
|
for (auto& joint : joints) {
|
|
joint.update(frametime);
|
|
}
|
|
}
|