├── CMakeLists.txt ├── README.md ├── _config.yml ├── build.sh ├── clean.sh ├── data ├── speedy.gif └── steady.gif ├── docs └── pid_control_document.pdf ├── libs ├── install-mac.sh └── install-ubuntu.sh ├── run.sh └── src ├── PID.cpp ├── PID.h ├── json.hpp └── main.cpp /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.11.0/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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # **Autonomous driving with PID controllers** 2 | 3 | ### 1. Objective 4 | This project is to use PID controllers to control the steering angle and the throttle for driving a car 5 | in a game simulator. The simulator provides cross-track error (CTE) via websocket. The PID 6 | (proportional-integral-differential) controllers give steering and throttle commands 7 | to drive the car reliably around the simulator track. 8 | 9 | 10 | #### Demo 1: Steady drive with constant throttle (click to see the full video) 11 | 12 | [![demo_gif1][gif1]](https://youtu.be/bI5RTZcRMAo) 13 | 14 | 15 | ### 2. Parameters of PID controller 16 | 17 | **Description of PID values in PID control** 18 | 19 | * **P** (proportional) accounts for present values of the error. For example, if the error is large and positive, 20 | the control output will also be large and positive. 21 | 22 | * **I** (integral) accounts for all past values of the error. For example, if the current output is not sufficiently 23 | strong, the integral of the error will accumulate over time, and the controller will respond by 24 | applying a stronger action. 25 | 26 | * **D** (differential) accounts for possible future trends of the error, based on its current rate of change. 27 | 28 | **Finally parameters** 29 | 30 | * PID parameters used for **steering angles**: 31 | 32 | * p value: 0.1 33 | * i value: 0.001 34 | * d value: 2.8 35 | 36 | * PID parameters used for **throttle**: 37 | 38 | * p value: 0.45 39 | * i value: 0.000 40 | * d value: 0.5 41 | 42 | **How to tune the parameters** 43 | 44 | The parameters are tuned manually with the order of: p, d, i. The d and i are first setted to be zeros, 45 | and 0.2 is used for the p value. I adjust the p value up and down till it could drive around the first 46 | corner and hard to imporve more. Then I keep the p value as it is, and increase the d value. Use the 47 | same approach for d value and i value. 48 | 49 | In order to automatically fine tune the parameters, an optimization algorithm **twiddle** can be used, which is exampled as follows: 50 | 51 | * [Twiddle algorithm](https://www.youtube.com/watch?v=2uQ2BSzDvXs) 52 | 53 | * [Twiddle sample code in python](https://martin-thoma.com/twiddle/) 54 | 55 | 56 | Demo 2: [speedy drive](https://youtu.be/E8uXIBIRg8M), which is targeting for dirivng the car as fast 57 | as possible, but as a side effect, the car starts to swing. In order to make the car drive 58 | fast as well as steady, further joint parameter tuning for both PID controllers need to be carried out. 59 | 60 | 61 | 62 | --- 63 | 64 | ## Code & Files 65 | ### 1. Dependencies & environment 66 | 67 | * cmake >= 3.5 68 | * make >= 4.1 69 | * gcc/g++ >= 5.4 70 | * uWebSockets: used for communication between the main code and the simulator. 71 | * [libs/install-mac](install-mac.sh) install uWebSockets in Mac. 72 | * [libs/install-ubuntu](install-ubuntu.sh) install uWebSockets in Ubuntu. 73 | 74 | 75 | ### 2. How to run the code 76 | 77 | 1. Clone this repo. 78 | 2. Clean the project: `$./clean.sh` 79 | 3. Build the project: `$./build.sh` 80 | 4. Run the project: `$./run.sh` 81 | 5. Start the [simulator v1.45](https://github.com/udacity/self-driving-car-sim/releases), 82 | select the PID Controller. 83 | 84 | 85 | ### 3. My project files 86 | 87 | * [CMakeLists.txt](CMakeLists.txt) is the cmake file. 88 | * [data](data) folder contains GIFs. 89 | * [src](src) folder contains the source code. 90 | * [clean.sh](clean.sh) cleans the project. 91 | * [build.sh](build.sh) builds the project. 92 | * [run.sh](run.sh) runs the project. 93 | * [libs](libs) scripts to install uWebSockets. 94 | 95 | 96 | 97 | ### 4. Code Style 98 | 99 | * [Google's C++ style guide](https://google.github.io/styleguide/cppguide.html). 100 | 101 | 102 | ### 5. Release History 103 | 104 | * 0.1.1 105 | * First proper release 106 | * Date 14 July 2017 107 | 108 | * 0.1.0 109 | * Create the repo 110 | * Date 13 July 2017 111 | 112 | --- 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | [//]: # (Image References) 123 | [image1]: ./data/1.png 124 | [gif1]: ./data/steady.gif 125 | [gif2]: ./data/speedy.gif 126 | 127 | -------------------------------------------------------------------------------- /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-minimal -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script to build all components from scratch, using the maximum available CPU power 3 | # 4 | # Given parameters are passed over to CMake. 5 | # Examples: 6 | # * ./build_all.sh -DCMAKE_BUILD_TYPE=Debug 7 | # * ./build_all.sh VERBOSE=1 8 | 9 | 10 | # Go into the directory where this bash script is contained. 11 | cd `dirname $0` 12 | 13 | # Compile code. 14 | mkdir -p build 15 | cd build 16 | cmake .. 17 | make -j `nproc` $* 18 | -------------------------------------------------------------------------------- /clean.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script to clean the tree from all compiled files. 3 | # You can rebuild them afterwards using "build.sh". 4 | 5 | # Remove the dedicated output directories 6 | cd `dirname $0` 7 | 8 | rm -rf build 9 | 10 | # We're done! 11 | echo Cleaned up the project! 12 | -------------------------------------------------------------------------------- /data/speedy.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JunshengFu/PID-controller/db47f63142436ea10cef348bfeee43ce5be690da/data/speedy.gif -------------------------------------------------------------------------------- /data/steady.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JunshengFu/PID-controller/db47f63142436ea10cef348bfeee43ce5be690da/data/steady.gif -------------------------------------------------------------------------------- /docs/pid_control_document.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JunshengFu/PID-controller/db47f63142436ea10cef348bfeee43ce5be690da/docs/pid_control_document.pdf -------------------------------------------------------------------------------- /libs/install-mac.sh: -------------------------------------------------------------------------------- 1 | brew install openssl libuv cmake 2 | git clone https://github.com/uWebSockets/uWebSockets 3 | cd uWebSockets 4 | git checkout e94b6e1 5 | patch CMakeLists.txt < ../cmakepatch.txt 6 | mkdir build 7 | export PKG_CONFIG_PATH=/usr/local/opt/openssl/lib/pkgconfig 8 | cd build 9 | cmake .. 10 | make 11 | sudo make install 12 | cd .. 13 | cd .. 14 | sudo rm -r uWebSockets 15 | -------------------------------------------------------------------------------- /libs/install-ubuntu.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | sudo apt-get update 3 | sudo apt-get install git libuv1-dev libssl-dev gcc g++ cmake make 4 | git clone https://github.com/uWebSockets/uWebSockets 5 | cd uWebSockets 6 | git checkout e94b6e1 7 | mkdir build 8 | cd build 9 | cmake .. 10 | make 11 | sudo make install 12 | cd .. 13 | cd .. 14 | sudo ln -s /usr/lib64/libuWS.so /usr/lib/libuWS.so 15 | sudo rm -r uWebSockets 16 | -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Script to run PID controller 3 | cd ./build 4 | ./pid 5 | -------------------------------------------------------------------------------- /src/PID.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PID.cpp 3 | * 4 | * Created on: 13 July, 2017 5 | * Author: Junsheng Fu 6 | */ 7 | 8 | #include 9 | #include "PID.h" 10 | 11 | using namespace std; 12 | 13 | /** 14 | * Constructor 15 | */ 16 | PID::PID() {} 17 | 18 | /** 19 | * Destructor. 20 | */ 21 | PID::~PID() {} 22 | 23 | 24 | /** 25 | * Initialize PID. 26 | * @param kp the proportional value for PID controller 27 | * @param ki the integral value for PID controller 28 | * @param kd the derivative value for PID controller 29 | */ 30 | void PID::Init(double kp, double ki, double kd) { 31 | Kp = kp; 32 | Ki = ki; 33 | Kd = kd; 34 | 35 | p_error = 0; 36 | i_error = 0; 37 | d_error = 0; 38 | } 39 | 40 | 41 | /** 42 | * Update the PID error variables given cross track error 43 | * @param cte the current cross track error 44 | */ 45 | void PID::UpdateError(double cte) { 46 | 47 | double pre_cte = p_error; 48 | 49 | p_error = cte; 50 | i_error += cte; 51 | d_error = cte - pre_cte; 52 | } 53 | 54 | 55 | /** 56 | * Compute the control command value according to PID controller 57 | * @return the steering angle 58 | */ 59 | double PID::OutputSteerAng() { 60 | 61 | return -Kp*p_error - Ki*i_error - Kd*d_error; 62 | } 63 | 64 | 65 | /** 66 | * Compute the throttle by PID controller 67 | * @param max_thro max throttle value 68 | * @return the computed throttle value 69 | */ 70 | double PID::OutputThrottle(double max_thro){ 71 | 72 | return max_thro - Kp*p_error - Ki*i_error - Kd*d_error; 73 | } -------------------------------------------------------------------------------- /src/PID.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PID.h 3 | * 4 | * Created on: 13 July, 2017 5 | * Author: Junsheng Fu 6 | */ 7 | 8 | #ifndef PID_H 9 | #define PID_H 10 | 11 | #include 12 | 13 | class PID { 14 | public: 15 | /** 16 | * Error terms for PID controller 17 | */ 18 | double p_error; 19 | double i_error; 20 | double d_error; 21 | 22 | /** 23 | * Coefficients 24 | */ 25 | double Kp; 26 | double Ki; 27 | double Kd; 28 | 29 | /** 30 | * Constructor 31 | */ 32 | PID(); 33 | 34 | /** 35 | * Destructor. 36 | */ 37 | virtual ~PID(); 38 | 39 | /** 40 | * Initialize PID. 41 | * @param kp the proportional value for PID controller 42 | * @param ki the integral value for PID controller 43 | * @param kd the derivative value for PID controller 44 | */ 45 | void Init(double kp, double ki, double kd); 46 | 47 | /** 48 | * Update the PID error variables given cross track error 49 | * @param cte the current cross track error 50 | */ 51 | void UpdateError(double cte); 52 | 53 | /** 54 | * Compute the control command value according to PID controller 55 | * @return the steering angle 56 | */ 57 | double OutputSteerAng(); 58 | 59 | /** 60 | * Compute the throttle by PID controller 61 | * @param max_thro max throttle value 62 | * @return the computed throttle value 63 | */ 64 | double OutputThrottle(double max_thro); 65 | }; 66 | 67 | #endif /* PID_H */ 68 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "json.hpp" 4 | #include "PID.h" 5 | #include 6 | 7 | #define PID_thro 0 // flag 8 | 9 | // for convenience 10 | using json = nlohmann::json; 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 | std::string hasData(std::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 != std::string::npos) { 25 | return ""; 26 | } 27 | else if (b1 != std::string::npos && b2 != std::string::npos) { 28 | return s.substr(b1, b2 - b1 + 1); 29 | } 30 | return ""; 31 | } 32 | 33 | 34 | // Reset the car back to starting position, and it can be used in twiddle 35 | void Restart(uWS::WebSocket ws){ 36 | std::string reset_msg = "42[\"reset\",{}]"; 37 | ws.send(reset_msg.data(), reset_msg.length(), uWS::OpCode::TEXT); 38 | } 39 | 40 | 41 | 42 | int main() 43 | { 44 | uWS::Hub h; 45 | 46 | // difine the PID controller for both steering angle, and throttle 47 | PID pid_steer; 48 | pid_steer.Init(0.1, 0.001, 2.8); 49 | 50 | PID pid_throttle; 51 | 52 | #if PID_thro 53 | pid_throttle.Init(0.45, 0.000, 0.5); 54 | #endif 55 | 56 | h.onMessage([&pid_steer, &pid_throttle](uWS::WebSocket ws, char *data, size_t length, uWS::OpCode opCode) { 57 | // "42" at the start of the message means there's a websocket message event. 58 | // The 4 signifies a websocket message 59 | // The 2 signifies a websocket event 60 | if (length && length > 2 && data[0] == '4' && data[1] == '2') 61 | { 62 | auto s = hasData(std::string(data).substr(0, length)); 63 | if (s != "") { 64 | auto j = json::parse(s); 65 | std::string event = j[0].get(); 66 | if (event == "telemetry") { 67 | // j[1] is the data JSON object 68 | double cte = std::stod(j[1]["cte"].get()); 69 | double speed = std::stod(j[1]["speed"].get()); 70 | double angle = std::stod(j[1]["steering_angle"].get()); 71 | double steer_value; 72 | double throttle = 0.4; 73 | 74 | // PID steering controller processing 75 | pid_steer.UpdateError(cte); 76 | steer_value = pid_steer.OutputSteerAng(); 77 | 78 | // PID throttle controller processing 79 | #if PID_thro 80 | double max_throttle = 0.8; 81 | pid_throttle.UpdateError(fabs(steer_value)); 82 | throttle = pid_throttle.OutputThrottle(max_throttle); 83 | #endif 84 | // Print the results 85 | std::cout << std::fixed; 86 | std::cout << std::setprecision(3); 87 | std::cout << "CTE: " << cte << "\t Steering: " << steer_value << "\t throttle: " << throttle <end(s.data(), s.length()); 110 | } 111 | else 112 | { 113 | // i guess this should be done more gracefully? 114 | res->end(nullptr, 0); 115 | } 116 | }); 117 | 118 | h.onConnection([&h](uWS::WebSocket ws, uWS::HttpRequest req) { 119 | std::cout << "Connected!!!" << std::endl; 120 | }); 121 | 122 | h.onDisconnection([&h](uWS::WebSocket ws, int code, char *message, size_t length) { 123 | ws.close(); 124 | std::cout << "Disconnected" << std::endl; 125 | }); 126 | 127 | int port = 4567; 128 | if (h.listen(port)) 129 | { 130 | std::cout << "Listening to port " << port << std::endl; 131 | } 132 | else 133 | { 134 | std::cerr << "Failed to listen to port" << std::endl; 135 | return -1; 136 | } 137 | h.run(); 138 | } 139 | --------------------------------------------------------------------------------