Dummy-Robot/2.Firmware/REF-STM32F4-fw/Robot/instances/dummy_robot.cpp
peng-zhihui 52c8328f51 [Fw] Add Dummy controller firmware.
Add description of Firmware.
2021-10-27 11:17:37 +08:00

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);
}