实现PID控制小车前进到指定位置
%% START CODE BLOCK %%
% 1. Calculate the heading (angle) to the goal.
% distance between goal and robot in x-direction
u_x = x_g-x;
% distance between goal and robot in y-direction
u_y = y_g-y;
% angle from robot to goal. Hint: use ATAN2, u_x, u_y here.
theta_g = atan2(u_y,u_x);
% 2. Calculate the heading error.
% error between the goal angle and robot's angle
% Hint: Use ATAN2 to make sure this stays in [-pi,pi].
e_k = atan2(sin(theta_g-theta),cos(theta_g-theta));
% 3. Calculate PID for the steering angle
% error for the proportional term
e_P = e_k;
% error for the integral term. Hint: Approximate the integral using
% the accumulated error, obj.E_k, and the error for
% this time step, e_k.
e_I = obj.E_k + e_k*dt;
% error for the derivative term. Hint: Approximate the derivative
% using the previous error, obj.e_k_1, and the
% error for this time step, e_k.
e_D = (e_k-obj.e_k_1)/dt;
%% END CODE BLOCK %%
最后运行