#include "controler.h"
#include "device/encoder_oid.h"
#include "module/config.h"



void arm_init(arm_t* arm,Config_RobotParam_t* cfg)
{
  assert_param(!(arm==NULL||cfg==NULL));
  for(int i =0;i<JOINT_NUM;i++)
  {
    ECD_OID_Register(&cfg->arm_param.joint[i].sensor.param);
    arm->joint[i].sensor.encoder = ECD_OID_GetEncoder(&cfg->arm_param.joint[i].sensor.param);
    Encoder_Set_Mode(&cfg->arm_param.joint[i].sensor.param , ECD_OID_MODE_QUERY);//询问模式
  }
}


void arm_data_updata(arm_t* arm)
{
  assert_param(!(arm==NULL));
  for (int i=0; i<JOINT_NUM; i++) {
    Encoder_Read_AngularVelocity(&arm->joint[i].sensor.encoder->param);
		ECD_OID_Update(&arm->joint[i].sensor.encoder->param);
		Encoder_Read_Value(&arm->joint[i].sensor.encoder->param);
		ECD_OID_Update(&arm->joint[i].sensor.encoder->param);
    arm->joint[i].feedback.position = arm->joint[i].sensor.encoder->feedback.angle_2PI;
    arm->joint[i].feedback.velocity = arm->joint[i].sensor.encoder->feedback.speed_rpm;
  }
}