69 lines
2.7 KiB
C
69 lines
2.7 KiB
C
#include "gom_protocol.h"
|
||
#include <string.h>
|
||
|
||
#define SATURATE(_IN, _MIN, _MAX) \
|
||
{ \
|
||
if ((_IN) <= (_MIN)) \
|
||
(_IN) = (_MIN); \
|
||
else if ((_IN) >= (_MAX)) \
|
||
(_IN) = (_MAX); \
|
||
}
|
||
|
||
/// @brief <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><CDB8><EFBFBD><EFBFBD><EFBFBD>ĸ<EFBFBD><C4B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͳ<EFBFBD><CDB2><EFBFBD>
|
||
/// @param motor_s Ҫת<D2AA><D7AA><EFBFBD>ĵ<EFBFBD><C4B5>ָ<EFBFBD><D6B8>ṹ<EFBFBD><E1B9B9>
|
||
void modify_data(MotorCmd_t *motor_s)
|
||
{
|
||
motor_s->motor_send_data.head[0] = 0xFE;
|
||
motor_s->motor_send_data.head[1] = 0xEE;
|
||
|
||
SATURATE(motor_s->id, 0, 15);
|
||
SATURATE(motor_s->mode, 0, 7);
|
||
SATURATE(motor_s->K_P, 0.0f, 25.599f);
|
||
SATURATE(motor_s->K_W, 0.0f, 25.599f);
|
||
SATURATE(motor_s->T, -127.99f, 127.99f);
|
||
SATURATE(motor_s->W, -804.00f, 804.00f);
|
||
SATURATE(motor_s->Pos, -411774.0f, 411774.0f);
|
||
|
||
motor_s->motor_send_data.mode.id = motor_s->id;
|
||
motor_s->motor_send_data.mode.status = motor_s->mode;
|
||
motor_s->motor_send_data.comd.k_pos = motor_s->K_P / 25.6f * 32768.0f;
|
||
motor_s->motor_send_data.comd.k_spd = motor_s->K_W / 25.6f * 32768.0f;
|
||
motor_s->motor_send_data.comd.pos_des = motor_s->Pos / 6.28318f * 32768.0f;
|
||
motor_s->motor_send_data.comd.spd_des = motor_s->W / 6.28318f * 256.0f;
|
||
motor_s->motor_send_data.comd.tor_des = motor_s->T * 256.0f;
|
||
motor_s->motor_send_data.CRC16 = crc_ccitt(0, (uint8_t *)&motor_s->motor_send_data, sizeof(RIS_ControlData_t) - sizeof(motor_s->motor_send_data.CRC16));
|
||
}
|
||
|
||
/// @brief <20><><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5>Ķ<EFBFBD><C4B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
/// @param motor_r Ҫת<D2AA><D7AA><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9>
|
||
void extract_data(MotorData_t *motor_r)
|
||
{
|
||
if (motor_r->motor_recv_data.head[0] != 0xFD || motor_r->motor_recv_data.head[1] != 0xEE)
|
||
{
|
||
motor_r->correct = 0;
|
||
motor_r->bad_msg++;
|
||
return;
|
||
}
|
||
motor_r->calc_crc = crc_ccitt(0, (uint8_t *)&motor_r->motor_recv_data, sizeof(RIS_MotorData_t) - sizeof(motor_r->motor_recv_data.CRC16));
|
||
if (motor_r->motor_recv_data.CRC16 != motor_r->calc_crc)
|
||
{
|
||
memset(&motor_r->motor_recv_data, 0, sizeof(RIS_MotorData_t));
|
||
motor_r->correct = 0;
|
||
motor_r->bad_msg++;
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
motor_r->motor_id = motor_r->motor_recv_data.mode.id;
|
||
motor_r->mode = motor_r->motor_recv_data.mode.status;
|
||
motor_r->Temp = motor_r->motor_recv_data.fbk.temp;
|
||
motor_r->MError = motor_r->motor_recv_data.fbk.MError;
|
||
motor_r->W = ((float)motor_r->motor_recv_data.fbk.speed / 256.0f) * 6.28318f;
|
||
motor_r->T = ((float)motor_r->motor_recv_data.fbk.torque) / 256.0f;
|
||
motor_r->Pos = 6.28318f * ((float)motor_r->motor_recv_data.fbk.pos) / 32768.0f;
|
||
motor_r->footForce = motor_r->motor_recv_data.fbk.force;
|
||
motor_r->correct = 1;
|
||
return;
|
||
}
|
||
}
|