202 static uint8_t data[] = { 0x40, 0x60, 0x00, 0x0E, 0x00};
214 static uint8_t data[] = { 0x60, 0x60, 0x00, 0x06 };
226 static uint8_t data[] = { 0x98, 0x60, 0x00, 0x11 };
238 static uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00};
250 static uint8_t data[] = { 0x40, 0x60, 0x00, 0x1F, 0x00};
276 static uint8_t data[] = { 0x60, 0x60, 0x00, 0x03 };
288 static uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00};
312 uint8_t data[] = { 0x64, 0x60, 0x00};
338 uint8_t data[] = { 0x20, 0x23, 0x00};
345 uint8_t data[] = { 0x21, 0x23, 0x00};
352 uint8_t data[] = { 0x01, 0x10, 0x00};
370 static uint8_t data[] = { 0x40, 0x60, 0x00, 0x0E, 0x00};
376 static uint8_t data[] = { 0x60, 0x60, 0x00, 0x03 };
383 static uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00};
403 if (
p->cmd_code == 0x01 &&
p->data[0] == 0x64 &&
p->data[1] == 0x60 &&
p->data[2] == 0x00) {
404 faulhaber.real_position =
p->data[3] | (
p->data[4] << 8) | (
p->data[5] << 16) | (
p->data[6] << 24);
408 0) - get_servo_min_FAULHABER(0)) * M_PI_2;
410 feedback.
idx = get_servo_idx_FAULHABER(0);
416 else if(
p->cmd_code == 0x02) {
420 else if(
p->cmd_code == 0x05) {
421 uint16_t status_code =
p->data[0] | (
p->data[1] << 8);
424 if(!
faulhaber.homing_completed && status_code&0x1000 && status_code&0x400)
428 if(!
faulhaber.position_ready && status_code&0x1000)
432 if(!
faulhaber.target_reached && status_code&0x400)
436 if(!(status_code&0x0001) || !(status_code&0x0002) || !(status_code&0x0004)) {
438 int rc = snprintf(error_msg, 200,
"[FH]%d %04X",
p->cmd_code, status_code);
445 int rc = snprintf(error_msg, 200,
"[FH]%d ",
p->cmd_code);
446 for(
int i = 0; i <
p->data_length; i++) {
447 rc += snprintf(error_msg + rc, 200 - rc,
"%02X",
p->data[i]);