From 647e5395b177804fafe35c98764a9c4874aa60b4 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 9 Aug 2023 16:24:56 +0900 Subject: [PATCH] Fix source voltage compensation on R6B board --- tfrog-motordriver/controlVelocity.h | 1 + tfrog-motordriver/main.c | 15 ++++++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/tfrog-motordriver/controlVelocity.h b/tfrog-motordriver/controlVelocity.h index 09a18f0..359ac15 100644 --- a/tfrog-motordriver/controlVelocity.h +++ b/tfrog-motordriver/controlVelocity.h @@ -177,6 +177,7 @@ typedef struct _DriverState uint32_t velcontrol; uint32_t ping_request; uint32_t odom_drop; + int32_t vsrc_max; } DriverState; #ifdef static_assert diff --git a/tfrog-motordriver/main.c b/tfrog-motordriver/main.c index 7ebc07e..4b9a74c 100644 --- a/tfrog-motordriver/main.c +++ b/tfrog-motordriver/main.c @@ -711,6 +711,19 @@ int main() driver_state.ifmode = 0; driver_state.watchdog = 0; driver_state.odom_drop = 0; + driver_state.vsrc_max = 0x3ff; + switch (driver_state.board_version) + { + case BOARD_R6A: + case BOARD_R4: + break; + case BOARD_R6B: +#if defined(tfrog_rev5) + driver_state.vsrc_max = 0x3ff * VSRC_CONV_B; +#endif + break; + } + // Driver loop while (1) { @@ -1012,7 +1025,7 @@ int main() driver_state.vsrc = Filter1st_Filter(&voltf, (int32_t)(analog[7] & 0x0FFF)); ADC_Start(); - if (driver_param.vsrc_rated >= 0x03FF) + if (driver_param.vsrc_rated >= driver_state.vsrc_max) { driver_state.vsrc_factor = 32768; }