The amc board uses ICC and CAN to move the wrist of ergoCub (#93

)

CMakeLists.txt

@@ -6,7 +6,7 @@
 cmake_minimum_required(VERSION 3.12)
 
 project(icub_firmware_shared
-        VERSION 1.38.0)
+        VERSION 1.38.1)
 
 find_package(YCM 0.11.0 REQUIRED)
 

can/canProtocolLib/iCubCanProto_types.h

@@ -78,7 +78,7 @@ extern "C" {
 #define ICUBCANPROTO_BOARDTYPE__AMCBLDC  18
 #define ICUBCANPROTO_BOARDTYPE__BMS      19
 #define ICUBCANPROTO_BOARDTYPE__MTB4C    20
-#define ICUBCANPROTO_BOARDTYPE__FFU      21
+#define ICUBCANPROTO_BOARDTYPE__AMC2C    21
 #define ICUBCANPROTO_BOARDTYPE__STRAIN2C 22
 #define ICUBCANPROTO_BOARDTYPE__BAT      23
 #define ICUBCANPROTO_BOARDTYPE__UNKNOWN  255
@@ -356,6 +356,11 @@ typedef struct
     int16_t                           reserved;
 } icubCanProto_calibrator_params_type7_hall_sensor_t;
 
+typedef struct  
+{
+    uint32_t undefined;
+} icubCanProto_calibrator_params_type_undefined_t;
+
 
 /** @typedef    typedef enum icubCanProto_calibration_type_t
     @brief      contains the possible types of calibration.
@@ -370,7 +375,8 @@ typedef enum
     icubCanProto_calibration_type3_abs_sens_digital         = 3,
     icubCanProto_calibration_type4_abs_and_incremental      = 4,
     icubCanProto_calibration_type6_mais                     = 6,
-    icubCanProto_calibration_type7_hall_sensor              = 7
+    icubCanProto_calibration_type7_hall_sensor              = 7,
+    icubCanProto_calibration_type_undefined                 = 255
 } icubCanProto_calibration_type_t;
 
 
@@ -391,6 +397,7 @@ typedef struct
         icubCanProto_calibrator_params_type4_abs_and_incremental_t      type4;
         icubCanProto_calibrator_params_type6_mais_t                     type6;
         icubCanProto_calibrator_params_type7_hall_sensor_t              type7;
+        icubCanProto_calibrator_params_type_undefined_t                 typeundefined;
     } params;                                                       /**< the params of the calibrator */   
 } icubCanProto_calibrator_t;
 

embot/core/embot_core.h

@@ -95,6 +95,7 @@ namespace embot { namespace core {
     using fpI08ParU32   = std::int8_t (*)(std::uint32_t);
     using fpWorker      = void (*)(void);
     using fpCaller      = void (*)(void *);
+    using fpResult      = void (*)(void *, bool);
     using fpPrint       = int (*)(const std::string &str);
 
     // useful converter for enum class
@@ -111,13 +112,26 @@ namespace embot { namespace core {
         fpCaller call {nullptr};
         void * arg {nullptr};
 
-        Callback() = default;
+        constexpr Callback() = default;
         constexpr Callback(fpCaller _c, void *_a) : call(_c), arg(_a) {}
         void load(fpCaller _c, void *_a) { call = _c; arg = _a; }
         void clear() { call = nullptr; arg = nullptr; }
         bool isvalid() const { if(nullptr != call) { return true; } else { return false; } } 
         void execute() const { if(true == isvalid()) { call(arg); } }
     };
+
+    struct Confirmer
+    {
+        fpResult call {nullptr};
+        void * arg {nullptr};
+
+        constexpr Confirmer() = default;
+        constexpr Confirmer(fpResult _c, void *_a) : call(_c), arg(_a) {}
+        void load(fpResult _c, void *_a) { call = _c; arg = _a; }
+        void clear() { call = nullptr; arg = nullptr; }
+        bool isvalid() const { if(nullptr != call) { return true; } else { return false; } } 
+        void execute(bool result) const { if(true == isvalid()) { call(arg, result); } }
+    };    
 
     // it is a holder of data, either mutable or constexpr
     // we dont want to change what is inside pointer unless basic initialization

eth/embobj/core/core/EoCommon.h

@@ -80,8 +80,8 @@ extern "C" {
 #include "stdio.h"
 
 
-
-
+// issues a compiler error if the sizeof the struct is not what in second argument
+#define EO_VERIFYsizeof(sname, ssize)       __emBODYportingVERIFYsizeof(sname, ssize)
 
 // - declaration of public user-defined types -------------------------------------------------------------------------
 
@@ -605,6 +605,21 @@ typedef struct
     uint8_t         val0;    
 } eOmap_int32_u08_t;
 
+
+// --
+
+typedef enum
+{ 
+    eobus_can1 = 0, eobus_can2 = 1, eobus_local = 2, eobus_icc1 = 3, eobus_icc2 = 4, eobus_ffu1 = 5, eobus_ffu2 = 6, eobus_none = 7             
+} eObus_t; 
+
+typedef struct
+{
+    uint8_t bus:3; // use only values from eObus_t
+    uint8_t ffu:1; 
+    uint8_t adr:4;      
+} eOlocation_t; EO_VERIFYsizeof(eOlocation_t, 1)
+
 // - public #define  --------------------------------------------------------------------------------------------------
 
 
@@ -620,10 +635,6 @@ typedef struct
 #define EO_CLIP_UINT16(u16)                 ( ((u16)>EO_UINT16_MAX) ? (EO_UINT16_MAX) : (u16) )
 
 
-
-// issues a compiler error if the sizeof the struct is not what in second argument
-#define EO_VERIFYsizeof(sname, ssize)       __emBODYportingVERIFYsizeof(sname, ssize)
-
 // issues a compiler error if the prop is false
 #define TOKENPASTE(x, y) x ## y
 #define TOKENPASTE2(x, y) TOKENPASTE(x, y)

eth/embobj/plus/comm-v2/icub/EoBoards.c

@@ -94,6 +94,7 @@ static const uint64_t s_eoboards_is_can_mask =  (0x1LL << eobrd_mc4) |
                                                 (0x1LL << eobrd_amcbldc)|
                                                 (0x1LL << eobrd_bms)|
                                                 (0x1LL << eobrd_mtb4c) |
+                                                (0x1LL << eobrd_amc2c)|
                                                 (0x1LL << eobrd_strain2c) | 
                                                 (0x1LL << eobrd_bat);
 
@@ -127,13 +128,18 @@ static const eOmap_str_str_u08_t s_eoboards_map_of_boards[] =
     {"amcbldc", "eobrd_amcbldc", eobrd_amcbldc},
     {"bms", "eobrd_bms", eobrd_bms},
     {"mtb4c", "eobrd_mtb4c", eobrd_mtb4c},
+    {"amc2c", "eobrd_amc2c", eobrd_amc2c},
     {"strain2c", "eobrd_strain2c", eobrd_strain2c},
     {"bat", "eobrd_bat", eobrd_bat},
 
     {"none", "eobrd_none", eobrd_none},
     {"unknown", "eobrd_unknown", eobrd_unknown}
 };  EO_VERIFYsizeof(s_eoboards_map_of_boards, (eobrd_type_numberof+2)*sizeof(eOmap_str_str_u08_t))
 
+// just to check that the number of can type board is lower than 32 otherwise we go up to the ems.
+// if that happens, the can boards will have a hole between [32, 63] ...
+static const uint8_t xxx[eobrd_ems4-eobrd_cantype_numberof+1] = {0};
+
 
 
 static const eOmap_str_str_u08_t s_eoboards_map_of_connectors[] =

eth/embobj/plus/comm-v2/icub/EoBoards.h

@@ -77,14 +77,15 @@ typedef enum
     eobrd_cantype_pmc               = ICUBCANPROTO_BOARDTYPE__PMC,      // 17 (pmc = piezo motor control)
     eobrd_cantype_amcbldc           = ICUBCANPROTO_BOARDTYPE__AMCBLDC,  // 18 (amcbldc)
     eobrd_cantype_bms               = ICUBCANPROTO_BOARDTYPE__BMS,      // 19 (bms)
-    eobrd_cantype_mtb4c             = ICUBCANPROTO_BOARDTYPE__MTB4C,    // 20 (mtb4c)     
+    eobrd_cantype_mtb4c             = ICUBCANPROTO_BOARDTYPE__MTB4C,    // 20 (mtb4c)  
+    eobrd_cantype_amc2c             = ICUBCANPROTO_BOARDTYPE__AMC2C,    // 21 (amc2c)    
     eobrd_cantype_strain2c          = ICUBCANPROTO_BOARDTYPE__STRAIN2C, // 22 (strain2c)
     eobrd_cantype_bat               = ICUBCANPROTO_BOARDTYPE__BAT,      // 23 (bat)
     eobrd_cantype_none              = 254, 	
     eobrd_cantype_unknown           = ICUBCANPROTO_BOARDTYPE__UNKNOWN   // 255 
 } eObrd_cantype_t;
 
-enum { eobrd_cantype_numberof = 23 };
+enum { eobrd_cantype_numberof = 24 };
 
 
 typedef enum
@@ -134,14 +135,15 @@ typedef enum
     eobrd_amcbldc               = eobrd_cantype_amcbldc,
     eobrd_bms                   = eobrd_cantype_bms,
     eobrd_mtb4c                 = eobrd_cantype_mtb4c,
+    eobrd_amc2c                 = eobrd_cantype_amc2c,
     eobrd_strain2c              = eobrd_cantype_strain2c,
     eobrd_bat                   = eobrd_cantype_bat,
 
     eobrd_none                  = 254,                      
     eobrd_unknown               = 255  // = ICUBCANPROTO_BOARDTYPE__UNKNOWN                     
 } eObrd_type_t;
 
-enum { eobrd_type_numberof = 27 };
+enum { eobrd_type_numberof = 28 };
 
 
 typedef struct                  

eth/embobj/plus/comm-v2/icub/EoManagement.c

@@ -84,7 +84,7 @@ static const char * s_mn_servicetype_strings[] =
     "eomn_serv_AS_psc",
     "eomn_serv_AS_pos",
     "eomn_serv_MC_mc4plusfaps",
-    "eomn_serv_MC_mc4pluspmc",
+    "eomn_serv_MC_advfoc",
     "eomn_serv_AS_ft",
     "eomn_serv_AS_battery"
 };  EO_VERIFYsizeof(s_mn_servicetype_strings, eomn_serv_types_numberof*sizeof(const char *))   

eth/embobj/plus/comm-v2/icub/EoManagement.h

@@ -59,7 +59,7 @@ extern "C" {
 
 // it allows to fit a EOarray of 64 bytes (or 16 words)
 #define EOMANAGEMENT_COMMAND_DATA_SIZE 68
-
+//#warning: review it
 // - declaration of public user-defined types ------------------------------------------------------------------------- 
 
 
@@ -461,7 +461,7 @@ typedef enum
     eomn_serv_AS_psc            = 12,
     eomn_serv_AS_pos            = 13,
     eomn_serv_MC_mc4plusfaps    = 14,
-    eomn_serv_MC_mc4pluspmc     = 15,
+    eomn_serv_MC_advfoc         = 15,
     eomn_serv_AS_ft             = 16,
     eomn_serv_AS_battery        = 17,
     eomn_serv_UNKNOWN           = 254,
@@ -591,7 +591,7 @@ typedef struct
 
 
 typedef struct
-{   // 24+292=268
+{   // 24+292=316
     eOmc_arrayof_4jomodescriptors_t         arrayofjomodescriptors; 
     eOmc_4jomo_coupling_t                   jomocoupling;   
 } eOmn_serv_config_data_mc_mc4plus_t;       EO_VERIFYsizeof(eOmn_serv_config_data_mc_mc4plus_t, 316)
@@ -607,7 +607,7 @@ typedef struct
 
 
 typedef struct
-{   // 24+292=268
+{   // 24+292=316
     eOmc_arrayof_4jomodescriptors_t         arrayofjomodescriptors; 
     eOmc_4jomo_coupling_t                   jomocoupling;   
 } eOmn_serv_config_data_mc_mc2plus_t;       EO_VERIFYsizeof(eOmn_serv_config_data_mc_mc2plus_t, 316)
@@ -628,25 +628,23 @@ typedef struct
     eOmc_4jomo_coupling_t                   jomocoupling;
 } eOmn_serv_config_data_mc_mc4plusfaps_t;   EO_VERIFYsizeof(eOmn_serv_config_data_mc_mc4plusfaps_t, 340)
 
-// we have 4 dc joints moved by the mc4plus + 3 joints moved by pmc. all of them are independent
+
 typedef struct
-{   // 24 + 39 + 1 + 116 = 180
-    eOmn_serv_config_data_as_pos_t          pos;
-    eOmc_arrayof_7jomodescriptors_t         arrayof7jomodescriptors;  
-    uint8_t                                 dummy[1];
-    eOmc_arrayof_7jointsetconfig_t          arrayof7jointsets;
-} eOmn_serv_config_data_mc_mc4pluspmc_t;    EO_VERIFYsizeof(eOmn_serv_config_data_mc_mc4pluspmc_t, 180)
+{   // 68+264 = 332   
+    eOmc_arrayof_4advjomodescriptors_t      arrayof4advjomodescriptors;  
+    eOmc_adv4jomo_coupling_t                adv4jomocoupling;  
+} eOmn_serv_config_data_mc_advfoc_t;        EO_VERIFYsizeof(eOmn_serv_config_data_mc_advfoc_t, 332)
 
 typedef union                               
-{   // max(324, 28, 316, 328, 340, 180)
+{   // max(324, 28, 316, 324, 316, 328, 340, 332)
     eOmn_serv_config_data_mc_foc_t          foc_based;
     eOmn_serv_config_data_mc_mc4_t          mc4_based;
     eOmn_serv_config_data_mc_mc4plus_t      mc4plus_based;
     eOmn_serv_config_data_mc_mc4plusmais_t  mc4plusmais_based;
     eOmn_serv_config_data_mc_mc2plus_t      mc2plus;
     eOmn_serv_config_data_mc_mc2pluspsc_t   mc2pluspsc;
     eOmn_serv_config_data_mc_mc4plusfaps_t  mc4plusfaps;
-    eOmn_serv_config_data_mc_mc4pluspmc_t   mc4pluspmc;
+    eOmn_serv_config_data_mc_advfoc_t       advfoc; 
 } eOmn_serv_config_data_mc_t;               EO_VERIFYsizeof(eOmn_serv_config_data_mc_t, 340) 
 
 typedef union                               
@@ -658,7 +656,7 @@ typedef union
 
 
 typedef struct                              
-{   // 1+3+340=344
+{   // 1+1+2+340=344
     uint8_t                                 type;               // use eOmn_serv_type_t to identify what kind of service it is
     uint8_t                                 diagnosticsmode;    // use eOmn_serv_diagn_mode_t
     uint16_t                                diagnosticsparam;   // i cannot fit eOmn_serv_diagn_cfg_t inside here because of alignment and i want to keep backwards compatibility
@@ -702,10 +700,10 @@ typedef enum
 
 
 typedef union
-{  //max( 344, 168)
+{   // max(344, 168) 
     eOmn_serv_configuration_t   configuration;
     eOmn_serv_arrayof_id32_t    arrayofid32;
-} eOmn_serv_parameter_t; EO_VERIFYsizeof(eOmn_serv_parameter_t, 344)
+} eOmn_serv_parameter_t;        EO_VERIFYsizeof(eOmn_serv_parameter_t, 344) 
 
 
 typedef struct                                
@@ -720,8 +718,18 @@ typedef struct
 typedef struct
 {   // 348
     eOmn_service_cmmnds_command_t           command;    
-} eOmn_service_cmmnds_t;                    EO_VERIFYsizeof(eOmn_service_cmmnds_t, 348)
+} eOmn_service_cmmnds_t;                    EO_VERIFYsizeof(eOmn_service_cmmnds_t, 348) 
 
+#if 0
+
+    rop -> [header-8B][data][extra-signature-4B][extra-time-8B]
+    we transmit the set<eOmn_service_cmmnds_t> with only extra-signature but NOT with extra-time,
+    so... we must configure maxROPsize to be
+      maxROPsize > (sizeof(eOmn_service_cmmnds_t) + 8 + 4) = 360
+    we have a maxROPsize = 384 and that is OK
+    we wnat to keep this maxROPsize, so we cannot have a sizeof(eOmn_service_cmmnds_t) > 384 - 12 = 372    
+
+#endif
 
 typedef struct
 {   // 1+1+1+1+28=32
@@ -757,7 +765,7 @@ typedef struct
 {   // 48+348=396    
     eOmn_service_status_t                   status;
     eOmn_service_cmmnds_t                   cmmnds;
-} eOmn_service_t;                           EO_VERIFYsizeof(eOmn_service_t, 396)  
+} eOmn_service_t;                           // EO_VERIFYsizeof(eOmn_service_t, 396)  
 
 
 // - declaration of extern public variables, ... but better using use _get/_set instead -------------------------------

eth/embobj/plus/comm-v2/icub/EoMotionControl.c

@@ -73,7 +73,7 @@ static const eOmap_str_str_u08_t s_eomc_map_of_actuators[] =
     {"foc", "eomc_act_foc", eomc_act_foc},
     {"mc4", "eomc_act_mc4", eomc_act_mc4},
     {"pwm", "eomc_act_pwm", eomc_act_pwm},
-    {"pmc", "eomc_act_pmc", eomc_act_pmc},
+    {"advfoc", "eomc_act_advfoc", eomc_act_advfoc},
 
     {"none", "eomc_act_none", eomc_act_none},
     {"unknown", "eomc_act_unknown", eomc_act_unknown}