From feb3d438fae59d3d66cdc986efec8e5851c5fd65 Mon Sep 17 00:00:00 2001 From: Duke Date: Wed, 24 Jul 2024 20:43:03 +0800 Subject: [PATCH] rename noc_encoder to nob_encoder --- Readme.md | 8 ++++---- mdrv_fw/usr/app_main.c | 4 ++-- mdrv_fw/usr/app_main.h | 2 +- mdrv_fw/usr/app_motor.c | 28 ++++++++++++++-------------- mdrv_fw/usr/config.c | 10 +++++----- tools/cali_encoder.py | 3 +++ 6 files changed, 29 insertions(+), 26 deletions(-) diff --git a/Readme.md b/Readme.md index 7d17eda..1ab89d1 100644 --- a/Readme.md +++ b/Readme.md @@ -112,19 +112,19 @@ Mode 4 of `state` is a position mode without acceleration or deceleration and is ## Command Demonstration -The address of the `state` itself is `0x0219` and its length is 1 byte. +The address of the `state` itself is `0x0240` and its length is 1 byte. To lock the motor and enter position mode after power up, send the following data to port 5: ``` -20 19 02 05 +20 40 02 05 ``` -`20` is the subcommand `write`, `19 02` is the little-endian for address 0x0219 (little-endian is used unless otherwise noted), +`20` is the subcommand `write`, `40 02` is the little-endian for address 0x0240 (little-endian is used unless otherwise noted), and `05` is the value to be written. The complete command containing the CRC is (host address defaults to `0`, motor address defaults to `0xfe`, 3rd byte is data length, last two bytes are CRC): ``` -00 fe 05 05 20 19 02 05 88 f5 +00 fe 05 05 20 40 02 05 58 e6 ``` The complete response package is: diff --git a/mdrv_fw/usr/app_main.c b/mdrv_fw/usr/app_main.c index 3aa9394..5055494 100644 --- a/mdrv_fw/usr/app_main.c +++ b/mdrv_fw/usr/app_main.c @@ -312,10 +312,10 @@ void cali_elec_angle(void) d_debug("cali [%d, %d]", pole_cnt, sub_cnt); if (sub_cnt == 0) { - a0 = csa.ori_encoder; + a0 = csa.nob_encoder; d_debug_c(" - a0: %04x\n", a0); } else if (sub_cnt == 2) { - a1 = csa.ori_encoder; + a1 = csa.nob_encoder; d_debug_c(" - a1: %04x\n", a1); } else { d_debug_c("\n"); diff --git a/mdrv_fw/usr/app_main.h b/mdrv_fw/usr/app_main.h index cbebc62..2d5e469 100644 --- a/mdrv_fw/usr/app_main.h +++ b/mdrv_fw/usr/app_main.h @@ -130,7 +130,7 @@ typedef struct { int32_t ori_pos; float delta_encoder; - uint16_t noc_encoder; // no compensation + uint16_t nob_encoder; // no bias uint16_t sen_encoder; int32_t sen_pos; float sen_speed; diff --git a/mdrv_fw/usr/app_motor.c b/mdrv_fw/usr/app_motor.c index 67b3449..af0304c 100644 --- a/mdrv_fw/usr/app_motor.c +++ b/mdrv_fw/usr/app_motor.c @@ -328,28 +328,26 @@ void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) int16_t out_pwm_u = 0, out_pwm_v = 0, out_pwm_w = 0; bool dbg_str = csa.dbg_str_msk && (csa.loop_cnt % csa.dbg_str_skip) == 0; - uint16_t noc_encoder_bk = csa.noc_encoder; csa.ori_encoder = encoder_read(); - csa.noc_encoder = csa.ori_encoder - csa.bias_encoder; //gpio_set_value(&dbg_out2, 1); // filter out disturbed error position values #if 1 if (hist_err < 0 || abs(hist[1] - hist[0]) > 500) { - csa.sen_encoder = csa.noc_encoder; + csa.nob_encoder = csa.ori_encoder; if (hist_err < 0) hist_err++; } else { uint32_t hist_raised[2] = {hist[0], hist[1]}; - uint32_t sen_raised = csa.noc_encoder; + uint32_t sen_raised = csa.ori_encoder; // empty in range: 1/3 to 2/3 if ((hist[0] < 0x10000/3 || hist[0] >= 0x10000*2/3) && (hist[1] < 0x10000/3 || hist[1] >= 0x10000*2/3)) { if (hist[0] < 0x10000/3 && hist[1] < 0x10000/3) { - if (csa.noc_encoder < 0x10000*2/3) + if (csa.ori_encoder < 0x10000*2/3) sen_raised += 0x10000; - } else if (csa.noc_encoder < 0x10000/3) { + } else if (csa.ori_encoder < 0x10000/3) { sen_raised += 0x10000; } if (hist[0] < 0x10000/3) @@ -360,35 +358,37 @@ void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) int16_t hist_delta = lroundf((int16_t)(hist_raised[1] - hist_raised[0]) * 0.8f); uint32_t estimate = hist_raised[1] + hist_delta; if (abs(sen_raised - estimate) > 500) { - csa.sen_encoder = estimate >= 0x10000 ? (estimate - 0x10000) : estimate; + csa.nob_encoder = estimate >= 0x10000 ? (estimate - 0x10000) : estimate; if(++hist_err > 2) hist_err = -2; } else { - csa.sen_encoder = csa.noc_encoder; + csa.nob_encoder = csa.ori_encoder; hist_err = 0; } } hist[0] = hist[1]; - hist[1] = csa.sen_encoder; + hist[1] = csa.nob_encoder; #else - csa.sen_encoder = csa.noc_encoder; + csa.nob_encoder = csa.ori_encoder; #endif - if (csa.sen_encoder != csa.noc_encoder) + if (csa.nob_encoder != csa.ori_encoder) encoder_err++; if (csa.cali_encoder_en) { - tbl_idx = csa.sen_encoder >> 4; + tbl_idx = csa.nob_encoder >> 4; tbl_idx_next = (tbl_idx == 4095) ? 0 : (tbl_idx + 1); - tbl_percent = (float)(csa.sen_encoder & 0xf) / 0x10; + tbl_percent = (float)(csa.nob_encoder & 0xf) / 0x10; uint16_t idx_val = *(uint16_t *)(CALI_ENCODER_TBL + tbl_idx * 2); uint32_t idx_next_val = *(uint16_t *)(CALI_ENCODER_TBL + tbl_idx_next * 2); if (tbl_idx == 4095) idx_next_val += 0x10000; - csa.sen_encoder = lroundf(idx_val + (idx_next_val - idx_val) * tbl_percent); + csa.nob_encoder = lroundf(idx_val + (idx_next_val - idx_val) * tbl_percent); } + csa.sen_encoder = csa.nob_encoder - csa.bias_encoder; + int16_t delta_enc = csa.sen_encoder - sen_encoder_bk; csa.delta_encoder = delta_enc; sen_encoder_bk = csa.sen_encoder; diff --git a/mdrv_fw/usr/config.c b/mdrv_fw/usr/config.c index b6ba458..9a305a9 100644 --- a/mdrv_fw/usr/config.c +++ b/mdrv_fw/usr/config.c @@ -96,7 +96,7 @@ const csa_t csa_dft = { { .offset = offsetof(csa_t, pid_i_sd) + offsetof(pid_f_t, i_term), .size = 4 * 2 }, { .offset = offsetof(csa_t, sen_encoder), .size = 2 }, { .offset = offsetof(csa_t, cal_v_sq), .size = 4 * 2 } - //{ .offset = offsetof(csa_t, noc_encoder), .size = 2 } + //{ .offset = offsetof(csa_t, nob_encoder), .size = 2 } }, { // speed { .offset = offsetof(csa_t, pid_speed) + offsetof(pid_f_t, target), .size = 4 * 3 }, { .offset = offsetof(csa_t, cal_current), .size = 4 }, @@ -343,13 +343,13 @@ void csa_list_show(void) CSA_SHOW(0, cal_v_sd, "v_sd info"); d_info("\n"); debug_flush(true); - CSA_SHOW(1, ori_encoder, "noc_encoder before add offset"); + CSA_SHOW(1, ori_encoder, "Origin encoder value"); CSA_SHOW(1, ori_pos, "sen_pos before add offset"); d_info("\n"); debug_flush(true); - CSA_SHOW(1, delta_encoder, "encoder value delta"); - CSA_SHOW(1, noc_encoder, "encoder value"); - CSA_SHOW(1, sen_encoder, "encoder value filtered"); + CSA_SHOW(1, delta_encoder, "Encoder value delta"); + CSA_SHOW(1, nob_encoder, "Encoder value before add bias"); + CSA_SHOW(1, sen_encoder, "Encoder value filtered"); CSA_SHOW(1, sen_pos, "multiturn + sen_encoder data"); CSA_SHOW(1, sen_speed, "delta_encoder filtered"); CSA_SHOW(0, sen_i_sq, "i_sq from adc"); diff --git a/tools/cali_encoder.py b/tools/cali_encoder.py index bd55eaf..d0ce9f5 100755 --- a/tools/cali_encoder.py +++ b/tools/cali_encoder.py @@ -9,6 +9,9 @@ #import numpy as np import matplotlib.pyplot as plt +# rotate motor by state = 1, cali_angle_step > 0 and bias_encoder = 0 +# enable plot0 for a short period, then export cdbus_gui data to mpk file + if len(sys.argv) <= 1: print(f'usage: {sys.argv[0]} mpk_dat_file') exit(-1)