classdef GoToGoal < simiam.controller.Controller
%% GOTOGOAL steers the robot towards a goal with a constant velocity using PID
% execute - Computes the left and right wheel speeds for go-to-goal.
% Copyright (C) 2013, Georgia Tech Research Corporation
% see the LICENSE file included with this software
% memory banks
% plot support
inputs = struct('x_g', 0, 'y_g', 0, 'v', 0);
outputs = struct('v', 0, 'w', 0);
function obj = GoToGoal()
%% GOTOGOAL Constructor
obj = firstname.lastname@example.org.Controller('go_to_goal');
% initialize memory banks
obj.Kp = 5;
obj.Ki = 0.01;
obj.Kd = 0.1;
obj.E_k = 0;
obj.e_k_1 = 0;
% plot support
% obj.p = simiam.util.Plotter();
function outputs = execute(obj, robot, state_estimate, inputs, dt)
%% EXECUTE Computes the left and right wheel speeds for go-to-goal.
% [v, w] = execute(obj, robot, x_g, y_g, v) will compute the
% necessary linear and angular speeds that will steer the robot
% to the goal location (x_g, y_g) with a constant linear velocity
% of v.
% See also controller/execute
% Retrieve the (relative) goal location
x_g = inputs.x_g;
y_g = inputs.y_g;
% Get estimate of current pose
[x, y, theta] = state_estimate.unpack();
% Compute the v,w that will get you to the goal
v = inputs.v;
% 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 = theta_g-theta;
e_k = atan2(sin(e_k),cos(e_k));
% 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;
w = obj.Kp*e_P + obj.Ki*e_I + obj.Kd*e_D;
% 4. Save errors for next time step
obj.E_k = e_I;
obj.e_k_1 = e_k;
obj.p.plot_2d_ref(dt, atan2(sin(theta),cos(theta)), theta_g, 'r');
% velocity control
v = 0.25/(log(abs(w)+2)+1); % = inputs.v;
outputs = obj.outputs; % make a copy of the output struct
outputs.v = v;
outputs.w = w;