1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
/*      File: hand_controller.h
 *       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).
 */
/**
 * @file ethercatcpp/shadow/hand_controller.h
 * @author Arnaud Meline (original developper)
 * @author Robin Passama (design and refactoring)
 * @brief include for ShadowHandController class
 * @date October 2018 - December 2022.
 * @ingroup ethercatcpp-shadow
 */

#pragma once

#include <ethercatcpp/shadow/common.h>
#include <ethercatcpp/shadow_hand.h>
#include <ethercatcpp/core.h>
#include <cstdint>

/*! \namespace ethercatcpp
 *
 * Root namespace for common and general purpose ethercatcpp packages
 */
namespace ethercatcpp {

namespace shadow {

/**
 * This class is only used to made a complete hand on EtherCAT device. One hand
 * is composed with two slaves (a fake and a controller). This class describe
 * the controler and permit communication between the shadow hand controller and
 * an ehtercat bus.
 * @brief This class describe the EtherCAT driver for the shadow hand controller
 *
 */
class ShadowHandController : public SlaveDevice {
public:
    /**
     * Constructor of ShadowHandController class
     * @brief The constructor have to have an information on witch hand is used
     * (left or right) and if user needs biotacs eletrode datas or not.
     * @param [in] hand_type represents the hand side (left or right)
     * @param [in] biotac_electrode_mode is a biotac_electrode_mode_t who
     * @param [in] control_mode the control mode for joints: torque or PWM
     * @param [in] serial_number serial number of the shadow hand
     */
    ShadowHandController(HandType hand_type, BiotacMode biotac_electrode_mode,
                         ControlMode control_mode, uint32_t serial_number = 0);

    virtual ~ShadowHandController();

    //! \brief Set the joint command vector. Joint indexes follow
    //! ShadowHand::JointNames. Values are interpreted as torque or PWM
    //! depending on the ControlMode set upon construction
    //!
    //! \param commands motor commands to apply
    void
    joints_commands(const std::array<int16_t, shadow::joint_count>& commands);

    void set_control_mode(ControlMode mode);

    //! \brief Get the measured joint torques vector
    //!
    //! \return std::array<int16_t, shadow::joint_count> measured torques
    std::array<int16_t, shadow::joint_count> joints_torques();

    //! \brief Get the measured joint positions vector
    //!
    //! \return std::array<uint16_t, shadow::joint_count> measured position
    std::array<uint16_t, shadow::joint_count> joints_positions();

    /**
     * @brief Get all biotacs pressures data for all fingers.
     * @return an array with all fingers presures (dynamic and absolute presure)
     * ordered from first finger to thumb finger. index 0 => First finger, index
     * 1 => Middle finger, index 2 => Ring finger, index 3 => Little finger,
     * index 4 => Thumb finger
     */
    std::array<BiotacPressures, shadow::biotac_count> biotacs_pressures();

    /**
     * @brief Function used to get all biotacs temperatures datas for all
     * fingers.
     * @return an array with all fingers temperatures (dynamic and absolute
     * temperatures) ordered from first finger to thumb finger. index 0 => First
     * finger, index 1 => Middle finger, index 2 => Ring finger, index 3 =>
     * Little finger, index 4 => Thumb finger
     */
    std::array<BiotacTemperatures, shadow::biotac_count> biotacs_temperatures();

    /**
     * @brief Function used to get all biotacs electrodes impedance datas for
     * all fingers.
     * @return an array with all fingers electrodes impedance ordered from first
     * finger to thumb finger. index 0 => First finger, index 1 => Middle
     * finger, index 2 => Ring finger, index 3 => Little finger, index 4 =>
     * Thumb finger
     */
    std::array<BiotacElectrodes, shadow::biotac_count> biotacs_electrodes();

    // FIXME change maps defs (joints_to_motors_matching_ and
    // calibration_position_map_) to read from a config file
    // FIXME ? make fonction to update Motors_Misc() (voltages, currants, flags,
    // ...); //update datas vector by type (motor_data_type)

private:
    BiotacMode biotac_electrode_mode_; // With or without BioTac electrodes<--- class member 'ShadowHandController::biotac_electrode_mode_' is never used.
    ControlMode control_mode_;<--- class member 'ShadowHandController::control_mode_' is never used.
    std::array<int16_t, shadow::joint_count> control_signs_;<--- class member 'ShadowHandController::control_signs_' is never used.

    void setup_control_signs(HandType hand_type);
    void apply_control_signs(std::array<int16_t, shadow::joint_count>& torques);

    void load_joints_To_motors_matching(HandType hand_type);
    uint16_t compute_configuration_motor_crc(unsigned int id_motor);

    void update_motors_torques();  // Update value of each odd or even motor
                                   // torque in mesured_motors_torque_
    void update_joints_position(); // Update value of each joints in
                                   // mesured_joints_position_
    void update_biotac_data();     // Update value of each biotacs fingers in
                               // biotacs_presure_, biotacs_temperatures_ and
                               // biotacs_electrodes_

    std::array<uint16_t, shadow::joint_count>
        mesured_joints_position_; // vector ordored first element = FFJ2,<--- class member 'ShadowHandController::mesured_joints_position_' is never used.
                                  // FFJ3, FFJ4, MFJ2, ..., MFJ4, RFJ2, ...,
                                  // RFJ4, LFJ2, ..., LFJ5, THJ2, ..., THJ5,
                                  // WRJ1, WRJ2.
    std::array<int16_t, NUM_MOTORS>
        mesured_motors_torque_; // vector ordored first element = Motor id 0 to<--- class member 'ShadowHandController::mesured_motors_torque_' is never used.
                                // last element motor id 19
    std::array<int16_t, NUM_MOTORS>
        motor_commands_; // vector ordored first element = Motor id 0 to<--- class member 'ShadowHandController::motor_commands_' is never used.
                         // last element motor id 19

    // Tactile BioTacs datas
    std::array<BiotacPressures, 5>
        biotacs_presure_; // array contain dynamic and absolute presure for the<--- class member 'ShadowHandController::biotacs_presure_' is never used.
                          // 5 fingers (0:FF, 1:MF, 2:RF, 3:LF 4:TH)
    std::array<BiotacTemperatures, 5>
        biotacs_temperatures_; // array contain dynamic and absolute temperature<--- class member 'ShadowHandController::biotacs_temperatures_' is never used.
                               // for the 5 fingers (0:FF, 1:MF, 2:RF, 3:LF
                               // 4:TH)
    std::array<BiotacElectrodes, 5>
        biotacs_electrodes_; // array contain electrodes impedance value for the<--- class member 'ShadowHandController::biotacs_electrodes_' is never used.
                             // 5 fingers (0:FF, 1:MF, 2:RF, 3:LF 4:TH)

    std::array<uint16_t, shadow::joint_count>
        joints_to_motors_matching_; // index : joint, value = id_motor<--- class member 'ShadowHandController::joints_to_motors_matching_' is never used.

    // Control parameters for internal FPID motors controllers
    // vector ordored first element = Motor id 0 to last element motor id 19
    struct MotorControllerConfiguration {
        MotorControllerConfiguration();

        std::array<int16_t, NUM_MOTORS> f_gains;<--- struct member 'MotorControllerConfiguration::f_gains' is never used.
        std::array<int16_t, NUM_MOTORS> p_gains;<--- struct member 'MotorControllerConfiguration::p_gains' is never used.
        std::array<int16_t, NUM_MOTORS> i_gains;<--- struct member 'MotorControllerConfiguration::i_gains' is never used.
        std::array<int16_t, NUM_MOTORS> d_gains;<--- struct member 'MotorControllerConfiguration::d_gains' is never used.
        std::array<int16_t, NUM_MOTORS> i_max;<--- struct member 'MotorControllerConfiguration::i_max' is never used.
        std::array<int16_t, NUM_MOTORS> pwm_max;<--- struct member 'MotorControllerConfiguration::pwm_max' is never used.
        std::array<int16_t, NUM_MOTORS> sg_refs;<--- struct member 'MotorControllerConfiguration::sg_refs' is never used.
        std::array<int16_t, NUM_MOTORS> deadbands;<--- struct member 'MotorControllerConfiguration::deadbands' is never used.
        std::array<int16_t, NUM_MOTORS> backlash_compensation;<--- struct member 'MotorControllerConfiguration::backlash_compensation' is never used.
    };
    MotorControllerConfiguration motor_controller_configuration_;
};
} // namespace shadow
} // namespace ethercatcpp