mirror of
https://gitee.com/peng_zhihui/Dummy-Robot
synced 2025-11-04 20:49:40 +08:00
107 lines
3.5 KiB
C++
107 lines
3.5 KiB
C++
#ifndef REF_STM32F4_FW_DUMMY_ROBOT_H
|
|
#define REF_STM32F4_FW_DUMMY_ROBOT_H
|
|
|
|
#include "algorithms/kinematic/6dof_kinematic.h"
|
|
#include "actuators/ctrl_step/ctrl_step.hpp"
|
|
|
|
class DummyHand
|
|
{
|
|
private:
|
|
CAN_HandleTypeDef* hcan;
|
|
uint8_t canBuf[8];
|
|
CAN_TxHeaderTypeDef txHeader;
|
|
|
|
float minAngle = 0;
|
|
float maxAngle = 45;
|
|
|
|
public:
|
|
uint8_t nodeID = 7;
|
|
float maxCurrent = 0.7;
|
|
|
|
DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id);
|
|
|
|
void SetAngle(float _angle);
|
|
void SetMaxCurrent(float _val);
|
|
void SetEnable(bool _enable);
|
|
|
|
// Communication protocol definitions
|
|
auto MakeProtocolDefinitions()
|
|
{
|
|
return make_protocol_member_list(
|
|
make_protocol_function("set_angle", *this, &DummyHand::SetAngle, "angle"),
|
|
make_protocol_function("set_enable", *this, &DummyHand::SetEnable, "enable"),
|
|
make_protocol_function("set_current_limit", *this, &DummyHand::SetMaxCurrent, "current")
|
|
);
|
|
}
|
|
};
|
|
|
|
class DummyRobot
|
|
{
|
|
private:
|
|
CAN_HandleTypeDef* hcan;
|
|
|
|
float jointSpeed = 20;
|
|
float endpointSpeed;
|
|
|
|
DOF6Kinematic* dof6Solver;
|
|
public:
|
|
explicit DummyRobot(CAN_HandleTypeDef* _hcan);
|
|
~DummyRobot();
|
|
|
|
DOF6Kinematic::Joint6D_t currentJoints = {0, 0, 90, 0, 0, 0};
|
|
DOF6Kinematic::Joint6D_t initPose = {0, 0, 90, 0, 0, 0};
|
|
|
|
CtrlStepMotor* motorAll;
|
|
CtrlStepMotor* motorJ1;
|
|
CtrlStepMotor* motorJ2;
|
|
CtrlStepMotor* motorJ3;
|
|
CtrlStepMotor* motorJ4;
|
|
CtrlStepMotor* motorJ5;
|
|
CtrlStepMotor* motorJ6;
|
|
DummyHand* hand;
|
|
|
|
float MoveJoints(DOF6Kinematic::Joint6D_t &_joints);
|
|
|
|
|
|
// Unit is degree/s
|
|
float MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6);
|
|
// Unit is mm/s
|
|
float MoveL(float _x, float _y, float _z, float _a, float _b, float _c);
|
|
|
|
void SetJointSpeed(float _speed);
|
|
|
|
void UpdateJointPos();
|
|
|
|
void Reboot();
|
|
void SetEnable(bool _enable);
|
|
void Homing(bool _alreadyCalib = false);
|
|
void Resting();
|
|
|
|
// Communication protocol definitions
|
|
auto MakeProtocolDefinitions()
|
|
{
|
|
return make_protocol_member_list(
|
|
make_protocol_function("update_joint_pos", *this, &DummyRobot::UpdateJointPos),
|
|
make_protocol_function("homing", *this, &DummyRobot::Homing, "already_pose_L"),
|
|
make_protocol_function("resting", *this, &DummyRobot::Resting),
|
|
make_protocol_object("joint_1", motorJ1->MakeProtocolDefinitions()),
|
|
make_protocol_object("joint_2", motorJ2->MakeProtocolDefinitions()),
|
|
make_protocol_object("joint_3", motorJ3->MakeProtocolDefinitions()),
|
|
make_protocol_object("joint_4", motorJ4->MakeProtocolDefinitions()),
|
|
make_protocol_object("joint_5", motorJ5->MakeProtocolDefinitions()),
|
|
make_protocol_object("joint_6", motorJ6->MakeProtocolDefinitions()),
|
|
make_protocol_object("joint_all", motorAll->MakeProtocolDefinitions()),
|
|
make_protocol_object("hand", hand->MakeProtocolDefinitions()),
|
|
make_protocol_function("reboot", *this, &DummyRobot::Reboot),
|
|
make_protocol_function("set_enable", *this, &DummyRobot::SetEnable, "enable"),
|
|
make_protocol_function("move_j", *this, &DummyRobot::MoveJ, "j1", "j2", "j3",
|
|
"j4", "j5", "j6"),
|
|
make_protocol_function("move_l", *this, &DummyRobot::MoveL, "x", "y", "z",
|
|
"a", "b", "c"),
|
|
make_protocol_function("set_joint_speed", *this, &DummyRobot::SetJointSpeed, "speed")
|
|
);
|
|
}
|
|
};
|
|
|
|
#endif //REF_STM32F4_FW_DUMMY_ROBOT_H
|