#pragma once

#include "main.h"
#include "bsp/can.h"
#include "device/motor.h"
#include "device/motor_lz.h"
#include "device/motor_rm.h"
#include "device/motor_dm.h"
#include "component/pid.h"

class Arm {
public:
    Arm();
    void init();
    void operator()();
private:
    void read_data();
    void calc_joint_angle();
    void calc_motor_angle();
    void send_control();
    void arm_relax();
    void arm_enable();
    void calc_pid();
    float joint_angle[6] = {0}; //关节角度
    // float motor_angle[6] = {0}; //电机角度
    MOTOR_DM_Param_t dm_motor_params[3];
    MOTOR_DM_t arm_dm_motor[3];
    MOTOR_LZ_Param_t lz_motor_params[3];
    MOTOR_LZ_t arm_lz_motor[3];
    MOTOR_LZ_MotionParam_t arm_lz_output[3];
    MOTOR_MIT_Output_t arm_dm_output[3];
    KPID_Params_t pid_params[7];
    KPID_Params_t pid_v_params[7];
    float output[6] = {0};
    KPID_t pid[6];
    KPID_t pid_v[6];
    float current_pos[6];
    float current_vel[6];
    uint64_t time = 0;
    uint64_t last_time = 0;
    uint64_t dt = 0;
    MOTOR_RM_Param_t motor_dji_param;
    MOTOR_RM_t arm_motor_dji;
};