Cppcheck report - ethercatcpp-shadow: /builds/ethercatcpp/ethercatcpp-shadow/src/ethercat_master/hand_controller.cpp

/*      File: hand_controller.cpp
 *       This file is part of the program ethercatcpp-shadow
 *       Program description : EtherCAT driver library for shadow hand.
 *       Copyright (C) 2018-2022 -  Robin Passama (CNRS/LIRMM) Arnaud Meline
 * (CNRS/LIRMM) Benjamin Navarro (CNRS/LIRMM). All Right reserved.
 *
 *       This software is free software: you can redistribute it and/or modify
 *       it under the terms of the CeCILL-C license as published by
 *       the CEA CNRS INRIA, either version 1
 *       of the License, or (at your option) any later version.
 *       This software is distributed in the hope that it will be useful,
 *       but WITHOUT ANY WARRANTY without even the implied warranty of
 *       MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 *       CeCILL-C License for more details.
 *
 *       You should have received a copy of the CeCILL-C License
 *       along with this software. If not, it can be found on the official
 * website of the CeCILL licenses family (http://www.cecill.info/index.en.html).
 */
#include "shadow_hand_buffers_definition.h"
#include <pid/log.h>
#include <ethercatcpp/shadow/hand_controller.h>

#include <limits>
#include <string>

namespace ethercatcpp {

using namespace shadow;

ShadowHandController::MotorControllerConfiguration::
    MotorControllerConfiguration() {
    f_gains.fill(300);
    p_gains.fill(6200);
    i_gains.fill(150);
    d_gains.fill(0);
    i_max = {70, 80, 120, 70, 70, 120, 70, 70, 70, 70,
             70, 70, 120, 70, 70, 120, 70, 70, 70, 70};
    pwm_max.fill(1023);
    sg_refs.fill(0);
    deadbands.fill(5);
    backlash_compensation.fill(
        MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_ENABLE);
    std::fill(backlash_compensation.begin() + 16, backlash_compensation.end(),
              MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_DISABLE);
}

ShadowHandController::~ShadowHandController() = default;

ShadowHandController::ShadowHandController(HandType hand_type,
                                           BiotacMode biotac_electrode_mode,
                                           ControlMode control_mode,
                                           uint32_t serial_number)
    : SlaveDevice(),
      biotac_electrode_mode_(biotac_electrode_mode),
      control_mode_{control_mode} {

    setup_control_signs(hand_type);

    motor_commands_.fill(0);

    // Slave specifications
    set_id("ShadowHandController", 0x530, 0x6);

    // Setting the serial number will make sure that the correct device is
    // present
    set_serial_number(serial_number);

    // Physical buffers configs
    // Enum for select type of syncmanager buffer
    // 1 => asynchro mailbox out (from master to slave)
    // 2 => asynchro mailbox in (from slave to master)
    // 3 => synchro buffer out (from master to slave)
    // 4 => synchro buffer in (from slave to master)

    define_physical_buffer<buffer_shadow_out_command_t>(
        SYNCHROS_OUT, 0x1000, 0x00010126); // size 0x003a
    define_physical_buffer<buffer_shadow_can_t>(SYNCHROS_OUT, 0x103a,
                                                0x00010126); // size 0x000c

    define_physical_buffer<buffer_shadow_in_status_t>(
        SYNCHROS_IN, 0x1046, 0x00010002); // size 0x00dc
    define_physical_buffer<buffer_shadow_can_t>(SYNCHROS_IN, 0x1122,
                                                0x00010002); // size 0x000c

    // Setting physical matching between motors and joints for this hand (left
    // or right)
    load_joints_To_motors_matching(hand_type);

    // defining steps

    //----------------------------------------------------------------------------//
    //                     I N I T     S T E P S //
    //----------------------------------------------------------------------------//

    enum class MotorGroup { Even, Odd };

    auto set_init_configuration =
        [this](MotorGroup group, TO_MOTOR_DATA_TYPE data_type,
               uint32_t tactile_data_type,
               const std::array<int16_t, NUM_MOTORS>& data,
               int16_t min = std::numeric_limits<int16_t>::min(),
               int16_t max = std::numeric_limits<int16_t>::max()) {
            auto buff =
                this->output_buffer<buffer_shadow_out_command_t>(0x1000);
            buff->EDC_command = EDC_COMMAND_SENSOR_DATA;
            buff->from_motor_data_type = MOTOR_DATA_FLAGS;
            buff->which_motors = static_cast<int16_t>(group);
            buff->to_motor_data_type = data_type;
            for (unsigned int id_motor = 0; id_motor < motor_commands_.size();
                 ++id_motor) {
                // Check and saturate then set values
                if (data[id_motor] <= min) {
                    buff->motor_data[id_motor] = min;
                } else if (data[id_motor] >= max) {
                    buff->motor_data[id_motor] = max;
                } else {
                    buff->motor_data[id_motor] = data[id_motor];
                }
            }
            buff->tactile_data_type = tactile_data_type;
        };

    enum ReadSensor {
        Torque = 1 << 0,
        Position = 1 << 1,
        Biotac = 1 << 2,
    };
    auto read_buffer = [this](int sensors) {
        auto buff = this->input_buffer<buffer_shadow_in_status_t>(
            0x1046); // Get buffer pointer
        if (buff->EDC_command == EDC_COMMAND_SENSOR_DATA) {
            if (sensors & ReadSensor::Torque) {
                update_motors_torques();
            }
            if (sensors & ReadSensor::Position) {
                update_joints_position();
            }
            if (sensors & ReadSensor::Biotac) {
                update_biotac_data();
            }
        }
    };

    // Step used to enable torque demand value, activate BioTacs and positions
    // sensors
    add_init_step(
        [this, set_init_configuration]() {
            // build basic control request from attributes
            // Build a init command to active shadow control and status data
            // and ask to shadow to update status datas for next cycle.
            std::array<int16_t, NUM_MOTORS> commands;
            commands.fill(0);

            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            set_init_configuration(MotorGroup::Even,
                                   control_mode_ == ControlMode::Torque
                                       ? MOTOR_DEMAND_TORQUE
                                       : MOTOR_DEMAND_PWM,
                                   TACTILE_BIOTAC_PDC_AND_TAC, commands);
        },
        [] {});

    ////////////////////////////////////////////////////////////////////////////
    //                INIT INTERNAL MOTORS CONTROLLERS                        //
    ////////////////////////////////////////////////////////////////////////////
    // Step to initialize Feed forward gain for internal motors controllers
    // and init biotacs datas
    add_init_step(
        [this, set_init_configuration]() {
            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            set_init_configuration(MotorGroup::Even, MOTOR_CONFIG_F,
                                   TACTILE_BIOTAC_TDC_AND_ELECTRODE_1,
                                   motor_controller_configuration_.f_gains,
                                   MOTOR_CONFIG_F_RANGE_MIN,
                                   MOTOR_CONFIG_F_RANGE_MAX);
        },
        [read_buffer]() { read_buffer(ReadSensor::Biotac); });

    // Step to initialize Proportional gain for internal motors controllers
    // and init biotacs datas
    add_init_step(
        [this, set_init_configuration]() {
            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            set_init_configuration(MotorGroup::Even, MOTOR_CONFIG_P,
                                   TACTILE_BIOTAC_ELECTRODE_2_AND_3,
                                   motor_controller_configuration_.p_gains,
                                   MOTOR_CONFIG_P_RANGE_MIN,
                                   MOTOR_CONFIG_P_RANGE_MAX);
        },
        [read_buffer]() { read_buffer(ReadSensor::Biotac); });

    // Step to initialize Integral gain for internal motors controllers
    // and init biotacs datas if ask by user
    add_init_step(
        [this, set_init_configuration]() {
            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            set_init_configuration(MotorGroup::Even, MOTOR_CONFIG_I,
                                   TACTILE_BIOTAC_ELECTRODE_4_AND_5,
                                   motor_controller_configuration_.i_gains,
                                   MOTOR_CONFIG_I_RANGE_MIN,
                                   MOTOR_CONFIG_I_RANGE_MAX);
        },
        [this, read_buffer]() {
            if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                read_buffer(ReadSensor::Biotac);
            }
        });

    // Step to initialize Derivative gain for internal motors controllers
    // and init biotacs datas if ask by user
    add_init_step(
        [this, set_init_configuration]() {
            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            set_init_configuration(MotorGroup::Even, MOTOR_CONFIG_D,
                                   TACTILE_BIOTAC_ELECTRODE_6_AND_7,
                                   motor_controller_configuration_.d_gains,
                                   MOTOR_CONFIG_D_RANGE_MIN,
                                   MOTOR_CONFIG_D_RANGE_MAX);
        },
        [this, read_buffer]() {
            if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                read_buffer(ReadSensor::Biotac);
            }
        });

    // Step to initialize Maximum integral windup gain for internal motors
    // controllers
    // and init biotacs datas if ask by user
    add_init_step(
        [this, set_init_configuration]() {
            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            set_init_configuration(MotorGroup::Even, MOTOR_CONFIG_IMAX,
                                   TACTILE_BIOTAC_ELECTRODE_8_AND_9,
                                   motor_controller_configuration_.i_max,
                                   MOTOR_CONFIG_IMAX_RANGE_MIN,
                                   MOTOR_CONFIG_IMAX_RANGE_MAX);
        },
        [this, read_buffer]() {
            if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                read_buffer(ReadSensor::Biotac);
            }
        });

    // Step to initialize Max_pwm value for internal motors controllers
    // and init biotacs datas if ask by user
    add_init_step(
        [this, set_init_configuration]() {
            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            set_init_configuration(MotorGroup::Even, MOTOR_CONFIG_MAX_PWM,
                                   TACTILE_BIOTAC_ELECTRODE_10_AND_11,
                                   motor_controller_configuration_.pwm_max,
                                   MOTOR_DEMAND_PWM_RANGE_MIN,
                                   MOTOR_DEMAND_PWM_RANGE_MAX);
        },
        [this, read_buffer]() {
            if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                read_buffer(ReadSensor::Biotac);
            }
        });

    // Step to initialize Strain gauge amp reference value for internal motors
    // controllers and init biotacs datas if ask by user
    add_init_step(
        [this, set_init_configuration]() {
            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            set_init_configuration(MotorGroup::Even, MOTOR_CONFIG_SG_REFS,
                                   TACTILE_BIOTAC_ELECTRODE_12_AND_13,
                                   motor_controller_configuration_.sg_refs);
        },
        [this, read_buffer]() {
            if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                read_buffer(ReadSensor::Biotac);
            }
        });

    // Step to initialize deadband and sign value for internal motors
    // controllers and init biotacs datas if ask by user
    add_init_step(
        [this, set_init_configuration]() {
            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            set_init_configuration(MotorGroup::Even, MOTOR_CONFIG_DEADBAND_SIGN,
                                   TACTILE_BIOTAC_ELECTRODE_14_AND_15,
                                   motor_controller_configuration_.deadbands,
                                   MOTOR_CONFIG_DEADBAND_RANGE_MIN,
                                   MOTOR_CONFIG_DEADBAND_RANGE_MAX);
        },
        [this, read_buffer]() {
            if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                read_buffer(ReadSensor::Biotac);
            }
        });

    // Step to valid config by sending CRC to motors
    // and init biotacs datas if ask by user
    add_init_step(
        [this, set_init_configuration]() {
            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            std::array<int16_t, NUM_MOTORS> crc;
            for (unsigned int id_motor = 0; id_motor < motor_commands_.size();
                 ++id_motor) {
                crc[id_motor] = static_cast<int16_t>(
                    compute_configuration_motor_crc(id_motor));
            }
            set_init_configuration(MotorGroup::Even, MOTOR_CONFIG_CRC,
                                   TACTILE_BIOTAC_ELECTRODE_16_AND_17, crc);
        },
        [this, read_buffer]() {
            if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                read_buffer(ReadSensor::Biotac);
            }
        });

    // Step to initialize backlash compensation for internal motors controllers
    // and init biotacs datas if ask by user
    add_init_step(
        [this, set_init_configuration]() {
            // Ask a valid data to  tactile in order to  obtain valid value on
            // ADC  position sensor of joint  1 and 2 ...
            set_init_configuration(
                MotorGroup::Even, MOTOR_SYSTEM_CONTROLS,
                TACTILE_BIOTAC_ELECTRODE_18_AND_19,
                motor_controller_configuration_.backlash_compensation);
        },
        [this, read_buffer]() {
            if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                read_buffer(ReadSensor::Biotac);
            }
        });

    ////////////////////////////////////////////////////////////////////////////
    //            END OF INIT INTERNAL MOTORS CONTROLLERS                     //
    ////////////////////////////////////////////////////////////////////////////

    // Step used to enable torque demand value, activate BioTacs and positions
    // sensors
    add_init_step(
        [this, set_init_configuration]() {
            // Build basic control request (output buffers) from attributes
            // Build a init command to active shadow controle and status datas
            // Update joints positions and electrode at init to have valid value
            // at start !
            // -> Configure all motors in torque control
            // -> Send a null torque command to all motors
            // -> Ask to get odd motors mesured torque to next step
            // -> Ask to get Tactiles BioTacs Pdc and Tac to next step
            // Ask a valid data to tactile in order to obtain valid value on ADC
            // position sensor of joint 1 and 2 ...
            std::array<int16_t, NUM_MOTORS> torques;
            torques.fill(0);
            set_init_configuration(MotorGroup::Odd,
                                   control_mode_ == ControlMode::Torque
                                       ? MOTOR_DEMAND_TORQUE
                                       : MOTOR_DEMAND_PWM,
                                   TACTILE_BIOTAC_PDC_AND_TAC, torques);
        },
        [this]() {
            auto buff = this->input_buffer<buffer_shadow_in_status_t>(0x1046);
            if (buff->EDC_command == EDC_COMMAND_SENSOR_DATA) {
                update_joints_position();
                if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                    update_biotac_data();
                }
            }
        });

    //----------------------------------------------------------------------------//
    //                      R U N I N G     S T E P S //
    //----------------------------------------------------------------------------//

    // Shadow Hand need 2 steps to command and to get all motors torque values
    auto prepare_buffer = [this](MotorGroup group, uint32_t tactile_data_type) {
        auto buff = this->output_buffer<buffer_shadow_out_command_t>(0x1000);
        buff->EDC_command = EDC_COMMAND_SENSOR_DATA;
        buff->from_motor_data_type = MOTOR_DATA_FLAGS;
        buff->which_motors = group == MotorGroup::Even ? 0 : 1;
        if (control_mode_ == ControlMode::PWM) {
            buff->to_motor_data_type = MOTOR_DEMAND_PWM;
            for (size_t i = 0; i < motor_commands_.size(); i++) {
                buff->motor_data[i] = motor_commands_[i];
            }
        } else {
            buff->to_motor_data_type = MOTOR_DEMAND_TORQUE;
            for (unsigned int id_motor = 0; id_motor < motor_commands_.size();
                 ++id_motor) {
                // Check and protect saturation values !!
                if (motor_commands_[id_motor] <=
                    MOTOR_DEMAND_TORQUE_RANGE_MIN) {
                    buff->motor_data[id_motor] = MOTOR_DEMAND_TORQUE_RANGE_MIN;
                } else if (motor_commands_[id_motor] >=
                           MOTOR_DEMAND_TORQUE_RANGE_MAX) {
                    buff->motor_data[id_motor] = MOTOR_DEMAND_TORQUE_RANGE_MAX;
                } else {
                    buff->motor_data[id_motor] = motor_commands_[id_motor];
                }
            }
        }

        buff->tactile_data_type = tactile_data_type;
    };

    add_run_step(
        [prepare_buffer]() { // FIRST REQUEST (even motors)
            // build basic control request (output buffers) from attributes
            // -> Configure all motors in torque control
            // -> Send torque command to all motors
            // -> Ask to get even motors mesured torque to next step
            // -> Ask to get Tactiles BioTacs Tdc to next step
            prepare_buffer(MotorGroup::Even,
                           TACTILE_BIOTAC_TDC_AND_ELECTRODE_1);
        },
        [read_buffer]() {
            // update attributes from response (input buffers)
            // -> update mesured torque of odd motors (type of data set in last
            // command step)
            // -> update Tactiles Biotacs datas (Pdc and Tac ask in last command
            // step)
            // Get buffer pointer
            read_buffer(ReadSensor::Torque | ReadSensor::Biotac);
        });

    add_run_step(
        [this, prepare_buffer]() { // SECOND REQUEST (odd motors)
            // build basic control request (output buffers) from attributes
            // -> Set all motors in torque control
            // -> Send torque command to all motors
            // -> Ask to get odd motors mesured torque
            if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                // If electrodes asked
                // update next electrode
                // (2/3)
                prepare_buffer(MotorGroup::Odd,
                               TACTILE_BIOTAC_ELECTRODE_2_AND_3);
            } else {
                // If electrode doesn't needs
                // ask Pdc/Tac to next step
                // (first next cycle step)
                prepare_buffer(MotorGroup::Odd, TACTILE_BIOTAC_PDC_AND_TAC);
            }
        },
        [read_buffer]() {
            // update attributes from response (input buffers)
            // -> update all positions sensors (all joints)
            // -> update mesured torque of even motors (type of data set in last
            // command)
            // -> update Tactiles Biotacs datas (Tdc ask in last command step)

            // update only in running step 2 because only 1 time is needed to
            // update all data
            read_buffer(ReadSensor::Torque | ReadSensor::Position |
                        ReadSensor::Biotac);
        });

    ////////////////////////////////////////////////////////////////////////////////
    //                  BIOTACS ELECTRODES UPDATES COMMAND //
    ////////////////////////////////////////////////////////////////////////////////

    // Biotacs Tactiles electrodes needs new step to update all datas.
    if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {

        // 1st step to update tactiles BioTacs eletrodes
        add_run_step(
            [prepare_buffer]() {
                // -> Update biotacs tactiles electrodes
                // -> Ask next Tactiles BioTacs electrodes datas
                // -> keep last "command step datas and configurations"
                prepare_buffer(MotorGroup::Odd,
                               TACTILE_BIOTAC_ELECTRODE_4_AND_5);
            },
            [read_buffer]() { read_buffer(ReadSensor::Biotac); });

        // 2nd step to update tactiles BioTacs eletrodes
        add_run_step(
            [prepare_buffer]() {
                // -> Update biotacs tactiles electrodes
                // -> Ask next Tactiles BioTacs electrodes datas
                // -> keep last "command step datas and configurations"
                prepare_buffer(MotorGroup::Odd,
                               TACTILE_BIOTAC_ELECTRODE_6_AND_7);
            },
            [read_buffer]() { read_buffer(ReadSensor::Biotac); });

        // 3rd step to update tactiles BioTacs eletrodes
        add_run_step(
            [prepare_buffer]() {
                // -> Update biotacs tactiles electrodes
                // -> Ask next Tactiles BioTacs electrodes datas
                // -> keep last "command step datas and configurations"
                prepare_buffer(MotorGroup::Odd,
                               TACTILE_BIOTAC_ELECTRODE_8_AND_9);
            },
            [read_buffer]() { read_buffer(ReadSensor::Biotac); });

        // 4th step to update tactiles BioTacs eletrodes
        add_run_step(
            [prepare_buffer]() {
                // -> Update biotacs tactiles electrodes
                // -> Ask next Tactiles BioTacs electrodes datas
                // -> keep last "command step datas and configurations"
                prepare_buffer(MotorGroup::Odd,
                               TACTILE_BIOTAC_ELECTRODE_10_AND_11);
            },
            [read_buffer]() { read_buffer(ReadSensor::Biotac); });

        // 5th step to update tactiles BioTacs eletrodes
        add_run_step(
            [prepare_buffer]() {
                // -> Update biotacs tactiles electrodes
                // -> Ask next Tactiles BioTacs electrodes datas
                // -> keep last "command step datas and configurations"
                prepare_buffer(MotorGroup::Odd,
                               TACTILE_BIOTAC_ELECTRODE_12_AND_13);
            },
            [read_buffer]() { read_buffer(ReadSensor::Biotac); });

        // 6th step to update tactiles BioTacs eletrodes
        add_run_step(
            [prepare_buffer]() {
                // -> Update biotacs tactiles electrodes
                // -> Ask next Tactiles BioTacs electrodes datas
                // -> keep last "command step datas and configurations"
                prepare_buffer(MotorGroup::Odd,
                               TACTILE_BIOTAC_ELECTRODE_14_AND_15);
            },
            [read_buffer]() { read_buffer(ReadSensor::Biotac); });

        // 7th step to update tactiles BioTacs eletrodes
        add_run_step(
            [prepare_buffer]() {
                // -> Update biotacs tactiles electrodes
                // -> Ask next Tactiles BioTacs electrodes datas
                // -> keep last "command step datas and configurations"
                prepare_buffer(MotorGroup::Odd,
                               TACTILE_BIOTAC_ELECTRODE_14_AND_15);
            },
            [read_buffer]() { read_buffer(ReadSensor::Biotac); });

        // 8th step to update tactiles BioTacs eletrodes
        add_run_step(
            [prepare_buffer]() {
                // -> Update biotacs tactiles electrodes
                // -> Ask next Tactiles BioTacs electrodes datas
                // -> keep last "command step datas and configurations"
                prepare_buffer(MotorGroup::Even,
                               TACTILE_BIOTAC_ELECTRODE_18_AND_19);
            },
            [read_buffer]() {
                read_buffer(ReadSensor::Torque | ReadSensor::Biotac);
            });

        // Step added to "close cycle" and prepare to ask good datas for next
        // cycle
        add_run_step(
            [prepare_buffer]() {
                // -> Update biotacs tactiles electrodes from last step
                // -> Ask next Tactiles BioTacs electrodes datas for next step
                // (next cycle)
                // -> keep last "command step datas and configurations"
                prepare_buffer(MotorGroup::Odd, TACTILE_BIOTAC_PDC_AND_TAC);
            },
            [read_buffer]() {
                read_buffer(ReadSensor::Torque | ReadSensor::Position |
                            ReadSensor::Biotac);
            });
    }

    ////////////////////////////////////////////////////////////////////////////////
    //               END OF BIOTACS ELECTRODES UPDATES COMMAND //
    ////////////////////////////////////////////////////////////////////////////////

} // namespace ethercatcpp

void ShadowHandController::update_biotac_data() {

    // Get buffer pointer
    auto buff = this->input_buffer<buffer_shadow_in_status_t>(0x1046);
    for (unsigned int id_tactile = 0; id_tactile < 5; ++id_tactile) {
        if (buff->tactile[id_tactile].data_valid.Pac0) {
            biotacs_presure_[id_tactile].Pac0 =
                buff->tactile[id_tactile].Pac[0];
        }
        if (buff->tactile[id_tactile].data_valid.Pac1) {
            biotacs_presure_[id_tactile].Pac1 =
                buff->tactile[id_tactile].Pac[1];
        }

        switch (buff->tactile_data_type) {
        case TACTILE_BIOTAC_PDC_AND_TAC:
            if (buff->tactile[id_tactile].data_valid.other_sensor_0) {
                biotacs_presure_[id_tactile].Pdc =
                    buff->tactile[id_tactile].other_sensor_0;
            }
            if (buff->tactile[id_tactile].data_valid.other_sensor_1) {
                biotacs_temperatures_[id_tactile].Tac =
                    buff->tactile[id_tactile].other_sensor_1;
            }
            break;
        case TACTILE_BIOTAC_TDC_AND_ELECTRODE_1:
            if (buff->tactile[id_tactile].data_valid.other_sensor_0) {
                biotacs_temperatures_[id_tactile].Tdc =
                    buff->tactile[id_tactile].other_sensor_0;
            }
            if (biotac_electrode_mode_ == BiotacMode::WithElectrodes) {
                if (buff->tactile[id_tactile].data_valid.other_sensor_1) {
                    biotacs_electrodes_[id_tactile][0] =
                        buff->tactile[id_tactile].other_sensor_1;
                }
            }
            break;

        case TACTILE_BIOTAC_ELECTRODE_2_AND_3:
        case TACTILE_BIOTAC_ELECTRODE_4_AND_5:
        case TACTILE_BIOTAC_ELECTRODE_6_AND_7:
        case TACTILE_BIOTAC_ELECTRODE_8_AND_9:
        case TACTILE_BIOTAC_ELECTRODE_10_AND_11:
        case TACTILE_BIOTAC_ELECTRODE_12_AND_13:
        case TACTILE_BIOTAC_ELECTRODE_14_AND_15:
        case TACTILE_BIOTAC_ELECTRODE_16_AND_17:
        case TACTILE_BIOTAC_ELECTRODE_18_AND_19: {
            size_t offset =
                1 + buff->tactile_data_type - TACTILE_BIOTAC_ELECTRODE_2_AND_3;
            const auto& tactile = buff->tactile[id_tactile];
            auto& electrodes = biotacs_electrodes_[id_tactile];
            if (tactile.data_valid.other_sensor_0) {
                electrodes[offset] = tactile.other_sensor_0;
            }
            if (tactile.data_valid.other_sensor_1) {
                electrodes[offset + 1] = tactile.other_sensor_1;
            }
        } break;
        } // end switch
    } // end for
}

uint16_t
ShadowHandController::compute_configuration_motor_crc(unsigned int id_motor) {

    const auto& configs = motor_controller_configuration_;

    // extract config data and store in the correct order to calculate CRC
    // (MAX_PWM, SG_REFS, F, P, I, D, IMAX, DEADBAND_SIGN)
    std::array<int16_t, 8> motor_config = {
        configs.pwm_max[id_motor], configs.sg_refs[id_motor],
        configs.f_gains[id_motor], configs.p_gains[id_motor],
        configs.i_gains[id_motor], configs.d_gains[id_motor],
        configs.i_max[id_motor],   configs.deadbands[id_motor]};

    word_to_bytes_t crc_data, crc_result;
    crc_result.word = 0;
    uint8_t crc_i;
<--- The scope of the variable 'crc_i' can be reduced. [+]
The scope of the variable 'crc_i' can be reduced. Warning: Be careful when fixing this message, especially when there are inner loops. Here is an example where cppcheck will write that the scope for 'i' can be reduced:
void f(int x)
{
    int i = 0;
    if (x) {
        // it's safe to move 'int i = 0;' here
        for (int n = 0; n < 10; ++n) {
            // it is possible but not safe to move 'int i = 0;' here
            do_something(&i);
        }
    }
}
When you see this message it is always safe to reduce the variable scope 1 level.
    std::array<uint16_t, 8> masks = {0x3096, 0x612c, 0xc419, 0x8832,
                                     0x1064, 0x20c8, 0x4190, 0x8320};
    for (size_t config_index = 0; config_index < motor_config.size();
         ++config_index) { // 0 to 7 for the 8 motors configs
        crc_data.word = static_cast<uint16_t>(motor_config[config_index]);

        crc_i = crc_result.byte[0] ^ crc_data.byte[0];
        crc_result.word >>= 8;
        for (size_t i = 0; i < 8; i++) {
            if (crc_i & (0x01 << i)) {
                crc_result.word ^= masks[i];
            }
        }

        crc_i = crc_result.byte[0] ^ crc_data.byte[1];
        crc_result.word >>= 8;
        for (size_t i = 0; i < 8; i++) {
            if (crc_i & (0x01 << i)) {
                crc_result.word ^= masks[i];
            }
        }
    }
    return crc_result.word;
}

void ShadowHandController::joints_commands(
    const std::array<int16_t, shadow::joint_count>& torque_cmd) {<--- Function 'joints_commands' argument 1 names different: declaration 'commands' definition 'torque_cmd'.
    auto torques = torque_cmd;
    apply_control_signs(torques);
    auto copy = [this, &torques](JointNames name) {
        motor_commands_[joints_to_motors_matching_[index_of(name)]] =
            torques[index_of(name)];
    };
    copy(JointNames::FFJ4);
    copy(JointNames::FFJ3);
    copy(JointNames::FFJ2);
    copy(JointNames::MFJ4);
    copy(JointNames::MFJ3);
    copy(JointNames::MFJ2);
    copy(JointNames::RFJ4);
    copy(JointNames::RFJ3);
    copy(JointNames::RFJ2);
    copy(JointNames::LFJ5);
    copy(JointNames::LFJ4);
    copy(JointNames::LFJ3);
    copy(JointNames::LFJ2);
    copy(JointNames::THJ5);
    copy(JointNames::THJ4);
    copy(JointNames::THJ3);
    copy(JointNames::THJ2);
    copy(JointNames::WRJ2);
    copy(JointNames::WRJ1);
}

void ShadowHandController::set_control_mode(ControlMode mode) {
    control_mode_ = mode;
}

std::array<int16_t, shadow::joint_count>
ShadowHandController::joints_torques() {
    auto get = [this](JointNames name) {
        return mesured_motors_torque_[joints_to_motors_matching_[index_of(
            name)]];
    };

    std::array<int16_t, shadow::joint_count> torques = {
        get(JointNames::FFJ2), get(JointNames::FFJ3), get(JointNames::FFJ4),
        get(JointNames::MFJ2), get(JointNames::MFJ3), get(JointNames::MFJ4),
        get(JointNames::RFJ2), get(JointNames::RFJ3), get(JointNames::RFJ4),
        get(JointNames::LFJ2), get(JointNames::LFJ3), get(JointNames::LFJ4),
        get(JointNames::LFJ5), get(JointNames::THJ2), get(JointNames::THJ3),
        get(JointNames::THJ4), get(JointNames::THJ5), get(JointNames::WRJ1),
        get(JointNames::WRJ2)};

    apply_control_signs(torques);
    return torques;
}

std::array<uint16_t, shadow::joint_count>
ShadowHandController::joints_positions() {
    return {
        // FFJ4-2
        mesured_joints_position_[2],
        mesured_joints_position_[1],
        mesured_joints_position_[0],

        // MFJ4-2
        mesured_joints_position_[5],
        mesured_joints_position_[4],
        mesured_joints_position_[3],

        // RFJ4-2
        mesured_joints_position_[8],
        mesured_joints_position_[7],
        mesured_joints_position_[6],

        // LFJ5-2
        mesured_joints_position_[12],
        mesured_joints_position_[11],
        mesured_joints_position_[10],
        mesured_joints_position_[9],

        // THJ5-2
        mesured_joints_position_[16],
        mesured_joints_position_[15],
        mesured_joints_position_[14],
        mesured_joints_position_[13],

        // WRJ2-1
        mesured_joints_position_[18],
        mesured_joints_position_[17],
    };
}

std::array<BiotacPressures, shadow::biotac_count>
ShadowHandController::biotacs_pressures() {
    return (biotacs_presure_);
}
std::array<BiotacTemperatures, shadow::biotac_count>
ShadowHandController::biotacs_temperatures() {
    return (biotacs_temperatures_);
}
std::array<BiotacElectrodes, shadow::biotac_count>
ShadowHandController::biotacs_electrodes() {
    return (biotacs_electrodes_);
}

// This function check errors and update values of torques motors
void ShadowHandController::update_motors_torques() {
    // Get pointer to status datas struct
    auto buff = this->input_buffer<buffer_shadow_in_status_t>(0x1046);
    for (unsigned int id_data_motor = 0;
         id_data_motor < sizeof(buff->motor_data_packet) / 4; ++id_data_motor) {
        if (buff->which_motors) { // odd motors (impair)
            // check if motor data arrived and if data had no error for the
            // correspondant motor get bit of the "real motor number (0->20)" in
            // which_motor_data_arrived who contain all motors infos
            if ((buff->which_motor_data_arrived &
                 (0x1 << (id_data_motor * 2 + 1))) &&
                !(buff->which_motor_data_had_errors &
                  (0x1 << (id_data_motor * 2 + 1)))) {
                mesured_motors_torque_.at(id_data_motor * 2 + 1) =
                    buff->motor_data_packet[id_data_motor].torque;
            }
        } else { // even motors (pair)
            // check if motor data arrived and if data had no error for the
            // correspondant motor
            // get bit of the "real motor number (0->20)" in
            // which_motor_data_arrived who contain all motors infos
            if ((buff->which_motor_data_arrived &
                 (0x1 << (id_data_motor * 2))) &&
                !(buff->which_motor_data_had_errors &
                  (0x1 << (id_data_motor * 2)))) {
                mesured_motors_torque_.at(id_data_motor * 2) =
                    buff->motor_data_packet[id_data_motor].torque;
            }
        }
    }
}

void ShadowHandController::update_joints_position() {
    // Get pointer to status datas struct
    auto buff = this->input_buffer<buffer_shadow_in_status_t>(0x1046);
    // For each position sensors
    size_t joint_idx = 0;
    auto ends_with = [](const std::string& str1, const std::string& str2) {
        auto it1 = --str1.end();
        auto it2 = --str2.end();
        while (it1 > str1.begin() and it2 > str2.begin() and *it1 == *it2) {
            --it1;
            --it2;
        }
        return it2 == str2.begin() and * it1 == *it2;
    };
    for (unsigned int id_sensor = 0; id_sensor < POSITION_SENSOR_NUM;
         ++id_sensor) { // from 0 to 25 (POSITION_SENSOR_NUM =26) to only read
                        // ADC value of positions sensors
        const auto& name = sensor_names[id_sensor];
        uint16_t raw_position = 0;
        if ((name == "THJ5A") || (name == "WRJ1A")) {
            // This 2 sensors have a particular calibration ( cal(THJ5) = cal(
            // raw_THJ5A + raw_THJ5B) ). In this particular case we add the 2
            // sensors (xA and xB) and jump the next cycle (to jump xB case).
            raw_position =
                buff->sensors[id_sensor] / 2 + buff->sensors[id_sensor + 1] / 2;
            ++id_sensor;
        } else if (ends_with(name, "J1")) {
            // J1 names are absent on LIRMM's hands because of the Biotac
            // sensors
            continue;
        } else { // otherwise take only one value
            raw_position = buff->sensors[id_sensor];
        }

        mesured_joints_position_[joint_idx++] = raw_position;
    }
}

void ShadowHandController::setup_control_signs(HandType hand_type) {
    if (control_mode_ == ControlMode::Torque) {
        if (hand_type == HandType::Right) {
            control_signs_ = {
                1,  -1, -1,     // FFJ4, FFJ3, FFJ2
                -1, -1, -1,     // MFJ4, MFJ3, MFJ2
                -1, 1,  -1,     // RFJ4, RFJ3, RFJ2
                1,  1,  1,  -1, // LFJ5, LFJ4, LFJ3, LFJ2
                -1, -1, -1, -1, // THJ5, THJ4, THJ3, THJ2
                1,  -1          // WRJ2, WRJ1
            };
        } else {
            control_signs_ = {
                -1, 1,  1,      // FFJ4, FFJ3, FFJ2
                1,  -1, 1,      // MFJ4, MFJ3, MFJ2
                -1, -1, 1,      // RFJ4, RFJ3, RFJ2
                -1, 1,  -1, -1, // LFJ5, LFJ4, LFJ3, LFJ2
                1,  1,  -1, 1,  // THJ5, THJ4, THJ3, THJ2
                -1, -1          // WRJ2, WRJ1
            };
        }
    } else {
        if (hand_type == HandType::Right) {
            control_signs_ = {
                -1, 1,  1,     // FFJ4, FFJ3, FFJ2
                1,  1,  1,     // MFJ4, MFJ3, MFJ2
                1,  -1, 1,     // RFJ4, RFJ3, RFJ2
                -1, -1, -1, 1, // LFJ5, LFJ4, LFJ3, LFJ2
                1,  1,  1,  1, // THJ5, THJ4, THJ3, THJ2
                -1, 1          // WRJ2, WRJ1
            };
        } else {
            control_signs_ = {
                1,  -1, -1,     // FFJ4, FFJ3, FFJ2
                -1, 1,  -1,     // MFJ4, MFJ3, MFJ2
                1,  1,  -1,     // RFJ4, RFJ3, RFJ2
                1,  -1, 1,  1,  // LFJ5, LFJ4, LFJ3, LFJ2
                -1, -1, 1,  -1, // THJ5, THJ4, THJ3, THJ2
                1,  1           // WRJ2, WRJ1
            };
        }
    }
} // namespace ethercatcpp

void ShadowHandController::apply_control_signs(
    std::array<int16_t, shadow::joint_count>& torques) {
    for (size_t i = 0; i < torques.size(); i++) {
        torques[i] *= control_signs_[i];
    }
}

void ShadowHandController::load_joints_To_motors_matching(HandType hand_type) {
    if (hand_type == HandType::Right) {
        joints_to_motors_matching_[index_of(JointNames::FFJ2)] = 6;
        joints_to_motors_matching_[index_of(JointNames::FFJ3)] = 5;
        joints_to_motors_matching_[index_of(JointNames::FFJ4)] = 7;
        joints_to_motors_matching_[index_of(JointNames::MFJ2)] = 0;
        joints_to_motors_matching_[index_of(JointNames::MFJ3)] = 2;
        joints_to_motors_matching_[index_of(JointNames::MFJ4)] = 1;
        joints_to_motors_matching_[index_of(JointNames::RFJ2)] = 11;
        joints_to_motors_matching_[index_of(JointNames::RFJ3)] = 12;
        joints_to_motors_matching_[index_of(JointNames::RFJ4)] = 10;
        joints_to_motors_matching_[index_of(JointNames::LFJ2)] = 16;
        joints_to_motors_matching_[index_of(JointNames::LFJ3)] = 15;
        joints_to_motors_matching_[index_of(JointNames::LFJ4)] = 17;
        joints_to_motors_matching_[index_of(JointNames::LFJ5)] = 13;
        joints_to_motors_matching_[index_of(JointNames::THJ2)] = 3;
        joints_to_motors_matching_[index_of(JointNames::THJ3)] = 14;
        joints_to_motors_matching_[index_of(JointNames::THJ4)] = 9;
        joints_to_motors_matching_[index_of(JointNames::THJ5)] = 19;
        joints_to_motors_matching_[index_of(JointNames::WRJ1)] = 18;
        joints_to_motors_matching_[index_of(JointNames::WRJ2)] = 8;
    } else {
        joints_to_motors_matching_[index_of(JointNames::FFJ2)] = 16;
        joints_to_motors_matching_[index_of(JointNames::FFJ3)] = 15;
        joints_to_motors_matching_[index_of(JointNames::FFJ4)] = 17;
        joints_to_motors_matching_[index_of(JointNames::MFJ2)] = 10;
        joints_to_motors_matching_[index_of(JointNames::MFJ3)] = 12;
        joints_to_motors_matching_[index_of(JointNames::MFJ4)] = 11;
        joints_to_motors_matching_[index_of(JointNames::RFJ2)] = 1;
        joints_to_motors_matching_[index_of(JointNames::RFJ3)] = 2;
        joints_to_motors_matching_[index_of(JointNames::RFJ4)] = 0;
        joints_to_motors_matching_[index_of(JointNames::LFJ2)] = 6;
        joints_to_motors_matching_[index_of(JointNames::LFJ3)] = 5;
        joints_to_motors_matching_[index_of(JointNames::LFJ4)] = 7;
        joints_to_motors_matching_[index_of(JointNames::LFJ5)] = 3;
        joints_to_motors_matching_[index_of(JointNames::THJ2)] = 13;
        joints_to_motors_matching_[index_of(JointNames::THJ3)] = 4;
        joints_to_motors_matching_[index_of(JointNames::THJ4)] = 19;
        joints_to_motors_matching_[index_of(JointNames::THJ5)] = 9;
        joints_to_motors_matching_[index_of(JointNames::WRJ1)] = 18;
        joints_to_motors_matching_[index_of(JointNames::WRJ2)] = 8;
    }
}

} // namespace ethercatcpp