diff --git a/build/src/robot-config.o b/build/src/robot-config.o new file mode 100644 index 00000000..a0f0c98a Binary files /dev/null and b/build/src/robot-config.o differ diff --git a/include/JAR-Template/drive.h b/include/JAR-Template/drive.h index 27a6f258..74fe1e8b 100644 --- a/include/JAR-Template/drive.h +++ b/include/JAR-Template/drive.h @@ -1,5 +1,6 @@ #pragma once #include "vex.h" +#include 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, float lookahead_distance); + void control_arcade(); void control_tank(); void control_holonomic(); diff --git a/include/JAR-Template/odom.h b/include/JAR-Template/odom.h index 3f9ef5c7..4dec7755 100644 --- a/include/JAR-Template/odom.h +++ b/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); }; \ No newline at end of file diff --git a/include/JAR-Template/util.h b/include/JAR-Template/util.h index 69e2b9ce..99d9e583 100644 --- a/include/JAR-Template/util.h +++ b/include/JAR-Template/util.h @@ -1,3 +1,12 @@ +#pragma once + +#include + +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); \ No newline at end of file +float deadband(float input, float width); + +float dist(Point p1, Point p2); + +template +constexpr T sign(T value) { + return value < 0 ? -1 : 1; +} + +std::vector line_circle_intersections(Point center, float radius, Point p1, Point p2); \ No newline at end of file diff --git a/src/JAR-Template/drive.cpp b/src/JAR-Template/drive.cpp index 0ad6972d..f2755e15 100644 --- a/src/JAR-Template/drive.cpp +++ b/src/JAR-Template/drive.cpp @@ -1,4 +1,5 @@ #include "vex.h" +#include 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 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 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_errorSidewaysTracker_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; } \ No newline at end of file diff --git a/src/JAR-Template/util.cpp b/src/JAR-Template/util.cpp index ed682fce..b6eece0f 100644 --- a/src/JAR-Template/util.cpp +++ b/src/JAR-Template/util.cpp @@ -1,4 +1,6 @@ #include "vex.h" +#include +#include // 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 line_circle_intersections(Point center, float radius, Point p1, Point p2) { + std::vector 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; } \ No newline at end of file