Function InitEncoder {
Interrupt setups;
Initialize rising-edge capture mode to the left and right encoder pins;
}

Function LeftEncoder_Response {
Compute period based on this capture and last caputure;
Update last capture;
Return the period from left encoder;
}

Function RightEncoder_Response {
Return the period from right encoder;
}

Function Read_RightEncoder {
Get period of right encoder signal;
Filter out noises of the period signal;
Compute equivalent R_CurrentPWM based on the period; 
Output R_CurrentPWM;
}

Function PIDresponseRight {
Query R_DesiredPWM input for right wheel;
Query R_CurrentPWM for right wheel;
Calculate control input ur based on PI control law;
Anti wind-up for ur;
SetPWM_Right(u);
}

Function Read_LeftEncoder {
Get period of left encoder signal;
Filter out noises of the period signal;
Compute equivalent L_CurrentPWM based on the period; 
Output L_CurrentPWM;
}

Function PIDresponseLeft {
Query L_DesiredPWM input for left wheel;
Query L_CurrentPWM for left wheel;
Calculate control input ul based on PI control law;
Anti wind-up for ul;
SetPWM_Left(u);
}