Commit 2b9af726 authored by mmroma's avatar mmroma
Browse files

Simple controller working

parent 15d1bcc1
function [U,X_des] = fullstate_lqr_intAct(X,params,all_params)
function [U,X_des] = fullstate_lqr_intAct(X,params)
%% fullstate_lqr
% Calls Attitude PID
......
function [params] = payload_controller(X_curr, params)
% payload_controller
% Adds additional terms to desired state for quadrotors
%% Stupid Simple Controller
load_x = X(params.num_vehicles*12 + 1);
load_y = X(params.num_vehicles*12 + 2);
load_z = X(params.num_vehicles*12 + 3);
% xd = params.xd(params.tstep_count,:)';
load_xd = params.payload.xd(params.tstep_count,:)';
%%
e_load_
end
function [params] = payload_controller(X, params)
% payload_controller
% Adds additional terms to desired state for quadrotors
%% Stupid Simple Controller
load_x = X(params.num_vehicles*12 + 1);
load_y = X(params.num_vehicles*12 + 2);
load_z = X(params.num_vehicles*12 + 3);
% xd = params.xd(params.tstep_count,:)';
load_xd = params.payload.xd(params.tstep_count,:)';
%%
e_load_pos = load_xd(1:3) - [load_x; load_y; load_z];
params.e_load_pos_int = params.e_load_pos_int + e_load_pos*params.multirotors{1}.ctlDt;
params.e_load_pos_int = max(params.e_load_pos_int,params.e_load_pos_int_LL);
params.e_load_pos_int = min(params.e_load_pos_int,params.e_load_pos_int_UL);
%% Add the error to the quads
for i =1:params.num_vehicles
params.multirotors{i}.xd(params.tstep_count,1:3) = ...
params.multirotors{i}.xd(params.tstep_count,1:3) +...
(params.e_load_pos_Kp * e_load_pos)' + ...
(params.e_load_pos_Ki * params.e_load_pos_int)';
% params.multirotors{i}.xd(params.tstep_count,1:3) = ...
% params.multirotors{i}.xd(params.tstep_count,1:3) +...
% (params.e_load_pos_Ki * params.e_load_pos_int)';
%
% params.multirotors{i}.xd(params.tstep_count,4:6) = ...
% params.multirotors{i}.xd(params.tstep_count,1:3) +...
% (params.e_load_pos_Kp * e_load_pos)';
end
end
......@@ -20,6 +20,13 @@ function [params] = setUpParameters_sphere_4()
params.payload.m = 0.5;
r = params.payload.diameter / 2;
% r = 0;
int_limit = 100;
params.e_load_pos_Kp = 1;
params.e_load_pos_Ki = 2;
params.e_load_pos_int_LL = [-int_limit;-int_limit;-int_limit];
params.e_load_pos_int_UL = [int_limit;int_limit;int_limit];
params.e_load_pos_int = zeros(3,1);
%% Load Multirotors
params.num_vehicles = 5;
......
......@@ -45,6 +45,11 @@ function [simData] = runSimulation(params)
% Compute Control Input
U_curr = [];
X_des_curr = [];
% Payload Augmented Desired State Control
params.tstep_count = tstep_count;
params = payload_controller(X_curr, params);
for i = 1:params.num_vehicles
X_inds = ((i-1)*12 + 1 ) : (i*12);
params.multirotors{i}.tstep_count = tstep_count;
......
......@@ -9,7 +9,7 @@ function [] = plotData(simData,params)
end
%% Plot!
% params.fig_num = plotData_quadrotor(simData,params);
params.fig_num = plotData_quadrotor(simData,params);
params.fig_num = plotData_payload(simData,params);
% params.fig_num = plotData_tethers(simData,params);
% params.fig_num = plotData_drag(simData,params);
......
......@@ -19,7 +19,7 @@ simData = runSimulation(params);
plotData(simData,params);
%% Animate
animate(simData,params);
% animate(simData,params);
%% Save the Data
% saveData(simData,params);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment