1
$\begingroup$

I implement different drive odometry for a robot. Trying to implement (proportional–integral–derivative controller) PID-based coordinate movements x, y, theta:

public void getX(double dt) {x += Math.cos(theta) * robotV() * dt;}
public void getY(double dt) {y += Math.sin(theta) * robotV() * dt;}
public void getTheta(double dt) {
    theta += ((rightLenV()-leftLenV()) / Constants.transmission_width)*dt;
    theta = adduction(theta);
}
public double adduction(double a){
    while (a > Math.PI) {a = a-2*Math.PI;}
    while (a < -Math.PI) {a = a+2*Math.PI;}
    return a;
}

my implement pid controller (bad working)

pidControllerVelocity = new PIDController(10, 0, 5);
public void velocity(double x_W, double y_W) {
    x_D = x_W - x;
    y_D = y_W - y;
    dS = Math.sqrt((Math.pow(x_D, 2) + Math.pow(y_D, 2)));


    u = Math.atan2(y_D, x_D);

    pidControllerVelocity.setSetpoint(adduction(u));
    double PD_velocity = pidControllerVelocity.calculate(theta);

    if (dS < 0.015) {
        leftMotor.set(0);
        rightMotor.set(0);
    } else {
        leftMotor.set(0.1+PD_velocity);
        rightMotor.set(0.1-PD_velocity);

    }
}

I'm working with Java library wpilib.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

The kinematics - odometry part seems okay. I think your general configuration for the controller is incorrect.

First of all, you are going to need to use two high-level controllers. One of them is going to provide the reference forward speed of your robot, V. The other one is going to provide the reference steering of the robot. Then you can map these desired values to the wheel speeds, in order to actuate your robot.

You have already calculated dS. You feed this dS to a controller (a sole P Controller may work just OK, you may not need the other terms). The output of the controller is going to be the desired speed - V_des. You also have already calculated angle "u", which is the relative angle of the vehicle to the goal point. You subtract the vehicle "theta" from this angle "u" to have the orientation error. Then, you can feed this error to a separate controller - a sole P controller may be enough for this too. The output of this controller is going to be the desired rate of orientation - Theta_dot_desired.

From your odometry function, we already know that the Theta_dot is equal to the speed difference between the right and left wheel, divided by the wheel base. It is not written in your odometry code directly, but I believe that the robotV variable must be equal to the sum of the wheel speeds, divided by two. From these two equations, you can solve for the wheel speeds, given the desired robotV and Theta_dot.

You can use the following equations I have written as a reference:

General Kinematics, given the Wheel Speeds:

enter image description here

Controller Equations:

enter image description here

Substituting the Controller Outputs to the Kinematic Equations, in order to calculate the Inverse Kinematics:

enter image description here

I am not very sure about how to use your library, but I believe that you can work it out if you understand the controller structure.

Improve this answer
$\endgroup$
8
  • $\begingroup$ the last two formulas that you wrote for vr and vl are substituted into the motors? $\endgroup$
    – gleb
    Commented May 14, 2022 at 10:39
  • $\begingroup$ If the motor rotation is directly related to the wheel speeds, then yes. The last two formulas tell at what speed each wheel is required to rotate. $\endgroup$
    – kucar
    Commented May 14, 2022 at 10:55
  • $\begingroup$ a rather strange bug or feature was discovered by me for some reason if I ask it to move to y= 2, then it starts to go wrong, but if y < 1 then everything is correct $\endgroup$
    – gleb
    Commented May 19, 2022 at 13:44
  • $\begingroup$ There might be many different reasons to that, however, maybe since the initial positional error is too high when y = 2; a large signal is being sent to the motors. You may try add upper and lower bounds to the motor speeds. In addition, for the orientation control you need to keep the angles between -pi, and pi. Maybe when y = 2, the result of the atan function or the overall expression produces angles which do not lie between -pi, and pi. You may consider using atan2 function, in that case. $\endgroup$
    – kucar
    Commented May 19, 2022 at 14:08
  • $\begingroup$ having chosen P more correctly, the coefficient has achieved a more even ride along y, but for large y it travels in an arc to the point $\endgroup$
    – gleb
    Commented May 20, 2022 at 12:32

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.