From 1e1b260fb973f2f095d1a17d0465a7aa1f486aa6 Mon Sep 17 00:00:00 2001 From: Marco Accame Date: Thu, 4 May 2023 12:58:57 +0200 Subject: [PATCH] Board `amc2c`: some fixes in motor control code (#377) --- .../application/proj/amc2c-appl-can.uvoptx | 26 ++++-- .../application/proj/amc2c-appl-can.uvprojx | 12 +-- .../embot_app_board_amc2c_mbd.demo.cpp | 81 +------------------ .../embot_app_board_amc2c_test.cpp | 12 +-- .../embot_app_board_amc2c_theMBD.cpp | 50 +++++++++--- .../board/amc2c/bsp/motorhal/motorhal.cpp | 21 +++-- .../board/amc2c/bsp/motorhal/motorhal.h | 3 +- .../sharedutils/rtw_enable_disable_motors.c | 25 ++++-- .../sharedutils/rtw_motor_config.c | 46 +++++++---- .../sharedutils/rtw_mutex.h | 22 ++--- .../arch-arm/embot/hw/embot_hw_motor.cpp | 11 ++- 11 files changed, 157 insertions(+), 152 deletions(-) diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvoptx b/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvoptx index bafbbc5aa8..48eaa35343 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvoptx +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvoptx @@ -453,7 +453,7 @@ 0 0 - 79 + 38 1
0
0 @@ -469,6 +469,22 @@ 1 0 + 79 + 1 +
0
+ 0 + 0 + 0 + 0 + 0 + 0 + ..\src\app-board-amc2c\embot_app_board_amc2c_mbd.demo.cpp + + + + + 2 + 0 296 1
0
@@ -483,7 +499,7 @@ - 2 + 3 0 278 1 @@ -1666,7 +1682,7 @@ mbd - 0 + 1 0 0 0 @@ -1769,7 +1785,7 @@ 17 70 - 1 + 8 0 0 0 @@ -1781,7 +1797,7 @@ 17 71 - 1 + 8 0 0 0 diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvprojx b/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvprojx index 65ecef8816..338888f9a3 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvprojx +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/proj/amc2c-appl-can.uvprojx @@ -931,12 +931,12 @@ rtw_enable_disable_motors.c - 1 + 8 ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c rtw_motor_config.c - 1 + 8 ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c @@ -2172,12 +2172,12 @@ rtw_enable_disable_motors.c - 1 + 8 ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c rtw_motor_config.c - 1 + 8 ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c @@ -3464,12 +3464,12 @@ rtw_enable_disable_motors.c - 1 + 8 ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_enable_disable_motors.c rtw_motor_config.c - 1 + 8 ..\..\..\amcbldc\application\src\model-based-design\sharedutils\rtw_motor_config.c diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_mbd.demo.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_mbd.demo.cpp index 6304a65c9b..a8d990f3dd 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_mbd.demo.cpp +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_mbd.demo.cpp @@ -18,94 +18,17 @@ // - external dependencies // -------------------------------------------------------------------------------------------------------------------- -#include "embot_hw_timer.h" - -#include "adcm.h" -#include "enc.h" -#include "hall.h" -#include "pwm.h" - -#include "embot_hw_motor.h" namespace embot::app::board::amc2c::mbd { -// void hei(void *p) -// { -// static volatile uint64_t prev {0}; -// static volatile uint64_t delta {0}; -// volatile uint64_t no = embot::core::now(); -// delta = no - prev; -// prev = no; -// } -// embot::core::Callback cc {hei, nullptr}; - void Startup(embot::prot::can::Address adr) { - // in debug mode we have to click the "RUN" button when stucked on main, before it will be unlocked by CM7 - // otherwise, it seems that this initialization is not performed. - embot::hw::motor::init(embot::hw::MOTOR::one, {}); - embot::app::board::amc2c::theMBD::getInstance().initialise({adr}); - -// embot::hw::timer::Config cfg{embot::core::time1millisec, embot::hw::timer::Mode::periodic, cc}; -// embot::hw::timer::init(embot::hw::TIMER::one, cfg); -// embot::hw::timer::start(embot::hw::TIMER::one); - - + embot::app::board::amc2c::theMBD::getInstance().initialise({adr}); } void OnTick(const std::vector &input, std::vector &output) { - - size_t numRXframes = input.size(); - - if(numRXframes > 0) - { - embot::prot::can::Frame f = input[0]; - - switch(f.id) - { - case 0x001: - { - // test 1 - PwmPhaseSet(1, 2, 3); - } break; - - case 0x002: - { - // test 2 - } break; - - default: - { - } break; - } - } - - - embot::app::board::amc2c::theMBD::getInstance().tick(input, output); - - -// static uint32_t cnt {0}; -// if(0 == (cnt++ % 1)) -// { -//// embot::core::print("MBD is ticking on an amc2c"); -// -// uint8_t b0 = cnt&0xff; -// uint8_t b1 = (cnt>>8)&0xff; -// uint8_t b2 = (cnt>>16)&0xff; -// uint8_t b3 = (cnt>>24)&0xff; -// embot::prot::can::Frame fr1 {0x202, 4, {b0, b1, b2, b3, 0, 0, 0, 0}}; -// output.push_back(fr1); -// } -// -// size_t numRXframes = input.size(); -// -// if(numRXframes > 0) -// { -// // just for test: i get the first only and i send it back -// embot::prot::can::Frame f = input[0]; -// output.push_back(f); -// } + embot::app::board::amc2c::theMBD::getInstance().tick(input, output); } } // end of namespace diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_test.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_test.cpp index 0dd6b2dd73..16cc7d1176 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_test.cpp +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_test.cpp @@ -48,17 +48,7 @@ namespace embot::app::board::amc2c::mbd { EncInit(); HallInit(); PwmInit(); - - - // in debug mode questa init non viene chiamata a meno che non si clicca "RUN" manualmente prima che venga sbloccato dal CM7 -// embot::hw::motor::init(embot::hw::MOTOR::one, {}); -// embot::app::board::amc2c::theMBD::getInstance().initialise({adr}); - -// embot::hw::timer::Config cfg{embot::core::time1millisec, embot::hw::timer::Mode::periodic, cc}; -// embot::hw::timer::init(embot::hw::TIMER::one, cfg); -// embot::hw::timer::start(embot::hw::TIMER::one); - - + } void OnTick(const std::vector &input, std::vector &output) diff --git a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp index 0740ec3edf..a86dbe2263 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp +++ b/emBODY/eBcode/arch-arm/board/amc2c/application/src/app-board-amc2c/embot_app_board_amc2c_theMBD.cpp @@ -19,9 +19,11 @@ #if defined(STM32HAL_BOARD_AMC2C) #include "embot_hw_bsp_amc2c.h" -#warning READ CAREFULLY: we may use the same object for both amcbldc and amc2c -#else +//#warning READ CAREFULLY: we may use the same object for both amcbldc and amc2c +#elif defined(STM32HAL_BOARD_AMCBLDC) #include "embot_hw_bsp_amcbldc.h" +#else +#error: choose a STM32HAL_BOARD_* #endif #include "embot_hw_button.h" @@ -45,6 +47,8 @@ // -------------------------------------------------------------------------------------------------------------------- //#define TEST_DURATION_FOC +#define TEST_FOC_logvalues + // If the target setup is the KOLLMORGEN motor on RED LEGO platform, // you need to enable the macro USE_KOLLMORGEN_SETUP, otherwise @@ -469,16 +473,29 @@ bool embot::app::board::amc2c::theMBD::Impl::tick(const std::vectorstop(); return true; } +#if defined(TEST_FOC_logvalues) + +struct dbgFOCvalues +{ + embot::hw::motor::Currents currents {0, 0, 0}; + uint8_t hall {0}; + embot::hw::motor::Position electricalangle {0}; + int32_t position {0}; + float jointangle {0.0}; + std::array pwms {0, 0, 0}; + + dbgFOCvalues() = default; +}; + +dbgFOCvalues dbgFOC {}; + +#endif void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owner, const embot::hw::motor::Currents * const currents) { @@ -499,8 +516,10 @@ void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owne // remember to manage impl->EXTFAULTisPRESSED ............ - // retrieve the current value of the Hall sensors - embot::hw::motor::gethallstatus(embot::hw::MOTOR::one, AMC_BLDC_U.SensorsData_p.motorsensors.hallABC); + // retrieve the current value of the Hall sensors + uint8_t hall {0}; + embot::hw::motor::gethallstatus(embot::hw::MOTOR::one, hall); + AMC_BLDC_U.SensorsData_p.motorsensors.hallABC = hall; // retrieve the current value of the encoder embot::hw::motor::Position electricalAngle {0}; @@ -514,7 +533,8 @@ void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owne electricalAngleOld = electricalAngle; // calculate the current joint position - position = position + delta / AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs; + size_t polepairs = (0 != AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs) ? AMC_BLDC_Y.ConfigurationParameters_p.motorconfig.pole_pairs : 1; + position = position + delta / polepairs; AMC_BLDC_U.SensorsData_p.motorsensors.angle = static_cast(electricalAngle)*0.0054931640625f; // (60 interval angle) @@ -538,7 +558,19 @@ void embot::app::board::amc2c::theMBD::Impl::onCurrents_FOC_innerloop(void *owne int32_T Vabc1 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[1] * 163.83F); int32_T Vabc2 = static_cast(AMC_BLDC_Y.ControlOutputs_p.Vabc[2] * 163.83F); +#if defined(TEST_FOC_logvalues) + + dbgFOC.currents = currs; + dbgFOC.hall = hall; + dbgFOC.electricalangle = electricalAngle; + dbgFOC.position = position; + dbgFOC.jointangle = static_cast(position) * 0.0054931640625f; + dbgFOC.pwms = {Vabc0, Vabc1, Vabc2}; + +#endif + embot::hw::motor::setpwm(embot::hw::MOTOR::one, Vabc0, Vabc1, Vabc2); + //#define DEBUG_PARAMS #ifdef DEBUG_PARAMS diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.cpp b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.cpp index 6db2590c25..2415792798 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.cpp +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.cpp @@ -434,7 +434,7 @@ struct hall_Data { uint8_t order[3] = {0, 1, 2}; int32_t counter {0}; - uint16_t angle {0}; + int32_t angle {0}; volatile uint8_t status {0}; void reset(Mode::SWAP s = Mode::SWAP::none) @@ -486,9 +486,9 @@ static uint8_t hall_Get() // if we have order = {0, 1, 2} we dont reorder anything, so we have v = x = H3H2H1 = CBA. // but in case swapBC is true, then we have order {0, 2, 1} which swap second w/third, so v = H2H3H1 = BCA - uint8_t v = (((v>>0)&0x1) >> _hall_internals.data.order[0]) | // H1 (pos 0) is moved to the final position - (((v>>1)&0x1) >> _hall_internals.data.order[1]) | // H2 (pos 1) is moved to the final position - (((v>>2)&0x1) >> _hall_internals.data.order[2]); // H3 (pos 2) is moved to the final position + uint8_t v = (((x>>0)&0x1) >> _hall_internals.data.order[0]) | // H1 (pos 0) is moved to the final position + (((x>>1)&0x1) >> _hall_internals.data.order[1]) | // H2 (pos 1) is moved to the final position + (((x>>2)&0x1) >> _hall_internals.data.order[2]); // H3 (pos 2) is moved to the final position return v; } @@ -497,8 +497,8 @@ static void hall_OnCapture(TIM_HandleTypeDef *htim) { _hall_internals.data.status = hall_Get(); - // in here we DONT ... perform any calibration of the encoder .... - // because in the wrist we dont have encoder. we may implement this calibration later on + // in here, we surely fill angle. + _hall_internals.data.angle = _hall_internals.mode.offset + hall_Table::status2angle(_hall_internals.data.status); } @@ -587,10 +587,15 @@ bool isstarted() return _hall_internals.started; } -uint8_t getstatus(void) +uint8_t getstatus() { return _hall_internals.data.status; -} +} + +int32_t getangle() +{ + return _hall_internals.data.angle; +} } // namespace embot::hw::motor::hall { diff --git a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.h b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.h index 03c5800462..ba51c1c09a 100644 --- a/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.h +++ b/emBODY/eBcode/arch-arm/board/amc2c/bsp/motorhal/motorhal.h @@ -160,7 +160,8 @@ bool init(const Configuration &config); bool deinit(); bool start(const Mode &mode); bool isstarted(); -uint8_t getstatus(void); +uint8_t getstatus(); +int32_t getangle(); } // namespace embot::hw::motor::hall { diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_enable_disable_motors.c b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_enable_disable_motors.c index 81c1de41c3..a54169b84b 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_enable_disable_motors.c +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_enable_disable_motors.c @@ -17,27 +17,36 @@ #include "rtw_enable_disable_motors.h" -#ifdef STM32HAL_BOARD_AMCBLDC + + +#if defined(STM32HAL_BOARD_AMCBLDC) || defined(STM32HAL_BOARD_AMC2C) #ifndef __cplusplus - #error this file must be compiled in C++ mode when deployed on the amcbldc target + #error this file must be compiled in C++ mode when deployed on the target #endif -// we use embot::hw + #include "embot_hw_motor.h" -#endif /* STM32HAL_BOARD_AMCBLDC */ + +#else + #warning: choose a STM32HAL_BOARD_* +#endif void rtw_enableMotor() { -#ifdef STM32HAL_BOARD_AMCBLDC +#if defined(STM32HAL_BOARD_AMCBLDC) || defined(STM32HAL_BOARD_AMC2C) embot::hw::motor::enable(embot::hw::MOTOR::one, true); -#endif /* STM32HAL_BOARD_AMCBLDC */ +#else + #warning: choose a STM32HAL_BOARD_* +#endif } void rtw_disableMotor() { -#ifdef STM32HAL_BOARD_AMCBLDC +#if defined(STM32HAL_BOARD_AMCBLDC) || defined(STM32HAL_BOARD_AMC2C) embot::hw::motor::enable(embot::hw::MOTOR::one, false); -#endif /* STM32HAL_BOARD_AMCBLDC */ +#else + #warning: choose a STM32HAL_BOARD_* +#endif } // - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.c b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.c index 2c1c411ca2..4f3a091a2d 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.c +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_motor_config.c @@ -16,25 +16,43 @@ */ #include "rtw_motor_config.h" -#ifdef STM32HAL_BOARD_AMCBLDC + +#if defined(STM32HAL_BOARD_AMCBLDC) || defined(STM32HAL_BOARD_AMC2C) #ifndef __cplusplus - #error this file must be compiled in C++ mode when deployed on the amcbldc target + #error this file must be compiled in C++ mode when deployed on the target #endif -// we use embot::hw + #include "embot_hw_motor.h" -#endif /* STM32HAL_BOARD_AMCBLDC */ + +#else + #warning: choose a STM32HAL_BOARD_* +#endif void rtw_configMotor(uint8_t has_quad_enc, int16_t rotor_enc_resolution, uint8_t pole_pairs, uint8_t has_hall_sens, uint8_t swapBC, uint16_t hall_sens_offset) { - #ifdef STM32HAL_BOARD_AMCBLDC - embot::hw::motor::config( - embot::hw::MOTOR::one, - has_quad_enc, - rotor_enc_resolution, - pole_pairs, - has_hall_sens, - 1, - 120*65536/360); - #endif /* STM32HAL_BOARD_AMCBLDC */ +#if defined(STM32HAL_BOARD_AMCBLDC) || defined(STM32HAL_BOARD_AMC2C) + +// embot::hw::motor::config( +// embot::hw::MOTOR::one, +// has_quad_enc, +// rotor_enc_resolution, +// pole_pairs, +// has_hall_sens, +// swapBC, // it was 1, but better use its param +// hall_sens_offset); // it was 120*65536/360, but better using its param +// // because: supervisor calls rtw_configMotor( , swapBC = 1, hall_sens_offset = 21845) and 120*65536/360 = 21845 + + // marco.accame: but now we use embot::hw::motor::Config + const embot::hw::motor::Config cfg { + rotor_enc_resolution, pole_pairs, hall_sens_offset, + (0 == has_quad_enc) ? false : true, + (0 == has_hall_sens) ? false : true, + (0 == swapBC) ? false : true + }; + embot::hw::motor::configure(embot::hw::MOTOR::one, cfg); + +#else + #warning: choose a STM32HAL_BOARD_* +#endif } \ No newline at end of file diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_mutex.h b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_mutex.h index 89da229734..7b04ad7727 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_mutex.h +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/model-based-design/sharedutils/rtw_mutex.h @@ -32,12 +32,13 @@ inline void rtw_mutex_lock(void) { if(false == IsException()) { - #ifdef STM32HAL_BOARD_AMCBLDC +#if defined(STM32HAL_BOARD_AMCBLDC) NVIC_DisableIRQ(DMA1_Channel2_IRQn); - #endif - #ifdef STM32HAL_BOARD_AMC2C - NVIC_DisableIRQ(BDMA_Channel2_IRQn); - #endif +#elif defined(STM32HAL_BOARD_AMC2C) + NVIC_DisableIRQ(DMA2_Stream0_IRQn); +#else + #warning: choose a STM32HAL_BOARD_* +#endif } } @@ -45,12 +46,13 @@ inline void rtw_mutex_unlock(void) { if(false == IsException()) { - #ifdef STM32HAL_BOARD_AMCBLDC +#if defined(STM32HAL_BOARD_AMCBLDC) NVIC_EnableIRQ(DMA1_Channel2_IRQn); - #endif - #ifdef STM32HAL_BOARD_AMC2C - NVIC_EnableIRQ(BDMA_Channel2_IRQn); - #endif +#elif defined(STM32HAL_BOARD_AMC2C) + NVIC_EnableIRQ(DMA2_Stream0_IRQn); +#else + #warning: choose a STM32HAL_BOARD_* +#endif } } diff --git a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp index 249d1f755e..67c127606e 100644 --- a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp +++ b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_motor.cpp @@ -429,7 +429,16 @@ namespace embot { namespace hw { namespace motor { Position s_hw_getencoder(MOTOR h) { - return embot::hw::motor::enc::getvalue(); + Position p {0}; + if(true == embot::hw::motor::enc::isstarted()) + { + p = embot::hw::motor::enc::getvalue(); + } + else if(true == embot::hw::motor::hall::isstarted()) + { + p = embot::hw::motor::hall::getangle(); + } + return p; } // Position s_hw_gethallcounter(MOTOR h)