From 21d933f0389194f6df0594d2fac5bc4f244f1c37 Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Thu, 19 May 2022 14:52:40 +0200
Subject: [PATCH] safe push
---
.../src/test_controller.cpp | 14 ++++++++++++++
simulation/controller/test_controllers.m | 14 ++++++++++----
2 files changed, 24 insertions(+), 4 deletions(-)
diff --git a/real_robot/coordination_formation_control_pkg/src/test_controller.cpp b/real_robot/coordination_formation_control_pkg/src/test_controller.cpp
index 7badb88..3944eb0 100644
--- a/real_robot/coordination_formation_control_pkg/src/test_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/test_controller.cpp
@@ -41,6 +41,20 @@ int main(int argc, char **argv)
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_m = 0;
diff --git a/simulation/controller/test_controllers.m b/simulation/controller/test_controllers.m
index 09e7c1c..8848d1e 100644
--- a/simulation/controller/test_controllers.m
+++ b/simulation/controller/test_controllers.m
@@ -14,10 +14,12 @@ controller_gains.c2_beta = 1;
controller_gains.c1_gamma = 1;
controller_gains.c2_gamma = 1;
controller_gains.c1_theta = 1;
+controller_gains.c1_delta = 1;
+
form_param.dist = 0.2;
form_param.type = 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.a = 5;
form_param.b = 5;
@@ -26,12 +28,13 @@ form_param.h_beta = 0.9;
form_param.d_obs = 0;
form_param.nav_type = 1; % triangle
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.integrator = 1;
-
+cooperation = 1;
%% demo state and goal
q0 = [1 1 2;
@@ -48,11 +51,14 @@ obs = [1, 0,0
-1.15,0,0;
0.5,2,3];
-ori = [];
+ori = 0;
c1 = SwamControllerAlg(robot_size, id,ind_pos,form_param,controller_gains);
c1.showFormation
-[input, input_vec ] = c1.controller(q0,p0,ref,obs,0);
\ No newline at end of file
+[input, input_vec ] = c1.controller(q0,p0,ref,obs,ori,cooperation);
+
+input
+input_vec
--
GitLab