My thesis was motivated by a real problem. Nowadays the available RC servo motors are not sufficient enough for the robotics applications. The original servo controller was not able to compensate the effect of the strain. Sometimes the robots can’t compensate even their own weight, and these effects cause disturbance in the operation of the system. Our goal in the semester was to develop an algorithm which could compensate these strain effects. The thesis can be separated for three main part. The first is that we have some problem with the hardware, especially we have already an H bridge but there were some operational problems whit this configuration. In the second part I have made an identification process on the system, and based on these results, I have developed a discrete time controller. The controller was tested by simulations, I measured the efficiency of the controller. In the last part I have substituted the estimated strain with the measurement of the current. Finally I have implemented my algorithm on the ATtiny85 microcontroller in C language.