Skip to content
Snippets Groups Projects
Commit 21d933f0 authored by Stevedan Ogochukwu Omodolor's avatar Stevedan Ogochukwu Omodolor
Browse files

safe push

parent 28869aad
No related branches found
No related tags found
No related merge requests found
...@@ -41,6 +41,20 @@ int main(int argc, char **argv) ...@@ -41,6 +41,20 @@ int main(int argc, char **argv)
robot_ind_t robot_ind; robot_ind_t robot_ind;
Eigen::MatrixXf uav_state_q(2,3);
Eigen::MatrixXf uav_state_p(2,3);
obstacle_t *current_obs = new obstacle_t[n_obs]();
waypoint_t current_waypoint;
uav_state_q << 1 1 2 -1 -1.1 -2;
uav_state_q << 0 0 0 0 0;
current_waypoint ref;
ref.q << 0,0;
ref.p << 5,1;
robot_ind.id = 1; robot_ind.id = 1;
robot_ind.id_m = 0; robot_ind.id_m = 0;
......
...@@ -14,10 +14,12 @@ controller_gains.c2_beta = 1; ...@@ -14,10 +14,12 @@ controller_gains.c2_beta = 1;
controller_gains.c1_gamma = 1; controller_gains.c1_gamma = 1;
controller_gains.c2_gamma = 1; controller_gains.c2_gamma = 1;
controller_gains.c1_theta = 1; controller_gains.c1_theta = 1;
controller_gains.c1_delta = 1;
form_param.dist = 0.2; form_param.dist = 0.2;
form_param.type = 2; form_param.type = 2;
form_param.k = 1.2; form_param.k = 1.2;
form_param.ratio = 0.8; % ratio between dist inter and obstacle form_param.ratio = 7; % ratio between dist inter and obstacle
form_param.eps = 0.1; form_param.eps = 0.1;
form_param.a = 5; form_param.a = 5;
form_param.b = 5; form_param.b = 5;
...@@ -26,12 +28,13 @@ form_param.h_beta = 0.9; ...@@ -26,12 +28,13 @@ form_param.h_beta = 0.9;
form_param.d_obs = 0; form_param.d_obs = 0;
form_param.nav_type = 1; % triangle form_param.nav_type = 1; % triangle
form_param.integrator = 2; form_param.integrator = 2;
form_param.dt = 0.01;
% form_param.nav_type = 2; % form_param.nav_type = 2;
% form_param.nav_type = 2; % form_param.nav_type = 2;
% form_param.nav_type = 2; % form_param.nav_type = 2;
form_param.integrator = 1; form_param.integrator = 1;
cooperation = 1;
%% demo state and goal %% demo state and goal
q0 = [1 1 2; q0 = [1 1 2;
...@@ -48,11 +51,14 @@ obs = [1, 0,0 ...@@ -48,11 +51,14 @@ obs = [1, 0,0
-1.15,0,0; -1.15,0,0;
0.5,2,3]; 0.5,2,3];
ori = []; ori = 0;
c1 = SwamControllerAlg(robot_size, id,ind_pos,form_param,controller_gains); c1 = SwamControllerAlg(robot_size, id,ind_pos,form_param,controller_gains);
c1.showFormation c1.showFormation
[input, input_vec ] = c1.controller(q0,p0,ref,obs,0); [input, input_vec ] = c1.controller(q0,p0,ref,obs,ori,cooperation);
\ No newline at end of file
input
input_vec
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment