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

Now the joints can run based only to motor encoder - without AMO sensors - and calibrate in hard stop. #438

Merged
merged 4 commits into from
Nov 13, 2023
Merged

Now the joints can run based only to motor encoder - without AMO sensors - and calibrate in hard stop. #438

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b0cc475
Now the joints can run based only to motor encoder - without AMO sens…
ale-git Nov 7, 2023
d3a1ce4
Updated mc4+ and mc2+ version numbers.
ale-git Nov 10, 2023
2d31a0d
Comments added.
ale-git Nov 13, 2023
626f268
Configurable parameters added to calibration type 10
ale-git Nov 13, 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
4 changes: 2 additions & 2 deletions emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,8 @@ namespace embot { namespace app { namespace eth {
.property =
{
Process::eApplication,
{1, 3},
{2023, Month::Oct, Day::twentyfour, 15, 21}
{1, 4},
{2023, Month::Nov, Day::thirteen, 10, 53}
},
.OStick = 1000*embot::core::time1microsec,
.OSstacksizeinit = 10*1024,
Expand Down
10 changes: 5 additions & 5 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,21 +81,21 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 77
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 78

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 11
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 13
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 15
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 10
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 20
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 53
// </h>build date
// </h>Info

Expand Down
10 changes: 5 additions & 5 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,21 +75,21 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 59
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 60

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 11
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 13
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 15
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 10
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 34
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 53
// </h>build date

// </h>Info
Expand Down
10 changes: 5 additions & 5 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 @@ -84,21 +84,21 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>

#define EOMTHEEMSAPPLCFG_VERSION_MINOR 80
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 81

// </h>version

// <h> build date
// <o> year <2010-2030>
#define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 11
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 13
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 15
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 10
// <o> minute <0-59>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 33
#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 53

// </h>build date

Expand Down
24 changes: 16 additions & 8 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,21 +24,29 @@

typedef struct // TripodCalib
{
int32_t pwm;
int32_t zero;
int32_t max_delta;
int32_t start_pos[3];
int32_t pwm; // [2FOC PWM units] (-32000 : +32000) = (-100% : +100%)
int32_t zero; // [millimiters]
int32_t max_delta; // [millimeters]
int32_t start_pos[3]; // [millimeters]

} TripodCalib;

typedef struct // CableCalib
{
int32_t pwm;
int32_t delta;
int32_t target;
int32_t cable_range;
int32_t pwm; // [MC2+ PWM units] (-3360 : +3360) = (-100% : +100%)
int32_t delta; // [icubdegrees]
int32_t target; // [icubdegrees]
int32_t cable_range; // [icubdegrees]
} CableCalib;

typedef struct // HardStopCalib
{
int32_t pwm; // [2FOC PWM units] (-32000 : +32000) = (-100% : +100%)
int32_t zero; // [icubdegrees]
int32_t space_thr; // [icubdegrees]
int32_t time_thr; // [milliseconds]
} HardStopCalib;



#endif // include-guard
Expand Down
41 changes: 35 additions & 6 deletions 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 @@ -596,24 +596,53 @@ BOOL JointSet_do_wait_calibration_10(JointSet* o)
{
int m = o->motors_of_set[k];

int e = o->encoders_of_set[k];
AbsEncoder *encoder = o->absEncoder+o->encoders_of_set[k];

if (AbsEncoder_is_hard_stop_calibrating(o->absEncoder+e))
if (AbsEncoder_is_fake(encoder))
{
Motor_set_pwm_ref(o->motor+m, o->motor[m].calib_pwm);

if (AbsEncoder_is_still(o->absEncoder+e, 12000, 1000))
if (Motor_is_still(o->motor+m, o->hard_stop_calib.space_thr, o->hard_stop_calib.time_thr))
{
AbsEncoder_calibrate_in_hard_stop(o->absEncoder+e);
o->motor[m].pos_calib_offset += o->motor[m].pos_fbk - o->hard_stop_calib.zero;
o->motor[m].pos_fbk = o->hard_stop_calib.zero;
o->motor[m].not_init = FALSE;
o->motor[m].pos_fbk_old = o->motor[m].pos_fbk;

char message[128];
snprintf(message, sizeof(message), "CALIBRATED!!!");
JointSet_send_debug_message(message, 0, 0, 0);

Motor_set_pwm_ref(o->motor+m, 0);
}
else
{
//char message[128];
//snprintf(message, sizeof(message), "fake ENCODER=%d", o->motor[m].partial_space);
//JointSet_send_debug_message(message, 0, 0, 0);

calibrated = FALSE;
}
}
else
{
Motor_set_pwm_ref(o->motor+m, 0);
if (AbsEncoder_is_hard_stop_calibrating(encoder))
{
Motor_set_pwm_ref(o->motor+m, o->motor[m].calib_pwm);

if (AbsEncoder_is_still(encoder, o->hard_stop_calib.space_thr, o->hard_stop_calib.time_thr))
{
AbsEncoder_calibrate_in_hard_stop(encoder);
}
else
{
calibrated = FALSE;
}
}
else
{
Motor_set_pwm_ref(o->motor+m, 0);
}
}
}

Expand Down
22 changes: 18 additions & 4 deletions 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 @@ -1861,14 +1861,28 @@ void JointSet_calibrate(JointSet* o, uint8_t e, eOmc_calibrator_t *calibrator)
o->calibration_timeout = 0;

Joint_set_hardware_limit(o->joint+e);

Motor_calibrate_withOffset(o->motor+e, 0);
Motor_set_run(o->motor+e, eomc_ctrl_out_type_pwm);
o->motor[e].calib_pwm = calibrator->params.type10.pwmlimit;

AbsEncoder_still_check_reset(o->absEncoder+e);
AbsEncoder_start_hard_stop_calibrate(o->absEncoder+e, calibrator->params.type10.calibrationZero);

if (AbsEncoder_is_fake(o->absEncoder+e))
{
AbsEncoder_calibrate_fake(o->absEncoder+e);
o->hard_stop_calib.zero = calibrator->params.type10.calibrationZero;
o->hard_stop_calib.pwm = calibrator->params.type10.pwmlimit;
o->hard_stop_calib.space_thr = 12000; // we can make them configurable (probably not needed)
o->hard_stop_calib.time_thr = 1000; // we can make them configurable (probably not needed)
}
else
{
o->hard_stop_calib.zero = calibrator->params.type10.calibrationZero;
o->hard_stop_calib.pwm = calibrator->params.type10.pwmlimit;
o->hard_stop_calib.space_thr = 12000; // we can make them configurable (probably not needed)
o->hard_stop_calib.time_thr = 1000; // we can make them configurable (probably not needed)
AbsEncoder_still_check_reset(o->absEncoder+e);
AbsEncoder_start_hard_stop_calibrate(o->absEncoder+e, calibrator->params.type10.calibrationZero);
}
}
break;

Expand Down
2 changes: 2 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,8 @@ typedef struct // JointSet
#endif

TripodCalib tripod_calib;
HardStopCalib hard_stop_calib;

} JointSet;

extern JointSet* JointSet_new(uint8_t n); //
Expand Down
24 changes: 24 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,30 @@ static void Motor_config_MC4p(uint8_t motor, eOmc_motor_config_t* config)
}
*/

BOOL Motor_is_still(Motor* o, int32_t space_window, int32_t time_window)
{
o->partial_space += o->pos_raw_fbk - o->pos_raw_fbk_old;
o->pos_raw_fbk_old = o->pos_raw_fbk;

BOOL still = FALSE;

if (++o->partial_timer > time_window)
{
still = abs(o->partial_space) < space_window;

o->partial_timer = 0;
o->partial_space = 0;
}

return still;
}

void Motor_still_check_reset(Motor* o)
{
o->partial_timer = 0;
o->partial_space = 0;
}

static void Motor_hardStopCalbData_reset(Motor* o)
{
memset(&o->hardstop_calibdata, 0, sizeof(HardStopCalibData));
Expand Down
3 changes: 3 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,9 @@ extern void Motor_init(Motor* o); //
extern void Motor_config(Motor* o, uint8_t ID, eOmc_motor_config_t* config); //
extern void Motor_destroy(Motor* o); //

extern BOOL Motor_is_still(Motor* o, int32_t space_window, int32_t time_window);
extern void Motor_still_check_reset(Motor* o);

extern void Motor_config_encoder(Motor* o, int32_t resolution);
extern void Motor_config_max_currents(Motor* o, eOmc_current_limits_params_t* current_params);

Expand Down
4 changes: 4 additions & 0 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -142,6 +142,9 @@ struct Motor_hid

CTRL_UNITS GEARBOX;

uint32_t partial_timer;
int32_t partial_space;

BOOL HAS_TEMP_SENSOR;
int16_t temperature_max;
int16_t temperature_fbk;
Expand All @@ -154,6 +157,7 @@ struct Motor_hid

int32_t pos_raw_cal_fbk;
int32_t pos_raw_fbk;
int32_t pos_raw_fbk_old;
int32_t vel_raw_fbk;

int32_t pos_calib_offset;
Expand Down