47 lines
1.8 KiB
C++
47 lines
1.8 KiB
C++
#include "include/Encoders.hpp"
|
|
#include "include/spi.hpp"
|
|
#include "include/mathUtils.hpp"
|
|
#include <spdlog/spdlog.h>
|
|
|
|
std::array<int32_t, ROBOT_MOTOR_COUNT> Encoders::get_positions() {
|
|
if (position_cache.is_expired()) {
|
|
uint8_t data[ROBOT_MOTOR_COUNT * 4] = {0};
|
|
Spi::getInstance().read_array(Spi::MOTOR_1_POS_B3, 4 * ROBOT_MOTOR_COUNT, data);
|
|
for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
|
|
cached_positions.at(i) = mathUtils::from_bytes<int32_t>(data + i * 4, 4);
|
|
}
|
|
position_cache.update();
|
|
}
|
|
return cached_positions;
|
|
}
|
|
|
|
std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_rad_s() {
|
|
if (velocity_cache.is_expired()) {
|
|
uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0};
|
|
Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data);
|
|
for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
|
|
auto velocity_ticks_s = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
|
|
auto velocity_rot_s = velocity_ticks_s / ROBOT_TICKS_PER_TURN;
|
|
auto velocity_rad_s = velocity_rot_s * 2 * M_PI;
|
|
|
|
auto change = velocity_rad_s - cached_velocities_rad_s.at(i);
|
|
change = mathUtils::limit_max(change, -ROBOT_ENCODER_MAX_CHANGE_RAD_S, ROBOT_ENCODER_MAX_CHANGE_RAD_S);
|
|
cached_velocities_rad_s.at(i) = cached_velocities_rad_s.at(i) + change;
|
|
}
|
|
velocity_cache.update();
|
|
}
|
|
return cached_velocities_rad_s;
|
|
}
|
|
|
|
std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_ticks_s() {
|
|
if (ticks_cache.is_expired()) {
|
|
uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0};
|
|
Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data);
|
|
for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
|
|
auto velocity_ticks_s = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
|
|
cached_velocities_ticks_s.at(i) = velocity_ticks_s;
|
|
}
|
|
ticks_cache.update();
|
|
}
|
|
return cached_velocities_ticks_s;
|
|
}
|