uint32_t MOT_GetPosDif(uint32_t pos1, uint32_t pos2, uint32_t *gt)
{
    if( pos1 > pos2)
    {
        *gt = 0;
        return (pos1 - pos2);
    }
    else
    {
        *gt = 1;
        return (pos2 - pos1);
    }
}

void MOT_PositionLoopDouble(void)
{
    uint32_t position1, position2, posdif, gt;
    uint32_t pwm1, pwm2;
    
    position1 = motor1_data_double->position;
    position2 = motor2_data_double->position;
    
    posdif = MOT_GetPosDif(position1, position2, &gt);
    
    if (posdif > glob_mot_data.max_pos_diff)
    {
        //retry
        glob_mot_data.pos_diff_count++;
        if (glob_mot_data.pos_diff_count > glob_mot_data.max_pos_diff_count)
        {
            //maximum retries - unable to adjust speed
            glob_mot_data.pos_diff_count = 0;
            //send stop command
            MOT_SendData(motor1_data_double, CAN_COM_STOP, 0);
            MOT_SendData(motor2_data_double, CAN_COM_STOP, 0);
            
            sys_status |= MOT_POS_DIFF;
        }
    }
    else
        glob_mot_data.pos_diff_count = 0;
    
    //no position difference - maximum speed  
    if (posdif < glob_mot_data.max_pos_diff)
    {
        MOT_SendData(motor1_data_double, CAN_COM_SSET, glob_mot_data.pwm_max);
        MOT_SendData(motor2_data_double, CAN_COM_SSET, glob_mot_data.pwm_max);
    }
    else
    {
        pwm1 = motor1_data_double->speed;  
        pwm2 = motor2_data_double->speed;
    
        if (gt == 0)  //position1 > position2 -> motor1 runs faster
        {
            //adjust pwm
            pwm1 -= glob_mot_data.pwm_delta; //motor1 speed down
            pwm2 += glob_mot_data.pwm_delta; //motor2 speed up
        
            //check min pwm limit
            if (pwm1 < glob_mot_data.pwm_min)
                pwm1 = glob_mot_data.pwm_min;
            //check max pwm limit    
            if (pwm2 > glob_mot_data.pwm_max)
                pwm2 = glob_mot_data.pwm_max;  
        }
        else if (gt == 1)  //position2 > position1 -> motor2 runs faster
        {
            //adjust pwm
            pwm1 += glob_mot_data.pwm_delta;  //motor1 speed up
            pwm2 -= glob_mot_data.pwm_delta; //motor2 speed down
        
            //check max pwm limit
            if (pwm1 > glob_mot_data.pwm_max)  
                pwm1 = glob_mot_data.pwm_max;
            //check min pwm limit    
            if (pwm2 < glob_mot_data.pwm_min)
                pwm2 = glob_mot_data.pwm_min;
        }
    }
    
    //update speed
     MOT_SendData(motor1_data_double, CAN_COM_SSET, pwm1);
     MOT_SendData(motor2_data_double, CAN_COM_SSET, pwm2);
}

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);
    }  
}