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

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