33 lines
1003 B
C
33 lines
1003 B
C
#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;
|
|
}
|
|
}
|
|
|
|
|