uint32_t MOT_GetMedianPosition(void)
{
     int i;
     uint32_t position=0;
     
     for (i = 0; i < g_linked_motors; i++)
     {
         position += linked_motor_data[i]->position;
     }
     position /= g_linked_motors;
     
     return position;
}

uint32_t ProcessMedianPosition(void)
{
    int i;
    uint32_t pos_diff=0;
    
    uint32_t median_position = MOT_GetMedianPosition();
    
    for (i = 0; i < g_linked_motors; i++)
    {
        if ( abs(linked_motor_data[i]->position - median_position) > glob_mot_data.max_pos_diff)
        {
            pos_diff = 1;
            
            if (linked_motor_data[i]->position > median_position)  //motor runs faster
            {
                //adjust speed -> speed down
                linked_motor_data[i]->pwm -= glob_mot_data.pwm_delta;
                //check limit
                if (linked_motor_data[i]->pwm < glob_mot_data.pwm_min)
                    linked_motor_data[i]->pwm = glob_mot_data.pwm_min;
            }
            else   //motor runs slower
            {
                //adjust speed -> speed up
                linked_motor_data[i]->pwm += glob_mot_data.pwm_delta;
                //check limit
                if (linked_motor_data[i]->pwm > glob_mot_data.pwm_max)
                    linked_motor_data[i]->pwm = glob_mot_data.pwm_max;
            }
        }
    }
    return pos_diff;
}

void MOT_PositionLoopMulti(void)
{
    int i;
    if (ProcessMedianPosition())
    {
        //retry
        glob_mot_data.pos_diff_count++;
        //maximum retries - unable to adjust speed
        if (glob_mot_data.pos_diff_count > glob_mot_data.max_pos_diff_count)
        {
            glob_mot_data.pos_diff_count = 0;
             //send stop command
            for (i = 0; i < g_linked_motors; i++)
                MOT_SendData(linked_motor_data[i], CAN_COM_STOP, 0);
        }
        else
        {
            //update speed
            for (i = 0; i < g_linked_motors; i++)
                MOT_SendData(linked_motor_data[i], CAN_COM_SSET, linked_motor_data[i]->pwm);
        }
    }
    else
    {
       glob_mot_data.pos_diff_count = 0;
       //set maximum speed
       for (i = 0; i < g_linked_motors; i++)
           MOT_SendData(linked_motor_data[i], CAN_COM_SSET, glob_mot_data.pwm_max);
    }   
}