Skip to content

Commit

Permalink
Some improvements for the ergoCub wrist (robotology#324)
Browse files Browse the repository at this point in the history
  • Loading branch information
@marcoaccame @valegagge
marcoaccame authored and valegagge committed May 5, 2023
1 parent 7e5056d commit a31a69d
Show file tree
Hide file tree
Showing 4 changed files with 107 additions and 13 deletions.
10 changes: 9 additions & 1 deletion emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
26 changes: 23 additions & 3 deletions emBODY/eBcode/arch-arm/board/amcbldc/application/src/amcbldc-main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<embot::prot::can::Frame> rxCANvector {};
embot::os::rtos::mutex_t *txm {nullptr};
std::vector<embot::prot::can::Frame> 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()
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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)
Expand Down
55 changes: 54 additions & 1 deletion emBODY/eBcode/arch-arm/board/amcbldc/application/src/embot_app_application_theMBDagent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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++
// --------------------------------------------------------------------------------------------------------------------
Expand Down Expand Up @@ -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<bool> 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)
Expand Down Expand Up @@ -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; i++)
{
faultvalues.push_back(embot::hw::button::pressed(buttonEXTfault));
}
bool allpressed = std::all_of(faultvalues.cbegin(), faultvalues.cend(), [](auto v){ return v; });
prevEXTFAULTisPRESSED = EXTFAULTisPRESSED = allpressed;
#else
prevEXTFAULTisPRESSED = EXTFAULTisPRESSED = false;
#endif
Expand Down Expand Up @@ -312,6 +336,33 @@ bool embot::app::application::theMBDagent::Impl::initialise()
return initted;
}

void embot::app::application::theMBDagent::Impl::onEXTFAULTpolling()
{
if(faultvalues.size() >= 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)
{
Expand Down Expand Up @@ -343,6 +394,8 @@ bool embot::app::application::theMBDagent::Impl::tick(std::vector<embot::prot::c
{
measureTick->start();

onEXTFAULTpolling();

if(prevEXTFAULTisPRESSED != EXTFAULTisPRESSED)
{
prevEXTFAULTisPRESSED = EXTFAULTisPRESSED;
Expand Down
29 changes: 21 additions & 8 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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];
Expand All @@ -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;
Expand Down Expand Up @@ -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];
Expand All @@ -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];

Expand Down

0 comments on commit a31a69d

Please sign in to comment.