# PID Control on differential drive robot

I have tried to control the Pioneer robot in Webots simulator to follow the static target. This is the code I have developed. My main goal is to make a go-to single goal controller. The problem is that if the start point angle of the robot (the yaw) and the endpoint angle for example would be zero, then it would not move along the y direction. Px(x,y,theta) For example, if the robot's start point is Ps(0,0,0) and the target is to reach end point Pe(4,1,0) or Pe(1,4,0), and have zero velocity on target, then it will not move along y direction at all.

from controller import Robot
import math
robot = Robot()
timestep = int(robot.getBasicTimeStep())
leftMotor = robot.getDevice('left wheel motor')
rightMotor = robot.getDevice('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
destX = 1
destY = 4
destT = 0
gps = robot.getDevice('gps')
gps.enable(timestep)
errIx = 0
errIy = 0
errIt = 0

kP = 0.1
kI = 0.0
kD = 0.05

L = 0.1
R = 1.1

position = gps.getValues()
while robot.step(timestep) != -1:
oldPosition = position
position = gps.getValues()
theta = math.atan2(position[1]-oldPosition[1],position[0]-oldPosition[0])

errX = destX - position[0]
errY = destY - position[1]
if (math.isnan(theta)):
errT = 0
else:
errT = destT - theta

errIx = 0.99 * errIx + 0.01 * errX
errIy = 0.99 * errIy + 0.01 * errY
errIt = 0.99 * errIt + 0.01 * errT

uX = errX * kP + errIx * kI # x dot or vx
uY = errY * kP + errIy * kI # y dot or vy
uT = errT * kP + errIt * kI # omega or theta dot
v = math.sqrt(uX**2 + uY**2)
vL = (v - (L * uT /2))/R
vR = (v + (L * uT /2))/R

if (abs(errX)< 0.1 and abs(errY)< 0.1 and abs(errT)< 0.1 ) :
leftMotor.setVelocity(0)
rightMotor.setVelocity(0)
break

leftMotor.setVelocity(vL)
rightMotor.setVelocity(vR)

• write that in a form of a question and add it to your post ... I want to ... is not a question Commented May 10 at 19:25