gkd_sentry_chassis.cpp

#include "device/chassis/gkd_sentry_chassis.hpp"
#include "core/async.hpp"
#include "utils/kinematics/mecanum.hpp"
#include "device/motor/base.hpp"
#include "device/motor/dji.h"
#include "utils/utils.hpp"
 
using namespace roboctrl::device;
 
ROBOCTRL_REGISTER_CHASSIS("device.gkd_sentry_chassis.v1", roboctrl::device::gkd_sentry_chassis);
ROBOCTRL_REGISTER_CHASSIS("device.standard_mecanum_chassis.v1", roboctrl::device::gkd_sentry_chassis);
ROBOCTRL_REGISTER_CHASSIS("ctrl.standard_mecanum_chassis.v1", roboctrl::device::gkd_sentry_chassis);
 
roboctrl::awaitable<void> gkd_sentry_chassis::task()
{
    while(true){
        co_await speed_decomposition();
        co_await roboctrl::wait_for(control_time_);
    }
}
 
bool gkd_sentry_chassis::init(const gkd_sentry_chassis::info_type& info){
    left_front_motor_ = &roboctrl::get<dji_motor>(info.left_front_motor);
    right_front_motor_ = &roboctrl::get<dji_motor>(info.right_front_motor);
    left_rear_motor_ = &roboctrl::get<dji_motor>(info.left_rear_motor);
    right_rear_motor_ = &roboctrl::get<dji_motor>(info.right_rear_motor);
    control_time_ = info.control_time;
    max_rotate_speed_ = info.max_rotate_speed;
    wheel_directions_ = info.wheel_directions;
    log_info("Chassis initiated");
    roboctrl::spawn(task());
    return true;
}
 
roboctrl::awaitable<void> gkd_sentry_chassis::speed_decomposition(){
    if (!enabled_) {
        co_await left_front_motor_->set(0.0f);
        co_await right_front_motor_->set(0.0f);
        co_await left_rear_motor_->set(0.0f);
        co_await right_rear_motor_->set(0.0f);
        co_return;
    }
 
    const auto wheels = utils::kinematics::mecanum_motor_targets(
        velocity_, rotate_speed_, max_wheel_speed_, wheel_directions_);
 
    log_debug("left_front_motor : {}",wheels[0]);
    log_debug("right_front_motor : {}",wheels[1]);
    log_debug("left_rear_motor : {}",wheels[2]);
    log_debug("right_rear_motor : {}",wheels[3]);
 
    co_await left_front_motor_->set(wheels[0]);
    co_await right_front_motor_->set(wheels[1]);
    co_await left_rear_motor_->set(wheels[2]);
    co_await right_rear_motor_->set(wheels[3]);
}