#pragma once
#include "main.h"
#include "component/kinematics.h"
#include "device/motor_dm.h"
#include "device/motor_lz.h"
#include "component/pid.h"

#define ga 9.81
#define __dm_handle(handle) (MOTOR_DM_t *)handle
#define __lz_handle(handle) (MOTOR_LZ_t *)handle
#define JOINT_NUM 6


typedef enum{
  DM_MOTOR=0,
  LZ_MOTOR
}arm_motor_type;



typedef struct{
  enum{
    set_relax_mode=0,
    set_follow_mode,
    start_storage,
    start_take
  }cmd;
  uint8_t cmd_prepare;

  float ref_angle[JOINT_NUM];
}arm_rx_data_t;

//关节结构体
typedef struct {
  struct{
    arm_motor_type type;
    void* handle;
  }motor;//执行器

  struct{
    float velocity;
    float position;
    float torque;
  }ref;//设定值

  struct {
    KPID_t Ppid;
    KPID_t Vpid;
    path_param_t path;
  }controler;//控制器

  struct {
    float angle_limit[2];
    float last_angle;
    int circles;
    float last_control_time;
  }status;//关节状态

  struct {
    float velocity;
    float position;
  }feedback;//反馈

}joint_t;

typedef struct{
  struct {
    struct {
      union{
      MOTOR_DM_Param_t dm_param;
      MOTOR_LZ_Param_t lz_param;
      }param;
      arm_motor_type type;
    }motor;
    struct{
      KPID_Params_t Ppid;
      KPID_Params_t Vpid;
    }pid;
    float angle_limit[2];
  }joint[JOINT_NUM];
  DH_parameter_t dh_param[6];
  float mass[6];
}arm_param_t;


//机械臂结构体
typedef struct hand_t
{
  float mass[6];
  robot_t kine;
  joint_t joint[JOINT_NUM];
  struct {
    uint8_t cmd_prepare;
    uint8_t busy;
    uint8_t step;
    uint8_t path_finish;
  }status;
  enum{
    RELAX_MODE=0,
    FOLLOW_MODE,
    STORAGE_MODE,
    TAKE_MODE
  }mode;

  

}arm_t;

