0
$\begingroup$

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)
$\endgroup$
3
  • $\begingroup$ what is your question? ... please add a focused, answerable question to your post $\endgroup$
    – jsotola
    Commented May 10 at 19:05
  • $\begingroup$ @jsotola I want a go to goal controller, preferably based on pid. $\endgroup$
    – V.Ajall
    Commented May 10 at 19:24
  • $\begingroup$ write that in a form of a question and add it to your post ... I want to ... is not a question $\endgroup$
    – jsotola
    Commented May 10 at 19:25

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.