[UNTESTED, DO NOT MERGE YET] Path Follower

include/JAR-Template/drive.h

@@ -1,5 +1,6 @@
 #pragma once
 #include "vex.h"
+#include <vector>
 
 enum drive_setup {ZERO_TRACKER_NO_ODOM, ZERO_TRACKER_ODOM, TANK_ONE_ENCODER, TANK_ONE_ROTATION, TANK_TWO_ENCODER, TANK_TWO_ROTATION, HOLONOMIC_TWO_ENCODER, HOLONOMIC_TWO_ROTATION};
 
@@ -115,6 +116,7 @@ class Drive
   vex::task odom_task;
   float get_X_position();
   float get_Y_position();
+  Point get_position();
 
   void drive_to_point(float X_position, float Y_position);
   void drive_to_point(float X_position, float Y_position, float drive_max_voltage, float heading_max_voltage);
@@ -132,6 +134,8 @@ class Drive
   void holonomic_drive_to_point(float X_position, float Y_position, float angle, float drive_max_voltage, float heading_max_voltage, float drive_settle_error, float drive_settle_time, float drive_timeout);
   void holonomic_drive_to_point(float X_position, float Y_position, float angle, float drive_max_voltage, float heading_max_voltage, float drive_settle_error, float drive_settle_time, float drive_timeout, float drive_kp, float drive_ki, float drive_kd, float drive_starti, float heading_kp, float heading_ki, float heading_kd, float heading_starti);
 
+  void follow_path(std::vector<Point>, float lookahead_distance);
+
   void control_arcade();
   void control_tank();
   void control_holonomic();

include/JAR-Template/odom.h

@@ -1,3 +1,7 @@
+#pragma once
+
+#include "util.h"
+
 class Odom
 {
 private:
@@ -6,10 +10,9 @@ class Odom
   float ForwardTracker_position;
   float SideWaysTracker_position;
 public:
-  float X_position;
-  float Y_position;
+  Point position;
   float orientation_deg;
-  void set_position(float X_position, float Y_position, float orientation_deg, float ForwardTracker_position, float SidewaysTracker_position);
+  void set_position(Point position, float orientation_deg, float ForwardTracker_position, float SidewaysTracker_position);
   void update_position(float ForwardTracker_position, float SidewaysTracker_position, float orientation_deg);
   void set_physical_distances(float ForwardTracker_center_distance, float SidewaysTracker_center_distance);
 };

include/JAR-Template/util.h

@@ -1,3 +1,12 @@
+#pragma once
+
+#include <vector>
+
+typedef struct {
+  double x;
+  double y;
+} Point;
+
 float reduce_0_to_360(float angle);
 
 float reduce_negative_180_to_180(float angle);
@@ -16,4 +25,13 @@ float to_volt(float percent);
 
 int to_port(int port);
 
-float deadband(float input, float width);
+float deadband(float input, float width);
+
+float dist(Point p1, Point p2);
+
+template <typename T>
+constexpr T sign(T value) {
+	return value < 0 ? -1 : 1;
+}
+
+std::vector<Point> line_circle_intersections(Point center, float radius, Point p1, Point p2);

src/JAR-Template/drive.cpp

@@ -1,4 +1,5 @@
 #include "vex.h"
+#include <vector>
 
 Drive::Drive(enum::drive_setup drive_setup, motor_group DriveL, motor_group DriveR, int gyro_port, float wheel_diameter, float wheel_ratio, float gyro_scale, int DriveLF_port, int DriveRF_port, int DriveLB_port, int DriveRB_port, int ForwardTracker_port, float ForwardTracker_diameter, float ForwardTracker_center_distance, int SidewaysTracker_port, float SidewaysTracker_diameter, float SidewaysTracker_center_distance) :
   wheel_diameter(wheel_diameter),
@@ -263,19 +264,24 @@ void Drive::set_heading(float orientation_deg){
 }
 
 void Drive::set_coordinates(float X_position, float Y_position, float orientation_deg){
-  odom.set_position(X_position, Y_position, orientation_deg, get_ForwardTracker_position(), get_SidewaysTracker_position());
+  odom.set_position(Point {X_position, Y_position}, orientation_deg, get_ForwardTracker_position(), get_SidewaysTracker_position());
   set_heading(orientation_deg);
   odom_task = task(position_track_task);
 }
 
+Point Drive::get_position() {
+  return odom.position;
+}
+
 float Drive::get_X_position(){
-  return(odom.X_position);
+  return odom.position.x;
 }
 
 float Drive::get_Y_position(){
-  return(odom.Y_position);
+  return odom.position.y;
 }
 
+
 void Drive::drive_to_point(float X_position, float Y_position){
   drive_to_point(X_position, Y_position, drive_max_voltage, heading_max_voltage, drive_settle_error, drive_settle_time, drive_timeout, drive_kp, drive_ki, drive_kd, drive_starti, heading_kp, heading_ki, heading_kd, heading_starti);
 }
@@ -322,6 +328,64 @@ void Drive::drive_to_point(float X_position, float Y_position, float drive_max_v
   DriveR.stop(hold);
 }
 
+void Drive::follow_path(std::vector<Point> path, float lookahead_distance){
+  PID drivePID(0, drive_kp, drive_ki, drive_kd, drive_starti, drive_settle_error, drive_settle_time, drive_timeout);
+  PID headingPID(0, heading_kp, heading_ki, heading_kd, heading_starti);
+
+	// Add current position to the start of the path so that intersections can be found initially, even if the robot is off the path.
+  path.insert(path.begin(), odom.position);
+
+  Point target_intersection = odom.position; // The point on the path that we should target with PID.
+
+	// Loop through all waypoints in the provided path.
+  for (int i = 0; i < (path.size() - 1); i++) {
+    Point start = path[i]; // The current waypoint
+    Point end = path[i+1]; // The next waypoint
+
+    while (dist(odom.position, end) > lookahead_distance) {
+      // Find the point(s) of intersection between a circle centered around our global position with the radius of our
+			// lookahead distance and a line segment formed between our starting/ending points.
+      // This can be 0-2 points depending on whether there are tangent, secant, or no intersections.
+      std::vector<Point> intersections = line_circle_intersections(odom.position, lookahead_distance, start, end);
+
+			// Choose the best intersection to go to, ensuring that we don't go backwards along the path.
+      if (intersections.size() == 2) {
+				// There are two intersections between our lookahead circle and the path. Find the one closest to the end of the line segment.
+        if (dist(intersections[0], end) < dist(intersections[1], end)) {
+					target_intersection = intersections[0];
+				} else {
+					target_intersection = intersections[1];
+				}
+      } else if (intersections.size() == 1) {
+        // There is one intersection. Go to that intersection.
+				target_intersection = intersections[0];
+      }
+
+      // Move towards the target intersection with PID
+      float drive_error = dist(odom.position, target_intersection);
+      float heading_error = reduce_negative_180_to_180(to_deg(atan2(target_intersection.x-odom.position.x,target_intersection.y-odom.position.y))-get_absolute_heading());
+      float drive_output = drivePID.compute(drive_error);
+
+      float heading_scale_factor = cos(to_rad(heading_error));
+      drive_output*=heading_scale_factor;
+      heading_error = reduce_negative_90_to_90(heading_error);
+      float heading_output = headingPID.compute(heading_error);
+
+      if (drive_error<drive_settle_error) { heading_output = 0; }
+
+      drive_output = clamp(drive_output, -fabs(heading_scale_factor)*drive_max_voltage, fabs(heading_scale_factor)*drive_max_voltage);
+      heading_output = clamp(heading_output, -heading_max_voltage, heading_max_voltage);
+
+      drive_with_voltage(drive_output+heading_output, drive_output-heading_output);
+      task::sleep(10);
+    }
+  }
+
+  desired_heading = get_absolute_heading();
+  DriveL.stop(hold);
+  DriveR.stop(hold);
+}
+
 void Drive::turn_to_point(float X_position, float Y_position){
   turn_to_point(X_position, Y_position, 0, turn_max_voltage, turn_settle_error, turn_settle_time, turn_timeout, turn_kp, turn_ki, turn_kd, turn_starti);
 }

src/JAR-Template/odom.cpp

@@ -5,11 +5,10 @@ void Odom::set_physical_distances(float ForwardTracker_center_distance, float Si
   this->SidewaysTracker_center_distance = SidewaysTracker_center_distance;
 }
 
-void Odom::set_position(float X_position, float Y_position, float orientation_deg, float ForwardTracker_position, float SidewaysTracker_position){
+void Odom::set_position(Point position, float orientation_deg, float ForwardTracker_position, float SidewaysTracker_position){
   this->ForwardTracker_position = ForwardTracker_position;
   this->SideWaysTracker_position = SidewaysTracker_position;
-  this->X_position = X_position;
-  this->Y_position = Y_position;
+  this->position = position;
   this->orientation_deg = orientation_deg;
 }
 
@@ -54,6 +53,6 @@ void Odom::update_position(float ForwardTracker_position, float SidewaysTracker_
   float X_position_delta = local_polar_length*cos(global_polar_angle); 
   float Y_position_delta = local_polar_length*sin(global_polar_angle);
 
-  X_position+=X_position_delta;
-  Y_position+=Y_position_delta;
+  position.x += X_position_delta;
+  position.y += Y_position_delta;
 }

src/JAR-Template/util.cpp

@@ -1,4 +1,6 @@
 #include "vex.h"
+#include <cmath>
+#include <vector>
 
 // There is probably a more efficient way to reduce angles than the ones specified below,
 // but these work without question, and you really only have to reduce them once or twice at 
@@ -63,4 +65,60 @@ float deadband(float input, float width){
     return(0);
   }
   return(input);
+}
+
+float dist(Point p1, Point p2) {
+  return std::hypot(p2.x - p1.x, p2.y - p1.y);
+}
+
+std::vector<Point> line_circle_intersections(Point center, float radius, Point p1, Point p2) {
+  std::vector<Point> intersections = {};
+
+	// Subtract the circle's center to offset the system to origin.
+	Point offset_1 = Point {p1.x - center.x, p1.y - center.y};
+	Point offset_2 = Point {p2.x - center.x, p2.y - center.y};
+
+	double dx = offset_2.x - offset_1.x;
+	double dy = offset_2.y - offset_1.y;
+	double dr = dist(offset_1, offset_2);
+	double D = (offset_1.x * offset_2.y) - (offset_1.y * offset_2.x); // Cross product of offset 1 and 2
+	double discriminant = std::pow(radius, 2) * std::pow(dr, 2) - std::pow(D, 2);
+
+	// If our discriminant is greater than or equal to 0, the line formed as a slope of
+	// point_1 and point_2 intersects the circle at least once.
+	if (discriminant >= 0) {
+		// https://mathworld.wolfram.com/Circle-LineIntersection.html
+		Point solution_1 = Point {
+			(D * dy + sign(dy) * dx * std::sqrt(discriminant)) / std::pow(dr, 2) + center.x,
+			(-D * dx + fabs(dy) * std::sqrt(discriminant)) / std::pow(dr, 2) + center.y
+    };
+		Point solution_2 = Point {
+			(D * dy - sign(dy) * dx * std::sqrt(discriminant)) / std::pow(dr, 2) + center.x,
+			(-D * dx - fabs(dy) * std::sqrt(discriminant)) / std::pow(dr, 2) + center.y
+    };
+
+
+		// Find the bounded intersections.
+		// solution_1 and solution_2 are assumed to be true when the line formed as a slope between point_1 and point_2
+		// extends infinitely, however we only want to consider intersections that are part of a line segment *between*
+		// point_1 and point_2.
+
+    // Find the minimum coordinates for each line (p1 and p2 being the start and end of the segment)
+		double min_x = std::min(p1.x, p2.x);
+		double max_x = std::max(p1.x, p2.x);
+		double min_y = std::min(p1.y, p2.y);
+		double max_y = std::max(p1.y, p2.y);
+
+		// Solution 1 intersects the circle within the bounds of point_1 and point_2
+		if ((solution_1.x >= min_x && solution_1.x <= max_x) && (solution_1.y >= min_y && solution_1.y <= max_y)) {
+			intersections.push_back(solution_1);
+		}
+
+		// Solution 2 intersects the circle within the bounds of point_1 and point_2
+		if ((solution_2.x >= min_x && solution_2.x <= max_x) && (solution_2.y >= min_y && solution_2.y <= max_y)) {
+			intersections.push_back(solution_2);
+		}
+	}
+
+	return intersections;
 }