How-To: Building a good Auton without Odometry or GPS

VEX Robotics Competition Discussion VRC
#21

No need for the setPosition command.

By literally having one PID right after another. You type one, hit return, and then type another.

Cool

Do not keep resetting the position. We want to know how far it has gone.

Lets amend this for you:

    double heading = Inertial9.rotation;
while (avgDrive() < distance) {
    terror = distance - avgDrive();  //distance is the desired output, and avgDrive is your function that returns the average distance traveled.
//derivative line here   
 toutput = terror * tkP; // + tder*tkD

    rerror = heading - Inertial9.rotation(); //5 
//derivative line here
    routput = error * rkP; // +rder*rkD



    LeftDrive.spin(forward, toutput - routput, percentUnits::pct);
    RightDrive.spin(forward, toutput + routput, percentUnits::pct);

There we go. However, I think that you should use volts instead of velocity. Velocity makes the smart motors run their own velocity PID, putting a second layer of conflated processing and possible oscillations and inefficiencies. By directly setting the voltage, you can ignore the built-in smart motor PIDs and directly control the motors with your feedback controller.

5 Likes
#22

Thank you so much for your amendments :blush:. Heres my updated code with voltage and derivative. If this is all good, all that would be left is to tune it, right?

void translate(int distance) {
  double terror;
  double rerror;

  double tder;
  double rder;

  double toutput;
  double routput;

  double tkP = 1.5;
  double rkP = 1.5;

  double rkD = 1.5;
  double tkD = 1.5;

  double tlastError = 0;
  double rlastError = 0;

  double heading = Inertial9.rotation;

  while (avgDrive() < distance) {
    terror = distance - avgDrive();  //distance is the desired output, and avgDrive is your function that returns the average distance traveled.
    tder = terror - tlastError;
    toutput = (terror * tkP) + (tder * tkD); // + tder*tkD


    rerror = heading - Inertial9.rotation(); //5 
    rder = rerror - rlastError;
    routput = (rerror * rkP) + (rder*rkD); // +rder*rkD

    LeftDrive.spin(forward, toutput - routput, voltageUnits::volt);
    RightDrive.spin(forward, toutput + routput, voltageUnits::volt);

    rlastError = rerror;                              
    tlastError = terror;
  }
}
#23

Very good. Looking at this, I feel like my advise fell short of an optimally formatted and labeled version of this, but I cannot think of what a better version might look like. At least it is well formatted, logical, and easy to read.

I recommend setting your motors back to zero position at the start of this, outside of the while loop.

Now you are going to want to eventually put in some integral elements, but they are much harder to work with due to windup, and when you do you will need to reduce kp by a factor of around *0.7 to make up for the added push of the integral term.

  • Wait Term: You will want to have a wait term of like 20ms to set the pacing. This is super necessary when this eventually becomes a task.

  • watchdog timer: you will want to have a timer that checks to see if it has gone on for too long, and then exits the loop if the maximum time has been exceeded. Easy way to do this is set your while (true) to be while (continue==true) and then change it to false.

  • Tolerance: Instead of just exiting when the position = target value, you also want to make sure the speed is low. You can use an if statement to check if everything is small enough (abs(err)<threshold) or make something fancier like (abs(err)+abs(der)<threshold).

  • have some arguments like tolerance, max speed (hard to write), inertia (changes your k values depending on how many objects you have), heading, max time, or if you want to stop or coast through something.

  • Something about feedforward, which is useful in some PIDs like lifts where you usually have a bias (like gravity)

Your code is only a few lines separate from the vision pid that my team used to grab the center goal even after a robot had plowed into it during auton. Once you learn how to get a good pid running, using different sensors is only as hard as mastering the sensor class methods.

Also, you can begin to integrate heading and odometry quite easily with this pid. Just use trig to derive a desired heading value from the (x, y) of your robot vs the (x’,y’) of the target location, and use pythag for a magnitude. You can also spline or Bezier along curves by making your (x’,y’) or “Carrot on a stick” as I call it, move as the output of a function that follows said curve based on how far along it you are. Lots of semi-scripted methods for scripting your robot’s path before looking to stuff like A*, route planning, and more. My recommendation is after getting this down, start playing with vision, where the sensor value is whatever the x coordinate of the largest object is, and the target value is the middle of the screen (you will need to figure that out on your own).

But thats just, like, my opinion. Other teams have other methods and names for that “next step” in autonomous programming.

3 Likes
#24

I don’t think using voltage instead of velocity is that important for most aspects of auto, but for driver absolutely.
Honestly once you understand a few key concepts and trig, Odom isn’t as hard as I think people make it out to be. But, it also doesn’t solve everything perfectly I’ve found. I use time based, normal distance while loops, and the vision sensor in addition to Odom. Time based is especially important if you want to do something like push an object into a wall. To short, and you won’t get there, to far and you’ll get stuck and won’t reach target.

Something slightly off topic that I think is important that no one ever mentions is that it’s important for all team members to understand some basic programming. Not how to program, but just enough to know when a problem can or can’t be solved by programming. There are a lot of thing’s that programming fixes a lot better than building something, such as a variable speed stacker motor for tower take over. I saw so many teams tossing stacks over running 100 percent speed with the tray vertical. Vice versa, some things just can’t be fixed with programming, and it’s important to recognize the difference.

1 Like
#25

Hi, thank you for the advice along the way. As a freshman, this is really cool and I’ll probably implement more advanced things later. One thing though, I am having a lot of trouble tuning this PD. I understand that kP needs to be raised until small oscillations, but I dont know where kP should start. Same with kD.

#26

I am glad to see that as a freshman you are taking on programming of this caliber. As mentioned, pushing objects into walls, grabbing objects/intaking, and tug-of-wars require very clever thinking and application of these (not so basic) PIDs, among other logic and sensors.

Tuning: Set kD to zero, and kP to 1. Change kP and continually adjust kP until it overshoots a little and oscillates a bit. Then tune kD.

For tuning kP you are essentially using a search algorithm to jump back and forth, bounding kP and zeroing in on a good number.
Here are some sample steps

  • Overshoots a lot: Divide kP by 10 (big number) and repeat.
  • Overshoots a little: Divide kP by 2 (small number) and repeat.
  • Moves way too slow: Multiply kP by 10 (Big number) and repeat
  • Could be stronger: Multiply kP by 1.5 (small number)

Stop when it overshoots a bit, and oscillates.

Tuning kD: Start at kd=kp. Double check that your wait is sufficient and that you are not printing anything to the controller or doing other crazy time intensive actions during your PID. Just repeat the process for kP, multiplying kD or dividing it until it slows it down nicely and gets rid of the oscillations.

If kD is way too big, it behaves erratically, basically reversing the motors constantly. If it is too small, it doesn’t do anything.

There are search algorithms for this that use the frequency of oscillations to dictate the precise values for things, but you can look those up.

2 Likes
#28

I really want to do odemetry since we are rebuilding our chassis for worlds.I just found the 23880B’s skills code for odemetry that you posted in a different post. I will try to implement this