From c5780a5a12cd6ea34480b6739a05c3e97bc76538 Mon Sep 17 00:00:00 2001 From: Alessandro Scalzo Date: Mon, 13 Nov 2023 18:27:08 +0100 Subject: [PATCH] Now the joints can run based only to motor encoder - without AMO sensors - and calibrate in hard stop. (#438) --- .../v1/cfg/theApplication_config.h | 4 +- .../v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 10 ++--- .../v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 10 ++--- .../v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 10 ++--- .../embobj/plus/mc/CalibrationHelperData.h | 24 +++++++---- .../arch-arm/embobj/plus/mc/Calibrators.c | 41 ++++++++++++++++--- .../eBcode/arch-arm/embobj/plus/mc/JointSet.c | 22 ++++++++-- .../eBcode/arch-arm/embobj/plus/mc/JointSet.h | 2 + emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c | 24 +++++++++++ emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h | 3 ++ .../arch-arm/embobj/plus/mc/Motor_hid.h | 4 ++ 11 files changed, 119 insertions(+), 35 deletions(-) diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h index 7fa2539536..1e994a1a1a 100644 --- a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h @@ -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, diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index 05a3899cb8..10219664f2 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -81,7 +81,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3) // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 77 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 78 // version @@ -89,13 +89,13 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 11 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 13 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 15 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 10 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 20 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 53 // build date // Info diff --git a/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index b225a9608b..5fdff3cc8b 100644 --- a/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -75,7 +75,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3 // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 59 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 60 // version @@ -83,13 +83,13 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 11 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 13 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 15 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 10 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 34 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 53 // build date // Info diff --git a/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index ce3dfab75d..5fb3c143fe 100644 --- a/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -84,7 +84,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3) // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 80 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 81 // version @@ -92,13 +92,13 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 11 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 13 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 15 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 10 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 33 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 53 // build date diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h index d80eaea228..1a7773cc6b 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h @@ -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 diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c index 7534f0d8c0..678bc71b99 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c @@ -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); + } } } diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c index 4edbfbd915..c500eff500 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c @@ -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; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h index af8e4be35b..257d60f3fb 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h @@ -123,6 +123,8 @@ typedef struct // JointSet #endif TripodCalib tripod_calib; + HardStopCalib hard_stop_calib; + } JointSet; extern JointSet* JointSet_new(uint8_t n); // diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c index f4671416eb..a6d035e2f3 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c @@ -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)); diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h index 8e4c90dc0e..7c24062a71 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h @@ -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); diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h index e3b4eb6aa4..030fd37874 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h @@ -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; @@ -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;