Motion Control Module

This module provides a Service (motion_control_service) to perform Torque/Velocity/Position control. This Service is recommended to be running at 100MHz Reference Clock Frequency. After running this service, the user can select the controlling mode (either torque, velocity, or position control mode). Depending on the selected mode, motion_control_service will call the corresponding function from module_controllers to calculate the proper reference value of the torque, and calculated values of reference torque will be sent to torque control service.

How to use

Important

We assume that you are using SOMANET Base and your app includes the required board support files for your SOMANET device.

See also

You might find useful the app_demo_motion_control example apps, which illustrate the use of this module:

  • BLDC Motion Control Demo
  1. First, add all the SOMANET Motion Control modules to your app Makefile.

    USED_MODULES = module_controllers lib_bldc_torque_control module_pwm module_adc module_hall_sensor module_utils module_profiles module_incremental_encoder module_gpio module_watchdog module_board-support
    

    Note

    Not all modules will be required, but when using a library it is recommended to include always all the contained modules. This will help solving internal dependency issues.

  2. Properly instantiate a Torque Control Service.

  3. Include the Motion Control Service header motion_control_service.h in your app.

  4. Inside your main function, instantiate the interfaces array for the Service-Clients communication.

  5. Outside your IFM tile, instantiate the Service. For that, first you will have to fill up your Service configuration and provide interfaces to your position feedback sensor Service and Torque Control Service.

  6. Now you can perform calls to the Motion Control Service through the interfaces connected to it. You can do this at whichever other core.

    #include <CORE_C22-rev-a.bsp>   //Board Support file for SOMANET Core C22 device
    #include <IFM_DC1K-rev-c3.bsp>  //Board Support file for SOMANET IFM DC100 device
                                    //(select your board support files according to your device)
    
    #include <pwm_service.h>
    #include <hall_service.h>
    #include <watchdog_service.h>
    #include <motorcontrol_service.h>
    #include <motion_control_service.h> // step 3
    
    PwmPorts pwm_ports = SOMANET_IFM_PWM_PORTS;
    WatchdogPorts wd_ports = SOMANET_IFM_WATCHDOG_PORTS;
    FetDriverPorts fet_driver_ports = SOMANET_IFM_FET_DRIVER_PORTS;
    ADCPorts adc_ports = SOMANET_IFM_ADC_PORTS;
    QEIHallPort qei_hall_port_1 = SOMANET_IFM_HALL_PORTS;
    QEIHallPort qei_hall_port_2 = SOMANET_IFM_QEI_PORTS;
    HallEncSelectPort hall_enc_select_port = SOMANET_IFM_QEI_PORT_INPUT_MODE_SELECTION;
    SPIPorts spi_ports = SOMANET_IFM_SPI_PORTS;
    port ?gpio_port_0 = SOMANET_IFM_GPIO_D0;
    port ?gpio_port_1 = SOMANET_IFM_GPIO_D1;
    port ?gpio_port_2 = SOMANET_IFM_GPIO_D2;
    port ?gpio_port_3 = SOMANET_IFM_GPIO_D3;
    
    int main(void)
    {
    
        // Motor control interfaces
        interface WatchdogInterface i_watchdog[2];
        interface UpdatePWM i_update_pwm;
        interface UpdateBrake i_update_brake;
        interface ADCInterface i_adc[2];
        interface TorqueControlInterface i_torque_control[2];
        interface MotionControlInterface i_motion_control[3];
        interface PositionFeedbackInterface i_position_feedback_1[3];
        interface PositionFeedbackInterface i_position_feedback_2[3];
        interface shared_memory_interface i_shared_memory[3];//step 4
    
        par
        {
            on tile[APP_TILE]:
            {
                 demo_motion_control(i_motion_control[0]); // step 6
            }
            on tile[APP_TILE]:
            {
                //step 5
                MotionControlConfig motion_ctrl_config;
    
                motion_ctrl_config.min_pos_range_limit =                  MIN_POSITION_RANGE_LIMIT;
                motion_ctrl_config.max_pos_range_limit =                  MAX_POSITION_RANGE_LIMIT;
                motion_ctrl_config.max_motor_speed =                      MOTOR_MAX_SPEED;
                motion_ctrl_config.polarity =                             POLARITY;
    
                motion_ctrl_config.enable_profiler =                      ENABLE_PROFILER;
                motion_ctrl_config.max_acceleration_profiler =            MAX_ACCELERATION_PROFILER;
                motion_ctrl_config.max_deceleration_profiler =            MAX_DECELERATION_PROFILER;
                motion_ctrl_config.max_speed_profiler =                   MAX_SPEED_PROFILER;
    
                motion_ctrl_config.position_control_strategy =            NL_POSITION_CONTROLLER;
    
                motion_ctrl_config.position_kp =                                POSITION_Kp;
                motion_ctrl_config.position_ki =                                POSITION_Ki;
                motion_ctrl_config.position_kd =                                POSITION_Kd;
                motion_ctrl_config.position_integral_limit =                   POSITION_INTEGRAL_LIMIT;
                motion_ctrl_config.moment_of_inertia =                    MOMENT_OF_INERTIA;
    
                motion_ctrl_config.velocity_kp =                           VELOCITY_Kp;
                motion_ctrl_config.velocity_ki =                           VELOCITY_Ki;
                motion_ctrl_config.velocity_kd =                           VELOCITY_Kd;
                motion_ctrl_config.velocity_integral_limit =              VELOCITY_INTEGRAL_LIMIT;
    
                motion_ctrl_config.brake_release_strategy =                BRAKE_RELEASE_STRATEGY;
                motion_ctrl_config.brake_release_delay =                 BRAKE_RELEASE_DELAY;
    
                //select resolution of sensor used for motion control
                if (SENSOR_2_FUNCTION == SENSOR_FUNCTION_COMMUTATION_AND_MOTION_CONTROL || SENSOR_2_FUNCTION == SENSOR_FUNCTION_MOTION_CONTROL) {
                    motion_ctrl_config.resolution  =                          SENSOR_2_RESOLUTION;
                } else {
                    motion_ctrl_config.resolution  =                          SENSOR_1_RESOLUTION;
                }
    
                motion_ctrl_config.dc_bus_voltage=                        DC_BUS_VOLTAGE;
                motion_ctrl_config.pull_brake_voltage=                    PULL_BRAKE_VOLTAGE;
                motion_ctrl_config.pull_brake_time =                      PULL_BRAKE_TIME;
                motion_ctrl_config.hold_brake_voltage =                   HOLD_BRAKE_VOLTAGE;
    
                motion_control_service(motion_ctrl_config, i_torque_control[0], i_motion_control, i_update_brake); //5
    }
    
    on tile[IFM_TILE]:
    {
        par
        {
            /* PWM Service */
            {
                pwm_config(pwm_ports);
    
                if (!isnull(fet_driver_ports.p_esf_rst_pwml_pwmh) && !isnull(fet_driver_ports.p_coast))
                    predriver(fet_driver_ports);
    
                //pwm_check(pwm_ports);//checks if pulses can be generated on pwm ports or not
                pwm_service_task(MOTOR_ID, pwm_ports, i_update_pwm,
                        i_update_brake, IFM_TILE_USEC);
    
            }
    
            /* ADC Service */
            {
                adc_service(adc_ports, i_adc /*ADCInterface*/, i_watchdog[1], IFM_TILE_USEC, SINGLE_ENDED);
            }
    
            /* Watchdog Service */
            {
                watchdog_service(wd_ports, i_watchdog, IFM_TILE_USEC);
            }
    
            /* Motor Control Service */
            {
                // step 2
                MotorcontrolConfig motorcontrol_config;
    
                motorcontrol_config.dc_bus_voltage =  DC_BUS_VOLTAGE;
                motorcontrol_config.phases_inverted = MOTOR_PHASES_NORMAL;
                motorcontrol_config.torque_P_gain =  TORQUE_Kp;
                motorcontrol_config.torque_I_gain =  TORQUE_Ki;
                motorcontrol_config.torque_D_gain =  TORQUE_Kd;
                motorcontrol_config.pole_pairs =  MOTOR_POLE_PAIRS;
                motorcontrol_config.commutation_sensor=SENSOR_1_TYPE;
                motorcontrol_config.commutation_angle_offset=COMMUTATION_ANGLE_OFFSET;
                motorcontrol_config.hall_state_angle[0]=HALL_STATE_1_ANGLE;
                motorcontrol_config.hall_state_angle[1]=HALL_STATE_2_ANGLE;
                motorcontrol_config.hall_state_angle[2]=HALL_STATE_3_ANGLE;
                motorcontrol_config.hall_state_angle[3]=HALL_STATE_4_ANGLE;
                motorcontrol_config.hall_state_angle[4]=HALL_STATE_5_ANGLE;
                motorcontrol_config.hall_state_angle[5]=HALL_STATE_6_ANGLE;
                motorcontrol_config.max_torque =  MOTOR_MAXIMUM_TORQUE;
                motorcontrol_config.phase_resistance =  MOTOR_PHASE_RESISTANCE;
                motorcontrol_config.phase_inductance =  MOTOR_PHASE_INDUCTANCE;
                motorcontrol_config.torque_constant =  MOTOR_TORQUE_CONSTANT;
                motorcontrol_config.current_ratio =  CURRENT_RATIO;
                motorcontrol_config.voltage_ratio =  VOLTAGE_RATIO;
                motorcontrol_config.temperature_ratio =  TEMPERATURE_RATIO;
                motorcontrol_config.rated_current =  MOTOR_RATED_CURRENT;
                motorcontrol_config.rated_torque  =  MOTOR_RATED_TORQUE;
                motorcontrol_config.percent_offset_torque =  APPLIED_TUNING_TORQUE_PERCENT;
                motorcontrol_config.protection_limit_over_current =  PROTECTION_MAXIMUM_CURRENT;
                motorcontrol_config.protection_limit_over_voltage =  PROTECTION_MAXIMUM_VOLTAGE;
                motorcontrol_config.protection_limit_under_voltage = PROTECTION_MINIMUM_VOLTAGE;
                motorcontrol_config.protection_limit_over_temperature = TEMP_BOARD_MAX;
    
                torque_control_service(motorcontrol_config, i_adc[0], i_shared_memory[2],
                        i_watchdog[0], i_torque_control, i_update_pwm, IFM_TILE_USEC);
            }
    
            /* Shared memory Service */
            [[distribute]] shared_memory_service(i_shared_memory, 3);
    
            /* Position feedback service */
            {
                PositionFeedbackConfig position_feedback_config;
                position_feedback_config.sensor_type = SENSOR_1_TYPE;
                position_feedback_config.resolution  = SENSOR_1_RESOLUTION;
                position_feedback_config.polarity    = SENSOR_1_POLARITY;
                position_feedback_config.velocity_compute_period = SENSOR_1_VELOCITY_COMPUTE_PERIOD;
                position_feedback_config.pole_pairs  = MOTOR_POLE_PAIRS;
                position_feedback_config.ifm_usec    = IFM_TILE_USEC;
                position_feedback_config.max_ticks   = SENSOR_MAX_TICKS;
                position_feedback_config.offset      = HOME_OFFSET;
                position_feedback_config.sensor_function = SENSOR_1_FUNCTION;
    
                position_feedback_config.biss_config.multiturn_resolution = BISS_MULTITURN_RESOLUTION;
                position_feedback_config.biss_config.filling_bits = BISS_FILLING_BITS;
                position_feedback_config.biss_config.crc_poly = BISS_CRC_POLY;
                position_feedback_config.biss_config.clock_frequency = BISS_CLOCK_FREQUENCY;
                position_feedback_config.biss_config.timeout = BISS_TIMEOUT;
                position_feedback_config.biss_config.busy = BISS_BUSY;
                position_feedback_config.biss_config.clock_port_config = BISS_CLOCK_PORT;
                position_feedback_config.biss_config.data_port_number = BISS_DATA_PORT_NUMBER;
    
                position_feedback_config.rem_16mt_config.filter = REM_16MT_FILTER;
    
                position_feedback_config.rem_14_config.hysteresis              = REM_14_SENSOR_HYSTERESIS;
                position_feedback_config.rem_14_config.noise_settings          = REM_14_SENSOR_NOISE_SETTINGS;
                position_feedback_config.rem_14_config.dyn_angle_error_comp    = REM_14_DYN_ANGLE_ERROR_COMPENSATION;
                position_feedback_config.rem_14_config.abi_resolution_settings = REM_14_ABI_RESOLUTION_SETTINGS;
    
                position_feedback_config.qei_config.number_of_channels = QEI_SENSOR_NUMBER_OF_CHANNELS;
                position_feedback_config.qei_config.signal_type        = QEI_SENSOR_SIGNAL_TYPE;
                position_feedback_config.qei_config.port_number        = QEI_SENSOR_PORT_NUMBER;
    
                position_feedback_config.hall_config.port_number = HALL_SENSOR_PORT_NUMBER;
    
                //setting second sensor
                PositionFeedbackConfig position_feedback_config_2 = position_feedback_config;
                position_feedback_config_2.sensor_type = 0;
                if (SENSOR_2_FUNCTION != SENSOR_FUNCTION_DISABLED) //enable second sensor
                {
                    position_feedback_config_2.sensor_type = SENSOR_2_TYPE;
                    position_feedback_config_2.polarity    = SENSOR_2_POLARITY;
                    position_feedback_config_2.resolution  = SENSOR_2_RESOLUTION;
                    position_feedback_config_2.velocity_compute_period = SENSOR_2_VELOCITY_COMPUTE_PERIOD;
                    position_feedback_config_2.sensor_function = SENSOR_2_FUNCTION;
                }
    
                position_feedback_service(qei_hall_port_1, qei_hall_port_2, hall_enc_select_port, spi_ports, gpio_port_0, gpio_port_1, gpio_port_2, gpio_port_3,
                        position_feedback_config, i_shared_memory[0], i_position_feedback_1,
                        position_feedback_config_2, i_shared_memory[1], i_position_feedback_2);
            }
        }
    }
    

    }

    return 0;

}

API

Definitions

PID_DENOMINATOR

Denominator for PID contants.

The values set by the user for such constants will be divided by this value (10000 by default).

POSITION_LIMIT_THRESHOLD

threshold in ticks to re-enable the position controler if the limit reached.

POSITION_CONTROL_LOOP_PERIOD

Period for the control loop [microseconds].

PID_DENOMINATOR

Denominator for PID contants.

The values set by the user for such constants will be divided by this value (10000 by default).

PID_DENOMINATOR

Denominator for PID contants.

The values set by the user for such constants will be divided by this value (10000 by default).

PID_DENOMINATOR

Denominator for PID contants.

The values set by the user for such constants will be divided by this value (10000 by default).

Global Types

Warning

doxygenstruct: Cannot find struct “MotionControlStrategies” in doxygen xml output

MotionControlConfig

Structure definition for a Motion Control Loop Service configuration.

Structure Members:

int position_control_strategy

Parameter for selecting between defferent types of position or velocity controllers.

int motion_profile_type

Parameter for selecting between different types of profilers (including torque/velocity/posiiton controllers.

int min_pos_range_limit

Parameter for setting the minimum position range.

int max_pos_range_limit

Parameter for setting the maximum position range.

int max_motor_speed

Parameter for setting the maximum motor speed.

int max_torque

Parameter for setting the maximum torque command which will be sent to torque controller.

int enable_profiler

Parameter for enabling/disabling the profiler.

int max_acceleration_profiler

Parameter for setting the maximum acceleration in profiler mode.

int max_deceleration_profiler

Parameter for setting the maximum deceleration in profiler mode.

int max_speed_profiler

Parameter for setting the maximum speed in profiler mode.

int max_torque_rate_profiler

Parameter for setting the maximum torque in profiler mode.

int position_kp

Parameter for position controller P-constant.

int position_ki

Parameter for position controller I-constant.

int position_kd

Parameter for position controller D-constant.

int position_integral_limit

Parameter for integral limit of position pid controller.

int position_control_autotune

Parameter for enabling/disabling automatic tuning of position controller.

int step_amplitude_autotune

The tuning procedure uses steps to evaluate the response of controller.

This input is equal to half of step command amplitude.

int counter_max_autotune

The period of step commands in ticks.

Each tick is corresponding to one execution sycle of motion_control_service. As a result, 3000 ticks when the frequency of motion_control_service is 1 ms leads to a period equal to 3 seconds for each step command.

int per_thousand_overshoot_autotune

Overshoot limit while tuning (it is set as per thousand of step amplitude)

int rise_time_freedom_percent_autotune

This value helps the tuner to find out whether the ki is high enough or not.

By default set this value to 300, and if the tuner is not able to find proper values (and the response is having oscillations), increase this value to 400 or 500.

int velocity_kp

Parameter for velocity controller P-constant.

int velocity_ki

Parameter for velocity controller I-constant.

int velocity_kd

Parameter for velocity controller D-constant.

int velocity_integral_limit

Parameter for integral limit of velocity pid controller.

int enable_velocity_auto_tuner

Parameter for enabling/disabling auto tuner for velocity controller.

int k_fb

Parameter for setting the feedback position sensor gain.

int resolution

Parameter for setting the resolution of position sensor [ticks/rotation].

int k_m

Parameter for setting the gain of torque actuator.

int moment_of_inertia

Parameter for setting the moment of inertia.

MotionPolarity polarity

Parameter for setting the polarity of the movement.

int brake_release_strategy

Parameter for setting different brake release strategies, e.g., shaking.

int brake_release_delay

Parameter for setting the delay between removing voltage from the brake and disabling the control.

int dc_bus_voltage

Parameter for setting the nominal (rated) value of dc-link.

int pull_brake_voltage

Parameter for setting the voltage for pulling the brake out!

int pull_brake_time

Parameter for setting the time of brake pulling.

int hold_brake_voltage

Parameter for setting the brake voltage after it is pulled.

int filter

Warning

doxygenstruct: Cannot find struct “MotionPolarity” in doxygen xml output

ControlConfig

Structure definition for a Control Loop Service configuration.

Structure Members:

int Kp_n

Value for proportional coefficient (Kp) in PID controller.

Kp = Kp_n/PID_DENOMINATOR (by default PID_DENOMINATOR = 10000)

int Ki_n

Value for integral coefficient (Ki) in PID controller.

Ki = Ki_n/PID_DENOMINATOR (by default PID_DENOMINATOR = 10000)

int Kd_n

Value for differential coefficient (Kd) in PID controller.

Kd = Kd_n/PID_DENOMINATOR (by default PID_DENOMINATOR = 10000)

int control_loop_period

Period for the control loop [microseconds].

int feedback_sensor

Sensor used for position control feedback [HALL_SENSOR, QEI_SENSOR].

int cascade_with_torque

Add torque controller at the end of velocity controller (only possible with FOC) [0, 1].

Motion Control Service

void motion_control_service(MotionControlConfig &pos_velocity_control_config, interface TorqueControlInterface client i_torque_control, interface MotionControlInterface server i_motion_control[3], client interface UpdateBrake i_update_brake)

Service to perform torque, velocity or position control.

You will need a Motor Control Stack running parallel to this Service, have a look at Motor Control Service for more information.

Note: It is important to allocate this service in a different tile from the remaining Motor Control stack.

Parameters:
  • pos_velocity_control_config – Configuration for ttorque/velocity/position controllers.
  • i_torque_control – Communication class to the Motor Control Service.
  • i_motion_control[3] – array of MotionControlInterfaces to communicate with upto 3 clients
  • i_update_brake – Interface to update brake configuration in PWM service
Returns:

void

void init_motion_control(interface MotionControlInterface client i_motion_control)

Initializer helper for the Position Control Service.

It is required the client to call this function before starting to perform position control.

Parameters:
  • i_motion_control – Communication class to the Position Control Service.
Returns:

void

Position Control Interface

interface MotionControlInterface

Interface type to communicate with the Motion Control Service.

void disable()

disables the motion control service

void enable_position_ctrl(int mode)

enables the position controler

Parameters:
  • mode-> – position control mode
void enable_velocity_ctrl(void)

enables the velocity controller

void set_j(int j)

sets the moment of inertia of the load

Parameters:
  • j-> – moment of intertia
void enable_torque_ctrl()

enables the torque controller

void set_torque(int target_torque)

sets the reference value of torque in torque control mode

Parameters:
  • target_torque – -> torque reference in mNm
MotionControlConfig get_motion_control_config()

Getter for current configuration used by the Service.

Returns:Current Service configuration.
void set_motion_control_config(MotionControlConfig in_config)

Setter for new configuration in the Service.

Parameters:
  • in_config – New Service configuration.
void set_motorcontrol_config(MotorcontrolConfig in_config)

Setter for new configuration in the Motorcontrol Service.

Parameters:
  • in_config – New Service configuration.
MotorcontrolConfig get_motorcontrol_config()

Getter for current configuration used by the Motorcontrol Service.

Returns:Current Service configuration.
void set_brake_status(int brake_status)

Sets brake status to ON (no movement) or OFF (possible to move)

Parameters:
  • brake_status – -> release if 1, block if 0
void update_brake_configuration()

updates the new brake configuration in pwm service

MotorcontrolConfig set_offset_detection_enabled()

Enables the offset detection process.

void reset_motorcontrol_faults()

Send a reset fault command to the motorcontrol.

void set_safe_torque_off_enabled()

Enables the safe-torque-off mode.

Returns:void
int get_position()

getter of actual position

int get_velocity()

getter of actual velocity

UpstreamControlData update_control_data(DownstreamControlData downstreamcontroldata)

responsible for data communication between torque controller and higher level controllers

Parameters:
  • downstreamcontroldata – -> structure including the commands for torque/velocity/position controller
Returns:

structure of type UpstreamControlData -> structure including the actual parameters (measurements, ...) from torque controller to higher controlling levels