//================================================================
// output signal     - duty, adc_a_data[0], range : 0 (-100%) - 2047 (0%) - 4096 (100%)
// command signal     - speed(RPM)
// feedback signal     - speed(RPM)  

current_position = (long)EQep1Regs.QPOSCNT;    // Get the current strut position

error_position =     ref_position - current_position;    // Get position error
error_differential = error_position - error_before;
//Position Control
command_rpm = (-1)*(error_position * kp  + error_integral * ki + error_differential * kd);
command = command_rpm / N_GAIN;
command +=  2047;
// Limit output value
if(command > 0xfff)    command = 0xfff;
if(command < 0x000) command = 0x000;    

// Setting motor speed
adc_a_data[0] = command;               
// Save current error
error_before    =    error_position;
error_integral +=    error_position;

//======================================================
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;