From cf03ce111d059665f59580d84dd30ff5fb98c241 Mon Sep 17 00:00:00 2001 From: Marco Accame Date: Wed, 23 Nov 2022 17:27:58 +0100 Subject: [PATCH] Some improvements for the ergoCub wrist (#324) --- .../appl-01/cfg/theApplication_config.h | 10 +++- .../amcbldc/application/src/amcbldc-main.cpp | 26 ++++++++- .../src/embot_app_application_theMBDagent.cpp | 55 ++++++++++++++++++- .../eBcode/arch-arm/embobj/plus/mc/JointSet.c | 29 +++++++--- 4 files changed, 107 insertions(+), 13 deletions(-) diff --git a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h b/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h index c7cb6f766a..7374510ce7 100644 --- a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h +++ b/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h @@ -38,7 +38,15 @@ namespace embot { namespace app { namespace eth { .property = { Process::eApplication, - {3, 0}, +#if defined(WRIST_MK2) + #if defined(WRIST_MK2_RIGHT) + {4, 4}, + #else + {3, 0}, + #endif +#else + {103, 0}, +#endif {2022, Month::Apr, Day::fourteen, 15, 16} }, .OStick = 1000*embot::core::time1microsec, diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/amcbldc-main.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/amcbldc-main.cpp index 369a858cb7..f72fdd4f68 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/amcbldc-main.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/amcbldc-main.cpp @@ -112,19 +112,27 @@ int main(void) #include "embot_os_rtos.h" +//#define SHARED_CIRCULAR + struct shared_t { + private: + + static constexpr size_t txCapacity {140}; + static constexpr size_t rxCapacity {20}; embot::os::rtos::mutex_t *rxm {nullptr}; std::vector rxCANvector {}; embot::os::rtos::mutex_t *txm {nullptr}; std::vector txCANvector {}; + public: + shared_t() { rxm = embot::os::rtos::mutex_new(); txm = embot::os::rtos::mutex_new(); - rxCANvector.reserve(10); - txCANvector.reserve(10); + rxCANvector.reserve(rxCapacity); + txCANvector.reserve(txCapacity); } size_t sizeofrx() @@ -149,6 +157,12 @@ struct shared_t bool addrx(const embot::prot::can::Frame &f) { embot::os::rtos::mutex_take(rxm, embot::core::reltimeWaitForever); +#if defined(SHARED_CIRCULAR) + if(rxCANvector.size() >= rxCapacity) + { + rxCANvector.erase(rxCANvector.begin()); + } +#endif rxCANvector.push_back(f); embot::os::rtos::mutex_release(rxm); return true; @@ -175,6 +189,12 @@ struct shared_t bool addtx(const embot::prot::can::Frame &f) { embot::os::rtos::mutex_take(txm, embot::core::reltimeWaitForever); +#if defined(SHARED_CIRCULAR) + if(txCANvector.size() >= txCapacity) + { + txCANvector.erase(txCANvector.begin()); + } +#endif txCANvector.push_back(f); embot::os::rtos::mutex_release(txm); return true; @@ -314,7 +334,7 @@ void myEVT::userdefOnEventRXcanframe(embot::os::Thread *t, embot::os::EventMask bool valid {false}; if(0 == embot::prot::can::frame2sender(frame)) { - valid = true; + valid = true; } if(valid) diff --git a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/embot_app_application_theMBDagent.cpp b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/embot_app_application_theMBDagent.cpp index c6454f28c5..975c57ceac 100644 --- a/emBODY/eBcode/arch-arm/board/amcbldc/application/src/embot_app_application_theMBDagent.cpp +++ b/emBODY/eBcode/arch-arm/board/amcbldc/application/src/embot_app_application_theMBDagent.cpp @@ -44,9 +44,15 @@ // you need to enable the macro USE_KOLLMORGEN_SETUP, otherwise // it is implied that motor on wrist mk2 is in use -#define EXTFAULT_enabled +//#define EXTFAULT_enabled #define EXTFAULT_handler_will_disable_motor +#define EXTFAULT_enabled_polling + +#if defined(EXTFAULT_enabled) +#undef EXTFAULT_enabled_polling +#endif + // -------------------------------------------------------------------------------------------------------------------- // - pimpl: private implementation (see scott meyers: item 22 of effective modern c++, item 31 of effective c++ // -------------------------------------------------------------------------------------------------------------------- @@ -226,6 +232,10 @@ struct embot::app::application::theMBDagent::Impl volatile embot::core::Time EXTFAULTpressedtime {0}; volatile embot::core::Time EXTFAULTreleasedtime {0}; + void onEXTFAULTpolling(); + std::vector faultvalues {}; + static constexpr size_t faultcapacity {5}; + #ifdef PRINT_HISTO_DEBUG void printHistogram_rxPkt(size_t cur_size); //cur_size is the currente queue size static constexpr uint8_t numOfBins {11}; //size of queue of the CAN drive plus 1 (in case any packet is received) @@ -278,6 +288,20 @@ bool embot::app::application::theMBDagent::Impl::initialise() embot::core::Callback cbkOnEXTFAULT {onEXTFAULTpressedreleased, this}; embot::hw::button::init(buttonEXTfault, {embot::hw::button::Mode::TriggeredOnPressAndRelease, cbkOnEXTFAULT, 0}); prevEXTFAULTisPRESSED = EXTFAULTisPRESSED = embot::hw::button::pressed(buttonEXTfault); +#elif defined(EXTFAULT_enabled_polling) + + buttonEXTfault = embot::hw::bsp::amcbldc::EXTFAULTbutton(); + + faultvalues.reserve(faultcapacity); + faultvalues.clear(); + + embot::hw::button::init(buttonEXTfault, {embot::hw::button::Mode::Polling, {}, 0}); + for(size_t i=0; i= faultcapacity) + { + faultvalues.erase(faultvalues.begin()); + } + bool pressed = embot::hw::button::pressed(buttonEXTfault); + faultvalues.push_back(pressed); + bool allpressed = std::all_of(faultvalues.cbegin(), faultvalues.cend(), [](auto v){ return v; }); + + EXTFAULTisPRESSED = allpressed; + AMC_BLDC_U.ExternalFlags_p.fault_button = EXTFAULTisPRESSED; + + if(true == EXTFAULTisPRESSED) + { + EXTFAULTpressedtime = embot::core::now(); +#if defined(EXTFAULT_handler_will_disable_motor) + embot::hw::motor::fault(embot::hw::MOTOR::one, true); +#endif + } + else + { + embot::hw::motor::fault(embot::hw::MOTOR::one, false); + EXTFAULTreleasedtime = embot::core::now(); + } + +} void embot::app::application::theMBDagent::Impl::onEXTFAULTpressedreleased(void *owner) { @@ -343,6 +394,8 @@ bool embot::app::application::theMBDagent::Impl::tick(std::vectorstart(); + onEXTFAULTpolling(); + if(prevEXTFAULTisPRESSED != EXTFAULTisPRESSED) { prevEXTFAULTisPRESSED = EXTFAULTisPRESSED; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c index beb33f6cf0..2e385ada8d 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c @@ -44,6 +44,18 @@ static void JointSet_set_inner_control_flags(JointSet* o); static const CTRL_UNITS DEG2ICUB = 65536.0f/360.0f; static const CTRL_UNITS ICUB2DEG = 360.0f/65536.0f; +#if defined(WRIST_MK2) + +#if defined(WRIST_MK2_RIGHT) + constexpr float prefactor = -1.0; + constexpr float postfactor = -1.0; +#else + constexpr float prefactor = +1.0; + constexpr float postfactor = +1.0; +#endif + +#endif + JointSet* JointSet_new(uint8_t n) // { JointSet* o = NEW(JointSet, n); @@ -798,6 +810,7 @@ void JointSet_do_pwm_control(JointSet* o) BOOL limits_torque_protection = FALSE; #ifdef WRIST_MK2 + if (o->must_park) { JointSet_start_park(o); @@ -840,9 +853,9 @@ void JointSet_do_pwm_control(JointSet* o) // JointSet_send_debug_message(NULL, 0, par16, par32); // } - o->wrist_decoupler.rtU.ypr[0] = ICUB2DEG*o->ypr_pos_ref[0]; + o->wrist_decoupler.rtU.ypr[0] = prefactor*ICUB2DEG*o->ypr_pos_ref[0]; o->wrist_decoupler.rtU.ypr[1] = ICUB2DEG*o->ypr_pos_ref[1]; - o->wrist_decoupler.rtU.ypr[2] = ICUB2DEG*o->ypr_pos_ref[2]; + o->wrist_decoupler.rtU.ypr[2] = prefactor*ICUB2DEG*o->ypr_pos_ref[2]; o->wrist_decoupler.rtU.theta_meas[0] = ICUB2DEG*(o->joint[0]).pos_fbk + o->arm_pos_off[0]; o->wrist_decoupler.rtU.theta_meas[1] = ICUB2DEG*(o->joint[1]).pos_fbk + o->arm_pos_off[1]; @@ -860,9 +873,9 @@ void JointSet_do_pwm_control(JointSet* o) } } - o->ypr_pos_fbk[0] = DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[0]); + o->ypr_pos_fbk[0] = postfactor*DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[0]); o->ypr_pos_fbk[1] = DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[1]); - o->ypr_pos_fbk[2] = DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[2]); + o->ypr_pos_fbk[2] = postfactor*DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[2]); // o->ypr_pos_fbk[0] = ZERO; // o->ypr_pos_fbk[1] = ZERO; @@ -1074,9 +1087,9 @@ static void JointSet_do_current_control(JointSet* o) // JointSet_send_debug_message(NULL, 0, par16, par32); // } - o->wrist_decoupler.rtU.ypr[0] = ICUB2DEG*o->ypr_pos_ref[0]; + o->wrist_decoupler.rtU.ypr[0] = prefactor*ICUB2DEG*o->ypr_pos_ref[0]; o->wrist_decoupler.rtU.ypr[1] = ICUB2DEG*o->ypr_pos_ref[1]; - o->wrist_decoupler.rtU.ypr[2] = ICUB2DEG*o->ypr_pos_ref[2]; + o->wrist_decoupler.rtU.ypr[2] = prefactor*ICUB2DEG*o->ypr_pos_ref[2]; o->wrist_decoupler.rtU.theta_meas[0] = ICUB2DEG*(o->joint[0]).pos_fbk + o->arm_pos_off[0]; o->wrist_decoupler.rtU.theta_meas[1] = ICUB2DEG*(o->joint[1]).pos_fbk + o->arm_pos_off[1]; @@ -1094,9 +1107,9 @@ static void JointSet_do_current_control(JointSet* o) } } - o->ypr_pos_fbk[0] = DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[0]); + o->ypr_pos_fbk[0] = postfactor*DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[0]); o->ypr_pos_fbk[1] = DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[1]); - o->ypr_pos_fbk[2] = DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[2]); + o->ypr_pos_fbk[2] = postfactor*DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[2]); CTRL_UNITS arm_pos_ref[3];