Pure Pursuit Program Help

Programming Support VEXcode V5 Tech Support
1

Hi I am the programmer/driver/builder/designer/leader (yes I know its a lot I’m the only person who has experience on this team) for VEX VRC team 91976A and I have been working on a pure pursuit program for this years game. I’m doing this because my team missed the mark for worlds last year because our program was bad, mostly because we have no kind of sensors or tracking. Which was really sad because we were third in qualifications and made it to semifinals for our intake motor to get unplugged.

I wanted to make sure that wont be a problem and make this program so that our skills will be something to fall back on. My main issue is that I am a bad programmer so I read all of the papers and other teams code to try to make one of my own. One issue I had though was that I had to make this in VEX Code due to my team only having access to Chromebooks.

I wanted help with formatting this code for the field control processes. For example during autonomous and driver control and things I want to run on the brain screen before matches. I don’t know how I would make it accessible for me to add new tracking points and have the intake run when the robot is at some target point. I tried to format this to run during driver control and autonomous and just got a ton of errors.

Another this is that the code doesn’t works very well and I don’t know if its because I’m bad at calibrating it and I’m doing it the wrong way or if I flat out just coded it wrong. I tried to make sure I coded it wright but truthfully I have no Idea what I’m doing and this is what came out.

This was coded in C++ btw

#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>


#include "vex.h"

using namespace vex;

// Brain should be defined by default
brain Brain;


// START V5 MACROS
#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

#define repeat(iterations)                                                     \
  for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS


// Robot configuration code.
rotation horizontalWheel = rotation(PORT1, false);

rotation verticalWheel = rotation(PORT2, false);

inertial imuSensor = inertial(PORT3);

motor Left_Drive_Bottom = motor(PORT4, ratio6_1, true);

motor Right_Drive_Bottom = motor(PORT5, ratio6_1, true);

motor Left_Drive_Mid = motor(PORT6, ratio6_1, true);

motor Right_Drive_Mid = motor(PORT7, ratio6_1, true);
#pragma endregion VEXcode Generated Robot Configuration

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       {author}                                                  */
/*    Created:      {date}                                                    */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// Include the V5 Library
#include "vex.h"
  
// Allows for easier use of the VEX Library
using namespace vex;



// Define your hardware here
// motor horizontalWheel = motor(PORT1);
// motor verticalWheel = motor(PORT2);
// inertial imuSensor = inertial(PORT3);

// Constants
const double TRACKING_WHEEL_RADIUS = 2.75; // Replace with the actual radius of your tracking wheels
const double TRACKING_WHEEL_CIRCUMFERENCE = 2 * 3.14159 * TRACKING_WHEEL_RADIUS;
const double DRIVE_WHEEL_RADIUS = 4; // Replace with the actual radius of your robot's wheels
const double DISTANCE_TOLERANCE = 1.0;     // Tolerance for distance to the target (in units of your coordinate system)
const double HEADING_TOLERANCE = 2.0;      // Tolerance for heading difference (in degrees) 
const double LOOKAHEAD_DISTANCE_SCALE = 2.0; // Adjust this value to control the lookahead distance

// Variables
double currentX = 0.0;
double currentY = 0.0;
double heading = 0.0; // In degrees

event DriverControl = event();
event Autonomous = event();
event Started = event();

// Function to update the robot's position and heading
void updatePosition() {
    // Read the rotation sensor values for horizontal and vertical wheels
    // Example:
    int horizontalRotation = horizontalWheel.angle(rotationUnits::deg);
    int verticalRotation = verticalWheel.angle(rotationUnits::deg);

    // Read the heading from the inertial sensor
    // Example:
    double heading = imuSensor.heading();

    // Calculate the distance traveled by horizontal and vertical wheels
    double horizontalDistance = (horizontalRotation / 360.0) * TRACKING_WHEEL_CIRCUMFERENCE;
    double verticalDistance = (verticalRotation / 360.0) * TRACKING_WHEEL_CIRCUMFERENCE;

    // Calculate the change in X and Y position
    double deltaX = (horizontalDistance - verticalDistance) / 2.0;
    double deltaY = (horizontalDistance + verticalDistance) / 2.0;

    // Update the X and Y position
    currentX += deltaX * cos(heading * 3.14159 / 180.0);
    currentY += deltaY * sin(heading * 3.14159 / 180.0);

    // Normalize the heading to [0, 360) degrees
    heading = fmod(heading, 360.0);
    }

    // Function to calculate the distance between two points (X1, Y1) and (X2, Y2)
    double distance1(double x1, double y1, double x2, double y2) {
    return sqrt(pow((x2 - x1), 2) + pow((y2 - y1), 2));
    }

    // Function to calculate the angle between two points (X1, Y1) and (X2, Y2)
    double angle(double x1, double y1, double x2, double y2) {
    return atan2(y2 - y1, x2 - x1) * 180.0 / 3.14159;
    }


    // Function to calculate the lookahead distance
    double calculateLookaheadDistance(double currentVelocity) {
    // Convert the velocity from RPM to inches per second (or other units as used for your coordinates)
    double velocityInInchesPerSecond = (currentVelocity * 2.0 * 3.14159 * DRIVE_WHEEL_RADIUS) / 60.0;

    // Calculate the lookahead distance based on the velocity and the LOOKAHEAD_DISTANCE_SCALE
    double lookaheadDistanceInInches = velocityInInchesPerSecond * LOOKAHEAD_DISTANCE_SCALE;

    // Return the lookahead distance in inches (or other units as used for your coordinates)
    return lookaheadDistanceInInches;

    return LOOKAHEAD_DISTANCE_SCALE * currentVelocity;
}


// Function to move the robot to a specified X, Y, and heading using pure pursuit
void moveTo(double targetX, double targetY, double targetHeading) {
    // Main control loop
    while (true) {
        // Read the current X, Y position and heading from sensors
        double currentX = 0.0; // Replace with the actual function to get the current X position
        double currentY = 0.0; // Replace with the actual function to get the current Y position
        double currentHeading = heading; // Replace with the actual function to get the current heading

        // Calculate the distance to the target point
        double distanceToTarget = distance1(currentX, currentY, targetX, targetY);

        // Calculate the difference between the target heading and the current heading
        double headingDifference = targetHeading - currentHeading;

        // Adjust headingDifference to be in the range [-180, 180] degrees
        if (headingDifference > 180.0) {
            headingDifference -= 360.0;
        } else if (headingDifference < -180.0) {
            headingDifference += 360.0;
        }

        // Get the average RPM of both sides of drivetrain wheels
        double leftMotorRPM = 0.6 * Left_Drive_Bottom.velocity(rpm);
        double rightMotorRPM = 0.6 * Right_Drive_Bottom.velocity(rpm);
        double averageMotorRPM = (leftMotorRPM + rightMotorRPM) / 2.0;

        // Calculate the current velocity (RPM) of the robot (replace with actual velocity calculation)
        double currentVelocity = averageMotorRPM;

        // Calculate the distance to the point on the lookahead circle
        double lookaheadDistance = calculateLookaheadDistance(currentVelocity);

        // Convert the lookahead distance to radians
        double lookaheadRadians = currentHeading * 3.14159 / 180.0;

        // Calculate the lookahead point coordinates
        double lookaheadX = currentX + lookaheadDistance * sin(lookaheadRadians);
        double lookaheadY = currentY + lookaheadDistance * cos(lookaheadRadians);
        double distanceToLookahead = distance1(currentX, currentY, lookaheadX, lookaheadY);

        // Calculate the curvature of the path
        double curvature = 2 * sin(headingDifference * 3.14159 / 180.0) / distanceToLookahead;

        // Calculate left and right velocities based on the curvature (adjust as needed)
        double maxVelocity = 600; // Adjust this based on your robot's capabilities
        double leftVelocity = maxVelocity * (2 + curvature * 0.5); // Adjust 0.5 based on your robot's dimensions
        double rightVelocity = maxVelocity * (2 - curvature * 0.5); // Adjust 0.5 based on your robot's dimensions

        Left_Drive_Bottom.spin(forward, leftVelocity, velocityUnits::rpm);
        Right_Drive_Bottom.spin(forward, rightVelocity, velocityUnits::rpm);
        Left_Drive_Mid.spin(forward, leftVelocity, velocityUnits::rpm);
        Right_Drive_Mid.spin(forward, rightVelocity, velocityUnits::rpm);

        // Break out of the loop if the robot has reached the target point and heading
        if (distanceToTarget < DISTANCE_TOLERANCE && fabs(headingDifference) < HEADING_TOLERANCE) {
            Left_Drive_Bottom.stop();
            Right_Drive_Bottom.stop();
            Left_Drive_Mid.stop();
            Right_Drive_Mid.stop();
            break;
        }

        // Delay to prevent excessive updates (adjust as needed)
        wait(20, msec);
    }
}

int main() {
  // Initialize VEX V5 components

  // List of target positions and headings
  struct TargetPoint {
    double x;
    double y;
    double heading;
  };

  TargetPoint targets[] = {
    {6, 6, 90},     // Target point 1
    {10, 8, 45},    // Target point 2
    {-2, 4, 180},   // Target point 3
    // Add more target points as needed
  };

      vex::competition::bStopTasksBetweenModes = false;
      Competition.autonomous(VEXcode_auton_task);
      Competition.drivercontrol(VEXcode_driver_task);

      // register event handlers
      DriverControl(onevent_DriverControl_0);
      Autonomous(onevent_Autonomous_0);
      Started(onevent_Started_0);

      wait(15, msec);
      // post event registration

      // set default print color to black
      printf("\033[30m");

      // wait for rotation sensor to fully initialize
      wait(30, msec);

      whenStarted1();

    int currentTargetIndex = 0; // Variable to track the current target point index

    // Main control loop
    while (true) {
        // Update the robot's position and heading
        updatePosition();

        // Perform other motor functions or tasks here

        // Check if the robot has reached the current target point
        if (currentTargetIndex < sizeof(targets) / sizeof(targets[0])) {
            double targetX = targets[currentTargetIndex].x;
            double targetY = targets[currentTargetIndex].y;
            double targetHeading = targets[currentTargetIndex].heading;

            // Move the robot to the current target point and heading using pure pursuit
            moveTo(targetX, targetY, targetHeading);

            // Increment the current target index to move to the next target point
            currentTargetIndex++;
        }

        // Perform other motor functions or tasks here

        // Delay to prevent the program from exiting immediately
        wait(100, msec);
    }

    // End of the main loop
}
2

You can’t reliably track position with a 100 msec delay. I would also recommend using multiple internal sensors and averaging them out to minimize drift.

1 Like
3

Like VexThirteen Said, you don’t want this where it Is, you want to move it after the curly bracket directly below it, (Move this line 2 lines down basically)

Personally, I would change this to a command by command system, you can keep the list of points, or just put them in manually, by having them in a list, you can say

moveTo(20, 10 , 90);
intake.spin(fwd, 90, volt);
moveTo(10,20,90);
othertask(otherparamaters);

This might allow you to have more control over actions taken. I would have an auton function personally. That might look like

Competition.autonomous(Yournameofautonfunction);

If you have any questions let me know! (Feel Free to ping as well)

If any of this information is wrong I blame being an IQ Kid

-Blaziumm

4

While this is much easier to read, this is harder to do in practice than you think. The problem arises because the robot bases its movement off of a path between points, and this path is often ahead of certain points where tasks need to be done.

Now, that isn’t to say this is impossible; I was able to get around this by giving the pure pursuit controller a vector populated with points, as well as the addresses of lambda functions (these functions perform the desired tasks). Depending on where in the vector you declare the lambda, dictates where in the path the function in concern is called:

Here is an example from spin up:

File file("data.txt");
bool not_at_end = true;

void AnotherTest(){

  file.content << "[";

  PIDTask = task(
    []()->int{
      while(not_at_end){
        wait(20, msec);
        file.content << "[" << X << ", " << Y << "], " << "\n";
      }
      return 0;
    }
  );

  ListOfLambdaWithPoint PurePursuitPathVector = {
    {{-48, 48}, NoLambda},
    {{48, -48},
      { [](){
        
      not_at_end = false;
      wait(0.025, sec);

      file.content << "]";
      file.write('w'); 

      RightDrive(stop();)
      LeftDrive(stop();)
      wait(100, sec);

    }, {2, 12}}},
  };
  
  Path PurePursuitPath(&PurePursuitPathVector);

  robot->SetPath(&PurePursuitPath);
  
  robot->PathGenLoop();
}

This particular autonomous was a way to test how much GPS flickers as I drive along a path, it would record points as it drove along the path, and once it reached the end, it would flush those points into a file on the SD card… we did this to decide if GPS or odometry would work better for us.

…anyway I am getting off topic, the point is, It is possible.
Yes, my approach is very hard to read sometimes, though, you could probably do some #define preprocessor wizardry stuff to make the syntax look more traditional.