From 72db41419712c173c7d622fa65b55d9d4a8285e8 Mon Sep 17 00:00:00 2001 From: sdekhterman Date: Fri, 1 Nov 2024 14:22:09 -0500 Subject: [PATCH] reads in sim value appropriately --- cpp_pid/plot.py | 53 ++++++++++--------------------------- cpp_pid/src/environment.cpp | 13 +++------ cpp_pid/src/main.cpp | 10 +++---- cpp_pid/src/server.cpp | 5 +--- dbw/dbw/dbw_vel_sim.py | 2 +- 5 files changed, 25 insertions(+), 58 deletions(-) diff --git a/cpp_pid/plot.py b/cpp_pid/plot.py index c9bbac4..a12004d 100644 --- a/cpp_pid/plot.py +++ b/cpp_pid/plot.py @@ -2,15 +2,14 @@ import matplotlib.pyplot as plt import pandas as pd -filename = '/home/auvsl/ros2_ws/2024-10-09_19-26-50' -cols = ['x','y', 'theta', 'input1', 'input2', 'input3', 'input4', 'linear', 'angular'] +filename = '/home/auvsl/ros2_ws/2024-11-01_14-04-31' +cols = ['x','y', 'theta', 'input', 'output'] data = pd.read_csv(filename,names=cols,header=None) plt.figure() -plt.plot(np.array([-9.60897, -0.608969, 8.39103, 15.391]),np.array([-2.95891, -2.95891, -2.95891, -2.95891]), 'o') plt.plot(data['x'].values,data['y'].values) -plt.xlim(-50,50) -plt.ylim(-50,50) +plt.xlim(0,200) +plt.ylim(-100,0) plt.xlabel('X Position (m)') plt.ylabel('Y Position (m)') plt.title('XY Position of Rgator When Manually Driving') @@ -21,40 +20,16 @@ plt.xlabel('Iterations') plt.ylabel('VectorNav Yaw (rad)') -# plt.figure() -# plt.plot(data['input1'].values) -# plt.title('Distance Line over Iterations') -# plt.xlabel('Iterations') -# plt.ylabel('Distance Line (m)') - -# plt.figure() -# plt.plot(data['input2'].values) -# plt.title('Theta Near over Iterations') -# plt.xlabel('Iterations') -# plt.ylabel('Theta Near (rad)') - -# plt.figure() -# plt.plot(data['input3'].values) -# plt.title('Theta Far over Iterations') -# plt.xlabel('Iterations') -# plt.ylabel('Theta Far (rad)') - -# plt.figure() -# plt.plot(data['input4'].values) -# plt.title('Distance Target over Iterations') -# plt.xlabel('Iterations') -# plt.ylabel('Distance Target (m)') - -# plt.figure() -# plt.plot(data['linear'].values) -# plt.title('Linear Speed over Iterations') -# plt.xlabel('Iterations') -# plt.ylabel('Linear Speed (m/s)') +plt.figure() +plt.plot(data['input'].values) +plt.title('Input over Iterations') +plt.xlabel('Iterations') +plt.ylabel('Distance Line (m)') -# plt.figure() -# plt.plot(data['angular'].values) -# plt.title('Agnular Velocity over Iterations') -# plt.xlabel('Iterations') -# plt.ylabel('Angular Velocity (rad/s)') +plt.figure() +plt.plot(data['output'].values) +plt.title('Angular Velocity over Iterations') +plt.xlabel('Iterations') +plt.ylabel('Angular Velocity (rad/s)') plt.show() \ No newline at end of file diff --git a/cpp_pid/src/environment.cpp b/cpp_pid/src/environment.cpp index 11f0285..26953f5 100644 --- a/cpp_pid/src/environment.cpp +++ b/cpp_pid/src/environment.cpp @@ -140,19 +140,14 @@ namespace auvsl // generate inputs to Environment based on the vehicle's postion relative to relavent waypoints double Environment::controllerInput(Eigen::VectorXd odomPos, Eigen::VectorXd waypoints){ // load in the current position, the last waypoint passed, and the current waypoint being targeted, and the next waypoint to be targeted - Eigen::Vector2d currentPos{ odomPos(1), odomPos(2)}; + double currentAng = Environment::angSat(odomPos(0)); Eigen::Vector2d past{ waypoints(0), waypoints(1)}; Eigen::Vector2d present{waypoints(2), waypoints(3)}; - Eigen::Vector2d future{ waypoints(4), waypoints(5)}; - // the 'a' and 'b' vectors are used to compute the scale vector rejection, perpendicular distance, from the current path segement - Eigen::Vector2d a = currentPos - past; - Eigen::Vector2d b = present - past; + double th1 = std::atan2( present(1) - past(1), present(0) - past(0)); - Eigen::Vector2d pre_norm(-b(1), b(0)); - Eigen::Vector2d unit_vector = pre_norm / pre_norm.norm(); - - double error = -a.dot(unit_vector); // distanceLine (the negative is due to how the membership functions interprests what is left/ right for this input) + // determine the vehicle's orientation difference relative the the current path segement and next path segment + double error = Environment::angSat(currentAng-th1); return error; } diff --git a/cpp_pid/src/main.cpp b/cpp_pid/src/main.cpp index 35c32f1..3496366 100644 --- a/cpp_pid/src/main.cpp +++ b/cpp_pid/src/main.cpp @@ -33,7 +33,7 @@ std::string getCurrentDateTime() { template void pushBack(T &mtrx, const double &x_position, const double &y_position, const double &theta, const double &input, const double &error) { auto n = mtrx.rows(); - mtrx.conservativeResize(n + 1, 9); + mtrx.conservativeResize(n + 1, 5); mtrx(n, 0) = x_position; mtrx(n, 1) = y_position; mtrx(n, 2) = theta; @@ -49,7 +49,7 @@ void writeToCSVfile(std::string name, Eigen::MatrixXd matrix) int main(int argc, char **argv) { - double T = 50, dt = 1/T, max = 70, min = -70, Kp = 10, Ki = 2, Kd = 5; + double T = 50, dt = 1/T, max = 70, min = -70, Kp = 100, Ki = 0, Kd = 0; //make a server object that callsback odomentry information, an object to analysis the relevant waypoints, if the vehcile should stop, path errors fed into the controller, and a controller object auvsl::Server* srvrObj = new auvsl::Server; @@ -66,9 +66,9 @@ int main(int argc, char **argv) auto odomSub = node->create_subscription("/vectornav/raw/common", 1, std::bind(&auvsl::Server::odomCallback, srvrObj, std::placeholders::_1)); // set the desired path - Eigen::MatrixXd path {{0,0}, {1, 0}, {2, 0}, {3, 0}}; + // Eigen::MatrixXd path {{0,0}, {10, 0}, {20, 0}, {30, 0}}; // Eigen::MatrixXd path {{0,0}, {1, 1}, {2, 2}, {3, 3}}; - //Eigen::MatrixXd path {{0,0}, {1, -1}, {2, -2}, {3, -3}}; + Eigen::MatrixXd path {{0,0}, {1, -1}, {2, -2}, {3, -3}}; Eigen::MatrixXd dataXYWypts; @@ -93,7 +93,7 @@ int main(int argc, char **argv) msg2.parkbrake = 0; // parking break false msg2.gear = 1; // forward gear - msg2.throttle = 50; // full throttle is 100 + msg2.throttle = 10; // full throttle is 100 msg2.steering = output; // full steering is 100 pushBack(dataXYWypts, srvrObj->odomPos(1), srvrObj->odomPos(2), srvrObj->odomPos(0), input, output); diff --git a/cpp_pid/src/server.cpp b/cpp_pid/src/server.cpp index 3647215..d8f0ba9 100644 --- a/cpp_pid/src/server.cpp +++ b/cpp_pid/src/server.cpp @@ -21,7 +21,7 @@ namespace auvsl // store the global yaw angle, x position, and y position off the vehicle void Server::odomCallback(const vectornav_msgs::msg::CommonGroup::SharedPtr msg1) { - odomPos(0) = -(msg1->yawpitchroll.x) * M_PI/180.0; // convert degrees to radians + odomPos(0) = M_PI/2 - (msg1->yawpitchroll.x) * M_PI/180.0; // convert degrees to radians double lat = (msg1->position.x) * M_PI/180.0; double lon = (msg1->position.y) * M_PI/180.0; double alt = msg1->position.z; @@ -34,8 +34,5 @@ namespace auvsl // compute the north east down position in meters from the latitude, longitude, and altitude in degrees cppmap3d::geodetic2enu(lat, lon, alt, lat0, lon0, alt0, odomPos(1), odomPos(2), throwAway); - - odomPos(0) -= 90; - } } \ No newline at end of file diff --git a/dbw/dbw/dbw_vel_sim.py b/dbw/dbw/dbw_vel_sim.py index a46ee59..b6185bf 100644 --- a/dbw/dbw/dbw_vel_sim.py +++ b/dbw/dbw/dbw_vel_sim.py @@ -15,7 +15,7 @@ def __init__(self): self.dbw_sub = self.create_subscription(Dbw, '/control_cmd', self.dbw_callback, 10) # init publisher - self.vcu_pub = self.create_publisher(UInt8MultiArray, '/can_bus_propulsion', 10) + self.vcu_pub = self.create_publisher(UInt8MultiArray, '/can_bus/propulsion', 10) # init timer self.timer = self.create_timer(0.1, self.timer_callback)