mirror of
https://gitee.com/peng_zhihui/Dummy-Robot
synced 2025-11-04 20:49:40 +08:00
280 lines
7.7 KiB
C++
280 lines
7.7 KiB
C++
#include <cmsis_os2.h>
|
|
#include <communication.hpp>
|
|
#include "dummy_robot.h"
|
|
|
|
inline float AbsMaxOf6(DOF6Kinematic::Joint6D_t &_joints)
|
|
{
|
|
float max = -1;
|
|
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
if (abs(_joints.a[i]) > max)
|
|
max = abs(_joints.a[i]);
|
|
}
|
|
|
|
return max;
|
|
}
|
|
|
|
DummyRobot::DummyRobot(CAN_HandleTypeDef* _hcan) :
|
|
hcan(_hcan)
|
|
{
|
|
motorAll = new CtrlStepMotor(_hcan, 0, false, 30, -180, 180);
|
|
motorJ1 = new CtrlStepMotor(_hcan, 1, true, 50, -170 - initPose.a[0], 170 - initPose.a[0]);
|
|
motorJ2 = new CtrlStepMotor(_hcan, 2, true, 30, -75 - initPose.a[1], 90 - initPose.a[1]);
|
|
motorJ3 = new CtrlStepMotor(_hcan, 3, true, 30, 35 - initPose.a[2], 180 - initPose.a[2]);
|
|
motorJ4 = new CtrlStepMotor(_hcan, 4, false, 24, -180 - initPose.a[3], 180 - initPose.a[3]);
|
|
motorJ5 = new CtrlStepMotor(_hcan, 5, true, 30, -120 - initPose.a[4], 120 - initPose.a[4]);
|
|
motorJ6 = new CtrlStepMotor(_hcan, 6, true, 50, -720 - initPose.a[5], 720 - initPose.a[5]);
|
|
hand = new DummyHand(_hcan, 7);
|
|
|
|
dof6Solver = new DOF6Kinematic(0.109f, 0.035f, 0.146f, 0.115f, 0.052f, 0.072f);
|
|
}
|
|
|
|
DummyRobot::~DummyRobot()
|
|
{
|
|
delete motorAll;
|
|
delete motorJ1;
|
|
delete motorJ2;
|
|
delete motorJ3;
|
|
delete motorJ4;
|
|
delete motorJ5;
|
|
delete motorJ6;
|
|
delete hand;
|
|
|
|
delete dof6Solver;
|
|
}
|
|
|
|
void DummyRobot::Reboot()
|
|
{
|
|
motorAll->Reboot();
|
|
}
|
|
|
|
float DummyRobot::MoveJoints(DOF6Kinematic::Joint6D_t &_joints)
|
|
{
|
|
DOF6Kinematic::Joint6D_t tmp = _joints - currentJoints;
|
|
float maxAngle = AbsMaxOf6(tmp);
|
|
float time = maxAngle / jointSpeed;
|
|
|
|
motorJ1->SetAngleWithTime(_joints.a[0] - initPose.a[0], time);
|
|
motorJ2->SetAngleWithTime(_joints.a[1] - initPose.a[1], time);
|
|
motorJ3->SetAngleWithTime(_joints.a[2] - initPose.a[2], time);
|
|
motorJ4->SetAngleWithTime(_joints.a[3] - initPose.a[3], time);
|
|
motorJ5->SetAngleWithTime(_joints.a[4] - initPose.a[4], time);
|
|
motorJ6->SetAngleWithTime(_joints.a[5] - initPose.a[5], time);
|
|
|
|
// currentJoints = _joints;
|
|
|
|
return time;
|
|
}
|
|
|
|
float DummyRobot::MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6)
|
|
{
|
|
DOF6Kinematic::Joint6D_t joints(_j1, _j2, _j3, _j4, _j5, _j6);
|
|
|
|
bool valid = true;
|
|
if (_j1 - initPose.a[0] > motorJ1->maxAngle ||
|
|
_j1 - initPose.a[0] < motorJ1->minAngle)
|
|
valid = false;
|
|
if (_j2 - initPose.a[1] > motorJ2->maxAngle ||
|
|
_j2 - initPose.a[1] < motorJ2->minAngle)
|
|
valid = false;
|
|
if (_j3 - initPose.a[2] > motorJ3->maxAngle ||
|
|
_j3 - initPose.a[2] < motorJ3->minAngle)
|
|
valid = false;
|
|
if (_j4 - initPose.a[3] > motorJ4->maxAngle ||
|
|
_j4 - initPose.a[3] < motorJ4->minAngle)
|
|
valid = false;
|
|
if (_j5 - initPose.a[4] > motorJ5->maxAngle ||
|
|
_j5 - initPose.a[4] < motorJ5->minAngle)
|
|
valid = false;
|
|
if (_j6 - initPose.a[5] > motorJ6->maxAngle ||
|
|
_j6 - initPose.a[5] < motorJ6->minAngle)
|
|
valid = false;
|
|
|
|
float time = 0;
|
|
if (valid)
|
|
time = MoveJoints(joints);
|
|
|
|
return time;
|
|
}
|
|
|
|
float DummyRobot::MoveL(float _x, float _y, float _z, float _a, float _b, float _c)
|
|
{
|
|
DOF6Kinematic::Pose6D_t pose6D(_x, _y, _z, _a, _b, _c);
|
|
DOF6Kinematic::IKSolves_t ikSolves{};
|
|
DOF6Kinematic::Joint6D_t lastJoint6D{};
|
|
|
|
dof6Solver->SolveIK(pose6D, lastJoint6D, ikSolves);
|
|
|
|
bool valid[8];
|
|
int validCnt = 0;
|
|
for (int i = 0; i < 8; i++)
|
|
{
|
|
valid[i] = true;
|
|
if (ikSolves.config[i].a[0] - initPose.a[0] > motorJ1->maxAngle ||
|
|
ikSolves.config[i].a[0] - initPose.a[0] < motorJ1->minAngle)
|
|
{
|
|
valid[i] = false;
|
|
continue;
|
|
}
|
|
if (ikSolves.config[i].a[1] - initPose.a[1] > motorJ2->maxAngle ||
|
|
ikSolves.config[i].a[1] - initPose.a[1] < motorJ2->minAngle)
|
|
{
|
|
valid[i] = false;
|
|
continue;
|
|
}
|
|
if (ikSolves.config[i].a[2] - initPose.a[2] > motorJ3->maxAngle ||
|
|
ikSolves.config[i].a[2] - initPose.a[2] < motorJ3->minAngle)
|
|
{
|
|
valid[i] = false;
|
|
continue;
|
|
}
|
|
if (ikSolves.config[i].a[3] - initPose.a[3] > motorJ4->maxAngle ||
|
|
ikSolves.config[i].a[3] - initPose.a[3] < motorJ4->minAngle)
|
|
{
|
|
valid[i] = false;
|
|
continue;
|
|
}
|
|
if (ikSolves.config[i].a[4] - initPose.a[4] > motorJ5->maxAngle ||
|
|
ikSolves.config[i].a[4] - initPose.a[4] < motorJ5->minAngle)
|
|
{
|
|
valid[i] = false;
|
|
continue;
|
|
}
|
|
if (ikSolves.config[i].a[5] - initPose.a[5] > motorJ6->maxAngle ||
|
|
ikSolves.config[i].a[5] - initPose.a[5] < motorJ6->minAngle)
|
|
{
|
|
valid[i] = false;
|
|
continue;
|
|
}
|
|
|
|
if (valid[i]) validCnt++;
|
|
}
|
|
|
|
if (validCnt)
|
|
{
|
|
float min = 1000;
|
|
uint8_t index = 0;
|
|
for (int i = 0; i < 8; i++)
|
|
{
|
|
if (valid[i])
|
|
{
|
|
for (int j = 0; j < 6; j++)
|
|
lastJoint6D.a[j] = ikSolves.config[i].a[j];
|
|
DOF6Kinematic::Joint6D_t tmp = currentJoints - lastJoint6D;
|
|
float maxAngle = AbsMaxOf6(tmp);
|
|
if (maxAngle < min)
|
|
{
|
|
min = maxAngle;
|
|
index = i;
|
|
}
|
|
}
|
|
}
|
|
|
|
return MoveJ(ikSolves.config[index].a[0], ikSolves.config[index].a[1], ikSolves.config[index].a[2],
|
|
ikSolves.config[index].a[3], ikSolves.config[index].a[4], ikSolves.config[index].a[5]);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void DummyRobot::UpdateJointPos()
|
|
{
|
|
motorJ1->UpdateAngle();
|
|
motorJ2->UpdateAngle();
|
|
motorJ3->UpdateAngle();
|
|
motorJ4->UpdateAngle();
|
|
motorJ5->UpdateAngle();
|
|
motorJ6->UpdateAngle();
|
|
}
|
|
|
|
void DummyRobot::SetJointSpeed(float _speed)
|
|
{
|
|
jointSpeed = _speed;
|
|
}
|
|
|
|
void DummyRobot::Homing(bool _alreadyCalib)
|
|
{
|
|
if (_alreadyCalib)
|
|
{
|
|
MoveJ(0, 0, 90, 0, 0, 0);
|
|
} else
|
|
{
|
|
float _angle = motorJ2->inverseDirection ? -75 : 75;
|
|
float stepMotorCnt = _angle / 360.0f * motorJ2->reduction * CtrlStepMotor::CTRL_CIRCLE_COUNT;
|
|
motorJ2->SetPositionWithTime(stepMotorCnt, 5);
|
|
|
|
_angle = motorJ3->inverseDirection ? 100 : -100; //tricky
|
|
stepMotorCnt = _angle / 360.0f * motorJ3->reduction * CtrlStepMotor::CTRL_CIRCLE_COUNT;
|
|
motorJ3->SetPositionWithTime(stepMotorCnt, 5);
|
|
|
|
osDelay(6000);
|
|
motorAll->Reboot();
|
|
}
|
|
}
|
|
|
|
void DummyRobot::Resting()
|
|
{
|
|
MoveJ(0, -75, 180, 0, 0, 0);
|
|
}
|
|
|
|
void DummyRobot::SetEnable(bool _enable)
|
|
{
|
|
motorAll->SetEnable(_enable);
|
|
hand->SetEnable(_enable);
|
|
}
|
|
|
|
DummyHand::DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id) :
|
|
nodeID(_id), hcan(_hcan)
|
|
{
|
|
txHeader =
|
|
{
|
|
.StdId = 0,
|
|
.ExtId = 0,
|
|
.IDE = CAN_ID_STD,
|
|
.RTR = CAN_RTR_DATA,
|
|
.DLC = 8,
|
|
.TransmitGlobalTime = DISABLE
|
|
};
|
|
}
|
|
|
|
void DummyHand::SetAngle(float _angle)
|
|
{
|
|
if (_angle > 30)_angle = 30;
|
|
if (_angle < 0)_angle = 0;
|
|
|
|
uint8_t mode = 0x02;
|
|
txHeader.StdId = 7 << 7 | mode;
|
|
|
|
// Float to Bytes
|
|
auto* b = (unsigned char*) &_angle;
|
|
for (int i = 0; i < 4; i++)
|
|
canBuf[i] = *(b + i);
|
|
|
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
|
}
|
|
|
|
void DummyHand::SetMaxCurrent(float _val)
|
|
{
|
|
if (_val > 1)_val = 1;
|
|
if (_val < 0)_val = 0;
|
|
|
|
uint8_t mode = 0x01;
|
|
txHeader.StdId = 7 << 7 | mode;
|
|
|
|
// Float to Bytes
|
|
auto* b = (unsigned char*) &_val;
|
|
for (int i = 0; i < 4; i++)
|
|
canBuf[i] = *(b + i);
|
|
|
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
|
}
|
|
|
|
void DummyHand::SetEnable(bool _enable)
|
|
{
|
|
if (_enable)
|
|
SetMaxCurrent(maxCurrent);
|
|
else
|
|
SetMaxCurrent(0);
|
|
}
|