I am trying to simulate a small differential wheeled robot that I am using for my swarm project. It is very small, and I made a basic model of the robot. The motors are controlled from an arduino by PWM, and there are encoders to give feedback on how much the two wheels have turned. My task is to do dead reckoning.

Since the existing differential drive plugin doesnot control the motors like how the arduino does, I made a code on my own to simulate the code run in the Arduino. But when I run the code, I get unexpected results as shown in the first 15 seconds of this youtube video I have uploaded.

Here is my code, if you ever want to look. The plugin gets the pos (x,y only) of the coordinate to which the robot should move, and it will be stored in xtarget and ytarget. For now, I have simply initialized both these variables to some value in the constructor for testing purpose.