VEX Ultrasonic Sensor

[Oromus]

I can't help you very much with the vision tracking part of the code due to never EVER working with the darn stuff, but I can help with the 6 inch spacing part.

//After you've found the stack and lined up
double kP = .2
double minSpeed = 0.3;
double maxSpeed = 0.5;
distance = ultra.getRangeInches();
if (distance != 6) {
     while (true) {
          distance = 6 - ultra.getRangeInches(); //Update the distance from our goal (6 inches).
          double p = distance * kP; //Calculate p, which is the speed we want to move at.
          
          //Making sure p is within maxSpeed and minSpeed and correcting it if it isn't.
          if (Math.abs(p) > maxSpeed) {
               p = maxSpeed * (Math.abs(p)/p); //p equals maxSpeed multiplied by the sign of p
          } else if (Math.abs(p) < minSpeed) {
               p = 0; //We're moving too slow, so we're saying we've stopped and hit our goal.
          }

          //Set our motors to p.
          motor1.set(p);
          motor2.set(p);

         //If we're not moving, we've made it to our goal and want to exit this loop.
          if (p == 0) {
               break;
          }
     }
}

Now let me explain what this is and how it works. This is a very simple P loop. How P loops work is a speed is calculated based off of how far you are away from your target. The farther you are away, the faster you move. The speed is calculated by multiplying distance by kP, which you'll need to fine-tune the value of to get it working well. The lower kP is, the more speed scaling you'll see (i.e. the robot slowing down much more as it approaches 6 inches). You'll also want to adjust maxSpeed and minSpeed. maxSpeed is the fastest the robot is allowed to go, and minSpeed is how slow the robot can be going before we say we're arrived at our destination.

This may be all you need, or you may need to expand on this and make it a PI, PD, or PID loop. If you have any issues with this, feel free to ask. I'm happy to help