71 for (
size_t i = can_->
available(); i > 0; --i) {
73 for (
size_t i = 0; i < 8; ++i) {
74 if (msg.
id == 0x201 + i) {
76 static_cast<int16_t
>(msg.
data[0] << 8 | msg.
data[1]);
77 int16_t delta = angle - prev_angle_[i];
80 }
else if (delta < -4096) {
83 position_[i] += delta;
84 prev_angle_[i] = angle;
86 rpm_[i] =
static_cast<int16_t
>(msg.
data[2] << 8 | msg.
data[3]);
87 current_[i] =
static_cast<int16_t
>(msg.
data[4] << 8 | msg.
data[5]);
97 for (
size_t i = 0; i < 4; ++i) {
98 msg.
data[i * 2] = current_target_[i] >> 8;
99 msg.
data[i * 2 + 1] = current_target_[i];
103 for (
size_t i = 0; i < 4; ++i) {
104 msg.
data[i * 2] = current_target_[i + 4] >> 8;
105 msg.
data[i * 2 + 1] = current_target_[i + 4];
130 std::array<int16_t, 8> prev_angle_ = {};
131 std::array<int64_t, 8> position_ = {};
132 std::array<int16_t, 8> rpm_ = {};
133 std::array<int16_t, 8> current_ = {};
134 std::array<int16_t, 8> current_target_ = {};
bool init(CANBase *can)
Definition c610.hpp:64
void set_position(ID id, float position)
Definition c610.hpp:116
float get_rpm(ID id)
Definition c610.hpp:110
int16_t get_current(ID id)
Definition c610.hpp:121
float get_rps(ID id)
Definition c610.hpp:112
void update()
Definition c610.hpp:69
float get_position(ID id)
Definition c610.hpp:114
void set_current(ID id, int16_t current)
Definition c610.hpp:123
Definition can_base.hpp:20
virtual bool receive(CANMessage *msg, uint32_t timeout)=0
virtual size_t available()=0
virtual bool transmit(const CANMessage *msg)=0
constexpr std::underlying_type_t< T > to_underlying(T value) noexcept
Definition utility.hpp:16
Definition can_base.hpp:13
uint8_t dlc
Definition can_base.hpp:16
uint32_t id
Definition can_base.hpp:15
uint8_t data[8]
Definition can_base.hpp:17
CANIDType id_type
Definition can_base.hpp:14