Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add fix for not causing joints to go in hw fault at yri restart #400

Merged
merged 2 commits into from
Jul 26, 2023
Merged

Add fix for not causing joints to go in hw fault at yri restart #400

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2b766ea
Add fix for not causing joints to go in hw fault: limits exceeded at …
MSECode Jul 20, 2023
a5ee68c
removed debug method
MSECode Jul 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/protocol/EoProtocolMC_fun_amc.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ extern void eoprot_fun_UPDT_mc_joint_config(const EOnv* nv, const eOropdescripto

if((eo_motcon_mode_foc == mcmode))
{
MController_config_joint(jxx, cfg);
MController_config_joint(jxx, cfg, mcmode);
}

}
Expand Down
8 changes: 4 additions & 4 deletions emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,14 +40,14 @@ namespace embot { namespace app { namespace eth {
Process::eApplication,
#if defined(WRIST_MK2)
#if defined(WRIST_MK2_RIGHT)
{101, 5},
{101, 6},
#else
{102, 5},
{102, 6},
#endif
#else
{103, 5},
{103, 6},
#endif
{2023, Month::Jul, Day::five, 14, 00}
{2023, Month::Jul, Day::twentyfour, 18, 00}
},
.OStick = 1000*embot::core::time1microsec,
.OSstacksizeinit = 10*1024,
Expand Down
8 changes: 4 additions & 4 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 69
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 70

// </h>version

Expand All @@ -91,11 +91,11 @@ extern "C" {
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 7
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 3
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 12
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 20
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00
// </h>build date
// </h>Info

Expand Down
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoprot-callbacks/EoProtocolMC_fun_ems4rd.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,7 +157,7 @@ extern void eoprot_fun_UPDT_mc_joint_config(const EOnv* nv, const eOropdescripto
if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) ||
(eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode))
{
MController_config_joint(jxx, cfg);
MController_config_joint(jxx, cfg, mcmode);
}
else if(eo_motcon_mode_mc4 == mcmode)
{
Expand Down
1 change: 0 additions & 1 deletion emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheEncoderReader.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -324,7 +324,6 @@ extern eOresult_t eo_encoderreader_Deactivate(EOtheEncoderReader *p)
return(eores_OK);
}


// make sure the timer is not running
eo_timer_Stop(s_eo_theencoderreader.diagnostics.reportTimer);

Expand Down
8 changes: 4 additions & 4 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheMotionController.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -886,7 +886,7 @@ extern eOresult_t eo_motioncontrol_Activate(EOtheMotionController *p, const eOmn
{
eo_motioncontrol_Deactivate(p);
}

// we use memmove() because it may be that source and destination are in the same location.
memmove(&p->service.servconfig, servcfg, sizeof(eOmn_serv_configuration_t));

Expand Down Expand Up @@ -1385,7 +1385,7 @@ extern eOresult_t eo_motioncontrol_Activate(EOtheMotionController *p, const eOmn
extern eOresult_t eo_motioncontrol_Start(EOtheMotionController *p)
{
// eo_errman_Trace(eo_errman_GetHandle(), "called: eo_motioncontrol_Start()", s_eobj_ownname);

if(NULL == p)
{
return(eores_NOK_nullpointer);
Expand Down Expand Up @@ -1485,7 +1485,7 @@ extern eOresult_t eo_motioncontrol_AddRegulars(EOtheMotionController *p, eOmn_se
extern eOresult_t eo_motioncontrol_Tick(EOtheMotionController *p)
{
// static uint32_t adc_values[4] = {0, 0, 0, 0};

if(NULL == p)
{
return(eores_NOK_nullpointer);
Expand Down Expand Up @@ -1558,7 +1558,7 @@ extern eOresult_t eo_motioncontrol_Tick(EOtheMotionController *p)
// 3) update status of motors and joints
s_eo_motioncontrol_UpdateJointMotorStatus(p);


return(eores_OK);
}

Expand Down
6 changes: 3 additions & 3 deletions emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 51
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 52
// </h>version

// <h> build date
Expand All @@ -84,9 +84,9 @@ extern "C" {
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 7
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 3
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 14
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00
// </h>build date
Expand Down
6 changes: 3 additions & 3 deletions emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>

#define EOMTHEEMSAPPLCFG_VERSION_MINOR 72
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 73

// </h>version

Expand All @@ -96,9 +96,9 @@ extern "C" {
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 7
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 3
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 14
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00

Expand Down
4 changes: 4 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -779,5 +779,9 @@ void AbsEncoder_clear_faults(AbsEncoder* o)
o->spike_cnt = 0;
}

BOOL AbsEncoder_is_initialized(AbsEncoder* o)
{
return (!o->state.bits.not_initialized);
}
// AbsEncoder
/////////////////////////////////////////////////////////
2 changes: 2 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/AbsEncoder.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -177,6 +177,8 @@ extern void AbsEncoder_start_hard_stop_calibrate(AbsEncoder* o, int32_t hard_sto
extern BOOL AbsEncoder_is_hard_stop_calibrating(AbsEncoder* o);
extern void AbsEncoder_calibrate_in_hard_stop(AbsEncoder* o);

extern BOOL AbsEncoder_is_initialized(AbsEncoder* o);


#endif // include-guard

Expand Down
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -798,7 +798,7 @@ BOOL JointSet_do_wait_calibration_14(JointSet* o)

//Debug code
char info[80];
snprintf(info, sizeof(info), "Calib Completed" );
snprintf(info, sizeof(info), "Calib Completed.");
JointSet_send_debug_message(info, j_ptr->ID, 0, 0);
//ended

Expand Down
32 changes: 29 additions & 3 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1551,7 +1551,7 @@ void MController_config_board(const eOmn_serv_configuration_t* brd_cfg)

#endif

void MController_config_joint(int j, eOmc_joint_config_t* config) //
void MController_config_joint(int j, eOmc_joint_config_t* config, eOmotioncontroller_mode_t mcmode) //
{
MController *o = smc;

Expand All @@ -1569,8 +1569,16 @@ void MController_config_joint(int j, eOmc_joint_config_t* config) //

for(int e=0; e< o->multi_encs; e++)
{
AbsEncoder_config(o->absEncoder+j*o->multi_encs+e, j, config->jntEncoderResolution, config->jntEncTolerance);
AbsEncoder_config_divisor(smc->absEncoder+j, config->gearbox_E2J);
//if encoder alredy initialized and mcmode == eo_motcon_mode_mc4plusmais
if(mcmode == eo_motcon_mode_mc4plusmais && AbsEncoder_is_initialized(o->absEncoder+j*o->multi_encs+e))
{
continue;
}
else
{
AbsEncoder_config(o->absEncoder+j*o->multi_encs+e, j, config->jntEncoderResolution, config->jntEncTolerance);
AbsEncoder_config_divisor(smc->absEncoder+j, config->gearbox_E2J);
}
}
}

Expand Down Expand Up @@ -1855,6 +1863,24 @@ void MController_set_interaction_mode(uint8_t j, eOmc_interactionmode_t interact
JointSet_set_interaction_mode(smc->jointSet+smc->j2s[j], interaction_mode);
}


void MController_Debug_Enc_values(const char * msg)
{

for (int s=0; s<smc->nSets; ++s)
{
JointSet* o = smc->jointSet+s;
AbsEncoder* e_ptr = o->absEncoder+o->encoders_of_set[0];
Joint* j_ptr = o->joint+o->joints_of_set[0];

int32_t enc_pos = AbsEncoder_position(e_ptr);
char info[256];
snprintf(info, sizeof(info), "%s p:%d d:%d z:%d m:%d dd:%.4f", msg, enc_pos, e_ptr->distance, e_ptr->zero, e_ptr->mul, e_ptr->div );
JointSet_send_debug_message(info, j_ptr->ID, 0, 0);
}
}


// --------------------------------------------------------------------------------------------------------------------
// - definition of static functions
// --------------------------------------------------------------------------------------------------------------------
Expand Down
10 changes: 9 additions & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,8 @@
extern "C" {
#endif

#include "EOtheMotionController.h" //TODO:to be removed


// to see basic types
#include "EoCommon.h"
Expand All @@ -53,7 +55,7 @@ extern void MController_init(void); //
extern void MController_deinit(void); //
extern void MController_config_board(const eOmn_serv_configuration_t* brd_cfg);

extern void MController_config_joint(int j, eOmc_joint_config_t* config); //
extern void MController_config_joint(int j, eOmc_joint_config_t* config, eOmotioncontroller_mode_t mcmode); //
extern void MController_config_motor(int m, eOmc_motor_config_t* config); //
extern void MController_config_Jjm(float **Jjm); //
extern void MController_config_Jje(float **Jje); //
Expand Down Expand Up @@ -116,6 +118,12 @@ extern void MController_update_motor_odometry_fbk_can(int m, void* data);
extern void MController_motor_raise_fault_i2t(int m);


extern void MController_Debug_Enc_values(const char * msg);
////////////////////////////////////////////////////////////////////////

//VALE: debug function. I'll remove it ASAP
//void MController_updated_debug_current_info(int j, int32_t avgCurrent, int32_t accum_Ep);

#ifdef __cplusplus
} // closing brace for extern "C"
#endif
Expand Down
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2130,7 +2130,7 @@ void JointSet_calibrate(JointSet* o, uint8_t e, eOmc_calibrator_t *calibrator)
{
//debug code
char info[70];
snprintf(info, 70, "Error! POS service NOT active");
snprintf(info, sizeof(info), "Error! POS service NOT active");
JointSet_send_debug_message(info, e, 0, 0);
//debug code ended
return;
Expand Down