From d929220e609e08d9ca2d3befbc4e8021d74ef3ca Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Sat, 28 May 2022 19:05:48 +0200
Subject: [PATCH] changed controller to roll pitch
---
.../config/missionConfig.yaml | 1 +
.../src/drone_node.cpp | 45 +++++++++++--------
2 files changed, 27 insertions(+), 19 deletions(-)
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index 0791816..ad147a1 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -1,6 +1,7 @@
n_drones: 2
n_ugvs: 0
hz_freq: 50 # controller frquency
+mass: 33 # grams
include_obstacle: false
include_orientation: false
n_obs: 0
diff --git a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
index 905efd9..68bc48d 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -9,7 +9,7 @@
#include "coordination_formation_control_pkg/logData.h"
#include <rosbag/bag.h>
-#include "crazyswarm/Hover.h"
+#include "crazyswarm/ZDistance.h"
#include "crazyswarm/Land.h"
#include "std_msgs/Empty.h"
#include <unistd.h>
@@ -60,6 +60,7 @@ int main(int argc, char **argv) {
ROS_INFO("Drone %d turned ON", robotid);
int n_drones;
int hz_freq, n_obs;
+ float mass = 0;
bool use_obstacle,use_orientation;
nh.getParam("/n_drones", n_drones);
std::cout << "NUmber of drones " << n_drones << std::endl;
@@ -67,6 +68,8 @@ int main(int argc, char **argv) {
nh.getParam("/include_obstacle", use_obstacle);
nh.getParam("/include_orientation", use_orientation);
nh.getParam("/n_obs", n_obs);
+ nh.getParam("/mass", mass);
+ mass = mass/1000.0;
float dt = 1/(float)hz_freq;
@@ -90,9 +93,9 @@ int main(int argc, char **argv) {
logdata->current_acc.resize(n_drones);
/* current command*/
- crazyswarm::Hover current_command;
- current_command.vx = 0;
- current_command.vy = 0;
+ crazyswarm::ZDistance current_command;
+ current_command.roll = 0;
+ current_command.pitch = 0;
current_command.yawrate = 0;
current_command.zDistance = 0;
@@ -179,7 +182,7 @@ int main(int argc, char **argv) {
/* publisher*/
std::stringstream ss;
ss << "/cf" << std::to_string(robotid) << "/cmd_hover";
- ros::Publisher vel_cmd_publisher = nh.advertise<crazyswarm::Hover>(ss.str(),1);
+ ros::Publisher vel_cmd_publisher = nh.advertise<crazyswarm::ZDistance>(ss.str(),1);
std::stringstream ss_stop;
ss_stop << "/cf" << std::to_string(robotid) << "/cmd_stop";
@@ -254,8 +257,8 @@ int main(int argc, char **argv) {
{
// send the robot to hover at zero velocity in plane
// std::cout << "Here" << std::endl;
- current_command.vx = 0;
- current_command.vy = 0;
+ current_command.roll = 0;
+ current_command.pitch = 0;
current_command.yawrate = 0;
current_command.zDistance = 0.5;
vel_cmd_publisher.publish(current_command);
@@ -309,14 +312,14 @@ int main(int argc, char **argv) {
input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
// std::cout << input << std::endl;
/* integrate acceleration*/
- Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0),uav_state_p(0, robotid-1),uav_state_p(1, robotid-1) );
- Eigen::Vector2d vel_command_;
- vel_command_ << input(0,0), input(1,0);
+ // Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0),uav_state_p(0, robotid-1),uav_state_p(1, robotid-1) );
+ // Eigen::Vector2d vel_command_;
+ // vel_command_ << input(0,0), input(1,0);
/* reached a certain waypoint and we want to stop sending command*/
if(next_waypoint->stop) // change back TODO
{
- current_command.vx = 0;
- current_command.vy = 0;
+ current_command.roll = 0;
+ current_command.pitch = 0;
}
else
{
@@ -324,19 +327,23 @@ int main(int argc, char **argv) {
/* get latest yaw*/
robot_state_t current_state_;
current_state_ = uavs[robotid-1]->get_state();
+
float yaw = current_state_.p(2)*M_PI/180.0; // TODO confirm that indeed it is in degrees
- float vxl = vel_command_(0)*std::cos(yaw) + vel_command_(1)*std::sin(yaw) ;
- float vyl = - vel_command_(0)*std::sin(yaw) + vel_command_(1)*std::cos(yaw) ;
+ float ax = input(0,0)*std::cos(yaw) + input(1,0)*std::sin(yaw) ;
+ float ay = - input(0,0)*std::sin(yaw) + input(1,0)*std::cos(yaw) ;
+ //
+ // float roll_command = ;
+ // float pitch_command = ;
// current_command.vx = vxl;
// current_command.vy = vyl;
row_vector_2d_t in;
- in << vxl, vyl;
+ in << ax, ay;
row_vector_2d_t range;
range << -0.15, 0.15;
row_vector_2d_t out = saturate(in, range);
- current_command.vx = out(0);
- current_command.vy = out(1);
+ current_command.roll = out(0)/mass; // conver it to the desired angle
+ current_command.pitch = out(1)/mass;
// vxx = vxl;
// vyy = vyl;
}
@@ -424,8 +431,8 @@ int main(int argc, char **argv) {
logdata->input_orientation.y = input(1,4);
logdata->input_integration.x = input(0,5);
logdata->input_integration.y = input(1,5);
- logdata->input_to_system.x = current_command.vx;
- logdata->input_to_system.y = current_command.vy;
+ logdata->input_to_system.x = current_command.roll;
+ logdata->input_to_system.y = current_command.pitch;
--
GitLab