├── CODEOWNERS ├── img.jpg ├── img1.jpg ├── CMakeLists.txt ├── src ├── PID.h ├── PID.cpp └── main.cpp ├── LICENSE ├── cmakepatch.txt └── README.md /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @domluna 2 | -------------------------------------------------------------------------------- /img.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/A7med01/CarND-PID-Controller/master/img.jpg -------------------------------------------------------------------------------- /img1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/A7med01/CarND-PID-Controller/master/img1.jpg -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(PID) 2 | 3 | cmake_minimum_required (VERSION 3.5) 4 | 5 | add_definitions(-std=c++11) 6 | 7 | set(CXX_FLAGS "-Wall") 8 | set(CMAKE_CXX_FLAGS "${CXX_FLAGS}") 9 | 10 | set(sources src/PID.cpp src/main.cpp) 11 | 12 | include_directories(/usr/local/include) 13 | link_directories(/usr/local/lib) 14 | 15 | if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 16 | 17 | include_directories(/usr/local/opt/openssl/include) 18 | link_directories(/usr/local/opt/openssl/lib) 19 | link_directories(/usr/local/Cellar/libuv/1*/lib) 20 | 21 | endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 22 | 23 | 24 | add_executable(pid ${sources}) 25 | 26 | target_link_libraries(pid z ssl uv uWS) 27 | -------------------------------------------------------------------------------- /src/PID.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | class PID { 5 | public: 6 | /** 7 | * Constructor 8 | */ 9 | PID(); 10 | 11 | /** 12 | * Destructor. 13 | */ 14 | virtual ~PID(); 15 | 16 | /** 17 | * Initialize PID. 18 | * @param (Kp_, Ki_, Kd_) The initial PID coefficients 19 | */ 20 | void Init(double Kp_, double Ki_, double Kd_); 21 | 22 | /** 23 | * Update the PID error variables given cross track error. 24 | * @param cte The current cross track error 25 | */ 26 | void UpdateError(double cte); 27 | 28 | /** 29 | * Calculate the total PID error. 30 | * @output The total PID error 31 | */ 32 | double TotalError(); 33 | 34 | private: 35 | /** 36 | * PID Errors 37 | */ 38 | double p_error; 39 | double i_error; 40 | double d_error; 41 | double prev_cte; 42 | 43 | /** 44 | * PID Coefficients 45 | */ 46 | double Kp; 47 | double Ki; 48 | double Kd; 49 | 50 | 51 | }; 52 | 53 | #endif // PID_H -------------------------------------------------------------------------------- /src/PID.cpp: -------------------------------------------------------------------------------- 1 | #include "PID.h" 2 | 3 | /** 4 | * TODO: Complete the PID class. You may add any additional desired functions. 5 | */ 6 | 7 | PID::PID() {} 8 | 9 | PID::~PID() {} 10 | 11 | void PID::Init(double Kp_, double Ki_, double Kd_) { 12 | /** 13 | * TODO: Initialize PID coefficients (and errors, if needed) 14 | */ 15 | PID::Kp = Kp_; 16 | PID::Ki = Ki_; 17 | PID::Kd = Kd_; 18 | 19 | p_error = 0.0; 20 | i_error = 0.0; 21 | d_error = 0.0; 22 | 23 | // Previous cte 24 | prev_cte = 0.0; 25 | 26 | 27 | } 28 | 29 | void PID::UpdateError(double cte) { 30 | /** 31 | * TODO: Update PID errors based on cte. 32 | */ 33 | 34 | // Proportional error 35 | p_error = cte; 36 | 37 | // Integral error 38 | i_error += cte ; 39 | 40 | // Diferential error 41 | d_error = cte - prev_cte; 42 | prev_cte = cte; 43 | 44 | } 45 | 46 | double PID::TotalError() { 47 | /** 48 | * TODO: Calculate and return the total error 49 | */ 50 | return -p_error * Kp - i_error * Ki - d_error * Kd; 51 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017-2018 Udacity, Inc. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /cmakepatch.txt: -------------------------------------------------------------------------------- 1 | --- CMakeLists.txt 2017-03-23 20:58:47.000000000 +0900 2 | +++ CMakeListsnew.txt 2017-03-23 20:57:33.000000000 +0900 3 | @@ -32,10 +32,16 @@ 4 | target_link_libraries (uWS LINK_PUBLIC ${OPENSSL_CRYPTO_LIBRARY}) 5 | target_link_libraries (uWS LINK_PUBLIC ${ZLIB_LIBRARY}) 6 | 7 | -if (UNIX) 8 | +if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") 9 | target_link_libraries (uWS LINK_PUBLIC pthread) 10 | install (TARGETS uWS DESTINATION /usr/lib64) 11 | install (FILES src/Extensions.h src/WebSocketProtocol.h src/Networking.h src/WebSocket.h src/Hub.h src/Group.h src/Node.h src/Socket.h src/HTTPSocket.h src/uWS.h src/uUV.h DESTINATION /usr/include/uWS) 12 | -endif (UNIX) 13 | +endif(${CMAKE_SYSTEM_NAME} MATCHES "Linux") 14 | + 15 | +if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 16 | +target_link_libraries (uWS LINK_PUBLIC pthread) 17 | +install (TARGETS uWS DESTINATION /usr/local/lib) 18 | +install (FILES src/Extensions.h src/WebSocketProtocol.h src/Networking.h src/WebSocket.h src/Hub.h src/Group.h src/Node.h src/Socket.h src/HTTPSocket.h src/uWS.h src/uUV.h DESTINATION /usr/local/include/uWS) 19 | +endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 20 | 21 | add_subdirectory(examples) 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # **PID Controller Project** 2 | [![Udacity - Self-Driving Car NanoDegree](https://s3.amazonaws.com/udacity-sdc/github/shield-carnd.svg)](http://www.udacity.com/drive) 3 | 4 | ![IMG](./img.jpg) 5 | 6 | --- 7 | 8 | ## PID Controller: 9 | The P, or "proportional", component had the most directly observable effect on the car's behavior. It causes the car to steer proportional (and opposite) to the car's distance from the lane center (which is the CTE) - if the car is far to the right it steers hard to the left, if it's slightly to the left it steers slightly to the right. 10 | 11 | The D, or "differential", component counteracts the P component's tendency to ring and overshoot the center line. A properly tuned D parameter will cause the car to approach the center line smoothly without ringing. 12 | 13 | The I, or "integral", component counteracts a bias in the CTE which prevents the P-D controller from reaching the center line. This bias can take several forms, such as a steering drift (as in the Control unit lessons). 14 | 15 | ![IMG1](./img1.jpg) 16 | 17 | 18 | #### hyperparameters: 19 | The final hyperparameters (P, I, D coefficients) were chosen by manual tuning according to the values used in the lesson. 20 | 21 | --- 22 | 23 | ## Dependencies 24 | 25 | * cmake >= 3.5 26 | * All OSes: [click here for installation instructions](https://cmake.org/install/) 27 | * make >= 4.1(mac, linux), 3.81(Windows) 28 | * Linux: make is installed by default on most Linux distros 29 | * Mac: [install Xcode command line tools to get make](https://developer.apple.com/xcode/features/) 30 | * Windows: [Click here for installation instructions](http://gnuwin32.sourceforge.net/packages/make.htm) 31 | * gcc/g++ >= 5.4 32 | * Linux: gcc / g++ is installed by default on most Linux distros 33 | * Mac: same deal as make - [install Xcode command line tools]((https://developer.apple.com/xcode/features/) 34 | * Windows: recommend using [MinGW](http://www.mingw.org/) 35 | * [uWebSockets](https://github.com/uWebSockets/uWebSockets) 36 | * Run either `./install-mac.sh` or `./install-ubuntu.sh`. 37 | * If you install from source, checkout to commit `e94b6e1`, i.e. 38 | ``` 39 | git clone https://github.com/uWebSockets/uWebSockets 40 | cd uWebSockets 41 | git checkout e94b6e1 42 | ``` 43 | Some function signatures have changed in v0.14.x. See [this PR](https://github.com/udacity/CarND-MPC-Project/pull/3) for more details. 44 | * Simulator. You can download these from the [project intro page](https://github.com/udacity/self-driving-car-sim/releases) in the classroom. 45 | 46 | Fellow students have put together a guide to Windows set-up for the project [here](https://s3-us-west-1.amazonaws.com/udacity-selfdrivingcar/files/Kidnapped_Vehicle_Windows_Setup.pdf) if the environment you have set up for the Sensor Fusion projects does not work for this project. There's also an experimental patch for windows in this [PR](https://github.com/udacity/CarND-PID-Control-Project/pull/3). 47 | 48 | ## Basic Build Instructions 49 | 50 | 1. Clone this repo. 51 | 2. Make a build directory: `mkdir build && cd build` 52 | 3. Compile: `cmake .. && make` 53 | 4. Run it: `./pid`. 54 | 55 | Tips for setting up your environment can be found [here](https://classroom.udacity.com/nanodegrees/nd013/parts/40f38239-66b6-46ec-ae68-03afd8a601c8/modules/0949fca6-b379-42af-a919-ee50aa304e6a/lessons/f758c44c-5e40-4e01-93b5-1a82aa4e044f/concepts/23d376c7-0195-4276-bdf0-e02f1f3c665d) 56 | 57 | ## Editor Settings 58 | 59 | We've purposefully kept editor configuration files out of this repo in order to 60 | keep it as simple and environment agnostic as possible. However, we recommend 61 | using the following settings: 62 | 63 | * indent using spaces 64 | * set tab width to 2 spaces (keeps the matrices in source code aligned) 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "json.hpp" 6 | #include "PID.h" 7 | 8 | // for convenience 9 | using nlohmann::json; 10 | using std::string; 11 | 12 | // For converting back and forth between radians and degrees. 13 | constexpr double pi() { return M_PI; } 14 | double deg2rad(double x) { return x * pi() / 180; } 15 | double rad2deg(double x) { return x * 180 / pi(); } 16 | 17 | // Checks if the SocketIO event has JSON data. 18 | // If there is data the JSON object in string format will be returned, 19 | // else the empty string "" will be returned. 20 | string hasData(string s) { 21 | auto found_null = s.find("null"); 22 | auto b1 = s.find_first_of("["); 23 | auto b2 = s.find_last_of("]"); 24 | if (found_null != string::npos) { 25 | return ""; 26 | } 27 | else if (b1 != string::npos && b2 != string::npos) { 28 | return s.substr(b1, b2 - b1 + 1); 29 | } 30 | return ""; 31 | } 32 | 33 | int main() { 34 | uWS::Hub h; 35 | 36 | PID pid; 37 | /** 38 | * TODO: Initialize the pid variable. 39 | */ 40 | pid.Init(0.2, 0.0, 3); 41 | 42 | h.onMessage([&pid](uWS::WebSocket ws, char *data, size_t length, 43 | uWS::OpCode opCode) { 44 | // "42" at the start of the message means there's a websocket message event. 45 | // The 4 signifies a websocket message 46 | // The 2 signifies a websocket event 47 | if (length && length > 2 && data[0] == '4' && data[1] == '2') { 48 | auto s = hasData(string(data).substr(0, length)); 49 | 50 | if (s != "") { 51 | auto j = json::parse(s); 52 | 53 | string event = j[0].get(); 54 | 55 | if (event == "telemetry") { 56 | // j[1] is the data JSON object 57 | double cte = std::stod(j[1]["cte"].get()); 58 | double speed = std::stod(j[1]["speed"].get()); 59 | double angle = std::stod(j[1]["steering_angle"].get()); 60 | double steer_value = 0.0; 61 | /** 62 | * TODO: Calculate steering value here, remember the steering value is 63 | * [-1, 1]. 64 | * NOTE: Feel free to play around with the throttle and speed. 65 | * Maybe use another PID controller to control the speed! 66 | */ 67 | 68 | pid.UpdateError(cte); 69 | steer_value = pid.TotalError(); 70 | 71 | // DEBUG 72 | std::cout << "CTE: " << cte << " Steering Value: " << steer_value 73 | << std::endl; 74 | 75 | json msgJson; 76 | msgJson["steering_angle"] = steer_value; 77 | msgJson["throttle"] = 0.3; 78 | auto msg = "42[\"steer\"," + msgJson.dump() + "]"; 79 | std::cout << msg << std::endl; 80 | ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT); 81 | } // end "telemetry" if 82 | } else { 83 | // Manual driving 84 | string msg = "42[\"manual\",{}]"; 85 | ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT); 86 | } 87 | } // end websocket message if 88 | }); // end h.onMessage 89 | 90 | h.onConnection([&h](uWS::WebSocket ws, uWS::HttpRequest req) { 91 | std::cout << "Connected!!!" << std::endl; 92 | }); 93 | 94 | h.onDisconnection([&h](uWS::WebSocket ws, int code, 95 | char *message, size_t length) { 96 | ws.close(); 97 | std::cout << "Disconnected" << std::endl; 98 | }); 99 | 100 | int port = 4567; 101 | if (h.listen(port)) { 102 | std::cout << "Listening to port " << port << std::endl; 103 | } else { 104 | std::cerr << "Failed to listen to port" << std::endl; 105 | return -1; 106 | } 107 | 108 | h.run(); 109 | } --------------------------------------------------------------------------------