Dummy-Robot/2.Firmware/Core-STM32F4-fw/Robot/instances/dummy_robot.cpp
2022-02-09 11:52:03 +08:00

547 lines
15 KiB
C++

#include "communication.hpp"
#include "dummy_robot.h"
inline float AbsMaxOf6(DOF6Kinematic::Joint6D_t _joints, uint8_t &_index)
{
float max = -1;
for (uint8_t i = 0; i < 6; i++)
{
if (abs(_joints.a[i]) > max)
{
max = abs(_joints.a[i]);
_index = i;
}
}
return max;
}
DummyRobot::DummyRobot(CAN_HandleTypeDef* _hcan) :
hcan(_hcan)
{
motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180);
motorJ[1] = new CtrlStepMotor(_hcan, 1, true, 50, -170, 170);
motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 30, -73, 90);
motorJ[3] = new CtrlStepMotor(_hcan, 3, true, 30, 35, 180);
motorJ[4] = new CtrlStepMotor(_hcan, 4, false, 24, -180, 180);
motorJ[5] = new CtrlStepMotor(_hcan, 5, true, 30, -120, 120);
motorJ[6] = new CtrlStepMotor(_hcan, 6, true, 50, -720, 720);
hand = new DummyHand(_hcan, 7);
dof6Solver = new DOF6Kinematic(0.109f, 0.035f, 0.146f, 0.115f, 0.052f, 0.072f);
}
DummyRobot::~DummyRobot()
{
for (int j = 0; j <= 6; j++)
delete motorJ[j];
delete hand;
delete dof6Solver;
}
void DummyRobot::Init()
{
SetCommandMode(DEFAULT_COMMAND_MODE);
SetJointSpeed(DEFAULT_JOINT_SPEED);
}
void DummyRobot::Reboot()
{
motorJ[ALL]->Reboot();
osDelay(500); // waiting for all joints done
HAL_NVIC_SystemReset();
}
void DummyRobot::MoveJoints(DOF6Kinematic::Joint6D_t _joints)
{
for (int j = 1; j <= 6; j++)
{
motorJ[j]->SetAngleWithVelocityLimit(_joints.a[j - 1] - initPose.a[j - 1],
dynamicJointSpeeds.a[j - 1]);
}
}
bool DummyRobot::MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6)
{
DOF6Kinematic::Joint6D_t targetJointsTmp(_j1, _j2, _j3, _j4, _j5, _j6);
bool valid = true;
for (int j = 1; j <= 6; j++)
{
if (targetJointsTmp.a[j - 1] > motorJ[j]->angleLimitMax ||
targetJointsTmp.a[j - 1] < motorJ[j]->angleLimitMin)
valid = false;
}
if (valid)
{
DOF6Kinematic::Joint6D_t deltaJoints = targetJointsTmp - currentJoints;
uint8_t index;
float maxAngle = AbsMaxOf6(deltaJoints, index);
float time = maxAngle * (float) (motorJ[index + 1]->reduction) / jointSpeed;
for (int j = 1; j <= 6; j++)
{
dynamicJointSpeeds.a[j - 1] =
abs(deltaJoints.a[j - 1] * (float) (motorJ[j]->reduction) / time * 0.1f); //0~10r/s
}
jointsStateFlag = 0;
targetJoints = targetJointsTmp;
return true;
}
return false;
}
bool 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;
for (int j = 1; j <= 6; j++)
{
if (ikSolves.config[i].a[j - 1] > motorJ[j]->angleLimitMax ||
ikSolves.config[i].a[j - 1] < motorJ[j]->angleLimitMin)
{
valid[i] = false;
continue;
}
}
if (valid[i]) validCnt++;
}
if (validCnt)
{
float min = 1000;
uint8_t indexConfig = 0, indexJoint = 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, indexJoint);
if (maxAngle < min)
{
min = maxAngle;
indexConfig = i;
}
}
}
return MoveJ(ikSolves.config[indexConfig].a[0], ikSolves.config[indexConfig].a[1],
ikSolves.config[indexConfig].a[2], ikSolves.config[indexConfig].a[3],
ikSolves.config[indexConfig].a[4], ikSolves.config[indexConfig].a[5]);
}
return false;
}
void DummyRobot::UpdateJointAngles()
{
motorJ[ALL]->UpdateAngle();
}
void DummyRobot::UpdateJointAnglesCallback()
{
for (int i = 1; i <= 6; i++)
{
currentJoints.a[i - 1] = motorJ[i]->angle + initPose.a[i - 1];
if (motorJ[i]->state == CtrlStepMotor::FINISH)
jointsStateFlag |= (1 << i);
else
jointsStateFlag &= ~(1 << i);
}
}
void DummyRobot::SetJointSpeed(float _speed)
{
if (_speed < 0)_speed = 0;
else if (_speed > 100) _speed = 100;
jointSpeed = _speed * jointSpeedRatio;
}
void DummyRobot::SetJointAcceleration(float _acc)
{
if (_acc < 0)_acc = 0;
else if (_acc > 100) _acc = 100;
for (int i = 1; i <= 6; i++)
motorJ[i]->SetAcceleration(_acc / 100 * DEFAULT_JOINT_ACCELERATION_BASES.a[i - 1]);
}
void DummyRobot::CalibrateHomeOffset()
{
// Disable FixUpdate, but not disable motors
isEnabled = false;
motorJ[ALL]->SetEnable(true);
// 1.Manually move joints to L-Pose [precisely]
// ...
motorJ[2]->SetCurrentLimit(0.5);
motorJ[3]->SetCurrentLimit(0.5);
osDelay(500);
// 2.Apply Home-Offset the first time
motorJ[ALL]->ApplyPositionAsHome();
osDelay(500);
// 3.Go to Resting-Pose
initPose = DOF6Kinematic::Joint6D_t(0, 0, 90, 0, 0, 0);
currentJoints = DOF6Kinematic::Joint6D_t(0, 0, 90, 0, 0, 0);
Resting();
osDelay(500);
// 4.Apply Home-Offset the second time
motorJ[ALL]->ApplyPositionAsHome();
osDelay(500);
motorJ[2]->SetCurrentLimit(1);
motorJ[3]->SetCurrentLimit(1);
osDelay(500);
Reboot();
}
void DummyRobot::Homing()
{
float lastSpeed = jointSpeed;
SetJointSpeed(10);
MoveJ(0, 0, 90, 0, 0, 0);
MoveJoints(targetJoints);
while (IsMoving())
osDelay(10);
SetJointSpeed(lastSpeed);
}
void DummyRobot::Resting()
{
float lastSpeed = jointSpeed;
SetJointSpeed(10);
MoveJ(REST_POSE.a[0], REST_POSE.a[1], REST_POSE.a[2],
REST_POSE.a[3], REST_POSE.a[4], REST_POSE.a[5]);
MoveJoints(targetJoints);
while (IsMoving())
osDelay(10);
SetJointSpeed(lastSpeed);
}
void DummyRobot::SetEnable(bool _enable)
{
motorJ[ALL]->SetEnable(_enable);
isEnabled = _enable;
}
void DummyRobot::UpdateJointPose6D()
{
dof6Solver->SolveFK(currentJoints, currentPose6D);
currentPose6D.X *= 1000; // m -> mm
currentPose6D.Y *= 1000; // m -> mm
currentPose6D.Z *= 1000; // m -> mm
}
bool DummyRobot::IsMoving()
{
return jointsStateFlag != 0b1111110;
}
bool DummyRobot::IsEnabled()
{
return isEnabled;
}
void DummyRobot::SetCommandMode(uint32_t _mode)
{
if (_mode < COMMAND_TARGET_POINT_SEQUENTIAL ||
_mode > COMMAND_MOTOR_TUNING)
return;
commandMode = static_cast<CommandMode>(_mode);
switch (commandMode)
{
case COMMAND_TARGET_POINT_SEQUENTIAL:
case COMMAND_TARGET_POINT_INTERRUPTABLE:
jointSpeedRatio = 1;
SetJointAcceleration(DEFAULT_JOINT_ACCELERATION_LOW);
break;
case COMMAND_CONTINUES_TRAJECTORY:
SetJointAcceleration(DEFAULT_JOINT_ACCELERATION_HIGH);
jointSpeedRatio = 0.3;
break;
case COMMAND_MOTOR_TUNING:
break;
}
}
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);
}
uint32_t DummyRobot::CommandHandler::Push(const std::string &_cmd)
{
osStatus_t status = osMessageQueuePut(commandFifo, _cmd.c_str(), 0U, 0U);
if (status == osOK)
return osMessageQueueGetSpace(commandFifo);
return 0xFF; // failed
}
void DummyRobot::CommandHandler::EmergencyStop()
{
context->MoveJ(context->currentJoints.a[0], context->currentJoints.a[1], context->currentJoints.a[2],
context->currentJoints.a[3], context->currentJoints.a[4], context->currentJoints.a[5]);
context->MoveJoints(context->targetJoints);
context->isEnabled = false;
ClearFifo();
}
std::string DummyRobot::CommandHandler::Pop(uint32_t timeout)
{
osStatus_t status = osMessageQueueGet(commandFifo, strBuffer, nullptr, timeout);
return std::string{strBuffer};
}
uint32_t DummyRobot::CommandHandler::GetSpace()
{
return osMessageQueueGetSpace(commandFifo);
}
uint32_t DummyRobot::CommandHandler::ParseCommand(const std::string &_cmd)
{
uint8_t argNum;
switch (context->commandMode)
{
case COMMAND_TARGET_POINT_SEQUENTIAL:
case COMMAND_CONTINUES_TRAJECTORY:
if (_cmd[0] == '>')
{
float joints[6];
float speed;
argNum = sscanf(_cmd.c_str(), ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2,
joints + 3, joints + 4, joints + 5, &speed);
if (argNum == 6)
{
context->MoveJ(joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]);
} else if (argNum == 7)
{
context->SetJointSpeed(speed);
context->MoveJ(joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]);
}
// Trigger a transmission immediately, in case IsMoving() returns false
context->MoveJoints(context->targetJoints);
while (context->IsMoving() && context->IsEnabled())
osDelay(5);
Respond(*usbStreamOutputPtr, "ok");
Respond(*uart4StreamOutputPtr, "ok");
} else if (_cmd[0] == '@')
{
float pose[6];
float speed;
argNum = sscanf(_cmd.c_str(), "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2,
pose + 3, pose + 4, pose + 5, &speed);
if (argNum == 6)
{
context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
} else if (argNum == 7)
{
context->SetJointSpeed(speed);
context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
}
// Trigger a transmission immediately, in case IsMoving() returns false
context->MoveJoints(context->targetJoints);
while (context->IsMoving())
osDelay(5);
Respond(*usbStreamOutputPtr, "ok");
Respond(*uart4StreamOutputPtr, "ok");
}
break;
case COMMAND_TARGET_POINT_INTERRUPTABLE:
if (_cmd[0] == '>')
{
float joints[6];
float speed;
argNum = sscanf(_cmd.c_str(), ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2,
joints + 3, joints + 4, joints + 5, &speed);
if (argNum == 6)
{
context->MoveJ(joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]);
} else if (argNum == 7)
{
context->SetJointSpeed(speed);
context->MoveJ(joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]);
}
Respond(*usbStreamOutputPtr, "ok");
Respond(*uart4StreamOutputPtr, "ok");
} else if (_cmd[0] == '@')
{
float pose[6];
float speed;
argNum = sscanf(_cmd.c_str(), "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2,
pose + 3, pose + 4, pose + 5, &speed);
if (argNum == 6)
{
context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
} else if (argNum == 7)
{
context->SetJointSpeed(speed);
context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
}
Respond(*usbStreamOutputPtr, "ok");
Respond(*uart4StreamOutputPtr, "ok");
}
break;
case COMMAND_MOTOR_TUNING:
break;
}
return osMessageQueueGetSpace(commandFifo);
}
void DummyRobot::CommandHandler::ClearFifo()
{
osMessageQueueReset(commandFifo);
}
void DummyRobot::TuningHelper::SetTuningFlag(uint8_t _flag)
{
tuningFlag = _flag;
}
void DummyRobot::TuningHelper::Tick(uint32_t _timeMillis)
{
time += PI * 2 * frequency * (float) _timeMillis / 1000.0f;
float delta = amplitude * sinf(time);
for (int i = 1; i <= 6; i++)
if (tuningFlag & (1 << (i - 1)))
context->motorJ[i]->SetAngle(delta);
}
void DummyRobot::TuningHelper::SetFreqAndAmp(float _freq, float _amp)
{
if (_freq > 5)_freq = 5;
else if (_freq < 0.1) _freq = 0.1;
if (_amp > 50)_amp = 50;
else if (_amp < 1) _amp = 1;
frequency = _freq;
amplitude = _amp;
}