-
Notifications
You must be signed in to change notification settings - Fork 3
/
tmotor_ak_actuators.hpp
102 lines (82 loc) · 2.61 KB
/
tmotor_ak_actuators.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
/// @file tmotor_ak_actuators.hpp
/// @brief T-Motor/CubeMars AK series actuators library header file.
/// @author ZiTe (honmonoh@gmail.com)
#ifndef TMOTOR_AK_ACTUATORS_HPP
#define TMOTOR_AK_ACTUATORS_HPP
#include <stdint.h>
/// @brief T-Motor/CubMars actuators.
namespace TMotorActuators
{
typedef struct
{
float positionMin;
float positionMax;
float velocityMin;
float velocityMax;
float torqueMin;
float torqueMax;
float kpMin;
float kpMax;
float kdMin;
float kdMax;
} MotorParameters;
/// @brief AK10-9 V1.1
const MotorParameters ak10_9_v1_1 = {-12.5,
12.5,
-50,
50,
-65,
65,
0,
500,
0,
5};
/// @brief AK series actuators/motor.
class AkActuators
{
public:
typedef void (*SendCanDataFunction)(uint32_t id, uint8_t dlc, uint8_t *data);
typedef struct
{
uint16_t id;
float position;
float velocity;
float torque;
} MotorState;
private:
/// @brief Motor ID, same as CAN ID.
uint8_t mId;
MotorParameters mMotorParameters;
/// @brief Send CAN Bus data function.
SendCanDataFunction sendCanData;
int32_t convFloatToUint(float val, float min, float max, uint8_t bits);
float convUintToFloat(int32_t val, float min, float max, uint8_t bits);
public:
/// @brief Construct a new AK actuators object.
///
/// @param motorId Motor ID, same as CAN ID.
/// @param motorParas Motor parameters.
/// @param canSendFunc CAN Bus send data function.
AkActuators(uint8_t motorId, MotorParameters motorParas, SendCanDataFunction canSendFunc);
/// @brief Enter motor control mode.
void enable(void);
/// @brief Exit motor control mode.
void disable(void);
/// @brief Set the current position of the motor to 0.
void setZeroPosition(void);
/// @brief Send command to contol the motor.
///
/// @param position Goal position in rad.
/// @param velocity Goal speed in rad/s.
/// @param torque Goal torque in Nm.
/// @param kp PID kp.
/// @param kd PID kd.
void move(float position, float velocity, float torque, float kp, float kd = 0);
MotorState parseMotorState(uint8_t *canData);
/// @brief Get the motor ID.
///
/// @return Motor ID
uint8_t getId(void);
};
}
#endif // TMOTOR_AK_ACTUATORS_HPP