Commit 95389c8e authored by mastefan's avatar mastefan
Browse files

Created yamls for own controller, working

parent cab02410
......@@ -324,6 +324,7 @@ add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
add_executable(TuningControllerService src/nodes/TuningControllerService.cpp)
add_executable(PickerControllerService src/nodes/PickerControllerService.cpp)
add_executable(DroneXControllerService src/nodes/DroneXControllerService.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
......@@ -407,6 +408,7 @@ add_dependencies(MpcControllerService d_fall_pps_generate_messages_cpp ${cat
add_dependencies(RemoteControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(TuningControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(PickerControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(DroneXControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(ParameterService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -461,6 +463,7 @@ endif()
target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
target_link_libraries(PickerControllerService ${catkin_LIBRARIES})
target_link_libraries(DroneXControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(ParameterService ${catkin_LIBRARIES})
......
......@@ -192,6 +192,7 @@ private slots:
void on_enable_remote_controller_clicked();
void on_enable_tuning_controller_clicked();
void on_enable_picker_controller_clicked();
void on_enable_droneX_controller_clicked();
......@@ -240,7 +241,7 @@ private slots:
// > For the dial
void on_picker_yaw_dial_valueChanged(int value);
// Interations with the DroneX controller tab
// Interactions with the DroneX controller tab
void on_take_off_dronex_button_clicked();
void on_land_on_dronex_button_clicked();
......
......@@ -267,8 +267,8 @@ void MainWindow::enableGUI()
void MainWindow::highlightSafeControllerTab()
{
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
......@@ -279,8 +279,8 @@ void MainWindow::highlightSafeControllerTab()
void MainWindow::highlightPickerControllerTab()
{
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
......@@ -291,8 +291,8 @@ void MainWindow::highlightDemoControllerTab()
{
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
......@@ -303,8 +303,8 @@ void MainWindow::highlightStudentControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
......@@ -315,8 +315,8 @@ void MainWindow::highlightMpcControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
}
......@@ -327,8 +327,8 @@ void MainWindow::highlightRemoteControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(6, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
}
void MainWindow::highlightTuningControllerTab()
......@@ -339,19 +339,19 @@ void MainWindow::highlightTuningControllerTab()
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(6, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(7, Qt::green);
}
void MainWindow::highlightDroneXControllerTab()
{
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(7, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(7, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
}
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
......@@ -1516,6 +1516,13 @@ void MainWindow::on_enable_picker_controller_clicked()
this->PPSClientCommandPublisher.publish(msg);
}
void MainWindow::on_enable_droneX_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_DRONEX_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
}
// TODO
......@@ -1523,11 +1530,17 @@ void MainWindow::on_enable_picker_controller_clicked()
void MainWindow::on_take_off_dronex_button_clicked(){
ROS_INFO("DroneX: Take off from DroneX! - Please upgrade my function in MainWindow.cpp!!!!!!!!!!!!!!!!!!!");
}
void MainWindow::on_land_on_dronex_button_clicked(){
ROS_INFO("DroneX: Land on DroneX - Please upgrade my function in MainWindow.cpp!!!!!!!!!!!!!!!!!!!");
ROS_INFO("DroneX: Land on DroneX - Please upgrade my function in MainWindow.cpp!!!!!!!!!!!!!!!!!!!");
ROS_INFO("Stefano and valentin were here");
std_msgs::Int32 msg;
msg.data = 10;
//this->droneXSetpointPublisher.publish(msg);
}
......
......@@ -4035,6 +4035,66 @@
<property name="bottomMargin">
<number>6</number>
</property>
<item row="2" column="0">
<widget class="QPushButton" name="enable_droneX_controller">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="font">
<font>
<pointsize>14</pointsize>
</font>
</property>
<property name="text">
<string>DroneX </string>
</property>
</widget>
</item>
<item row="5" column="2">
<widget class="QPushButton" name="load_demo_yaml_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="font">
<font>
<pointsize>14</pointsize>
</font>
</property>
<property name="text">
<string>Demo</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_22">
<property name="sizePolicy">
......@@ -4349,7 +4409,7 @@
</property>
</widget>
</item>
<item row="10" column="0">
<item row="11" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
......@@ -4422,36 +4482,6 @@
</property>
</widget>
</item>
<item row="5" column="2">
<widget class="QPushButton" name="load_demo_yaml_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
<height>50</height>
</size>
</property>
<property name="font">
<font>
<pointsize>14</pointsize>
</font>
</property>
<property name="text">
<string>Demo</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="Line" name="line_3">
<property name="orientation">
......@@ -4473,15 +4503,15 @@
</property>
</widget>
</item>
<item row="10" column="1">
<item row="11" column="1">
<widget class="Line" name="line_5">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QPushButton" name="enable_mpc_controller">
<item row="8" column="0">
<widget class="QPushButton" name="enable_remote_controller">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
......@@ -4506,12 +4536,19 @@
</font>
</property>
<property name="text">
<string>MPC</string>
<string>Remote</string>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QPushButton" name="enable_remote_controller">
<item row="9" column="1">
<widget class="Line" name="line_19">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QPushButton" name="enable_tuning_controller">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
......@@ -4536,19 +4573,12 @@
</font>
</property>
<property name="text">
<string>Remote</string>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="Line" name="line_19">
<property name="orientation">
<enum>Qt::Vertical</enum>
<string>Tuning</string>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QPushButton" name="enable_tuning_controller">
<item row="8" column="2">
<widget class="QPushButton" name="load_remote_yaml_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
......@@ -4573,12 +4603,12 @@
</font>
</property>
<property name="text">
<string>Tuning</string>
<string>Remote</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QPushButton" name="enable_picker_controller">
<item row="7" column="0">
<widget class="QPushButton" name="enable_mpc_controller">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
......@@ -4603,12 +4633,12 @@
</font>
</property>
<property name="text">
<string>Picker</string>
<string>MPC</string>
</property>
</widget>
</item>
<item row="4" column="2">
<widget class="QPushButton" name="load_picker_yaml_button">
<item row="4" column="0">
<widget class="QPushButton" name="enable_picker_controller">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
......@@ -4637,8 +4667,8 @@
</property>
</widget>
</item>
<item row="8" column="2">
<widget class="QPushButton" name="load_remote_yaml_button">
<item row="4" column="2">
<widget class="QPushButton" name="load_picker_yaml_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
......@@ -4663,7 +4693,7 @@
</font>
</property>
<property name="text">
<string>Remote</string>
<string>Picker</string>
</property>
</widget>
</item>
......
// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Place for students to implement their controller
//
// ----------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------
// III N N CCCC L U U DDDD EEEEE SSSS
// I NN N C L U U D D E S
// I N N N C L U U D D EEE SSS
// I N NN C L U U D D E S
// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
// ----------------------------------------------------------------------------------
// These various headers need to be included so that this controller service can be
// connected with the D-FaLL system.
//some useful libraries
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include <ros/package.h>
//the generated structs from the msg-files have to be included
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
//#include "d_fall_pps/CustomControllerYAML.h"
#include "d_fall_pps/CustomButton.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
#include <std_msgs/Int32.h>
// ----------------------------------------------------------------------------------
// DDDD EEEEE FFFFF III N N EEEEE SSSS
// D D E F I NN N E S
// D D EEE FFF I N N N EEE SSS
// D D E F I N NN E S
// DDDD EEEEE F III N N EEEEE SSSS
// ----------------------------------------------------------------------------------
// These constants are defined to make the code more readable and adaptable.
// Universal constants
#define PI 3.1415926535
// These constants define the modes that can be used for controller the Crazyflie 2.0,
// the constants defined here need to be in agreement with those defined in the
// firmware running on the Crazyflie 2.0.
// The following is a short description about each mode:
// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors
// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors, and additionally request the
// body frame roll, pitch, and yaw angular rates from the PID rate
// controllers implemented in the Crazyflie 2.0 firmware.
// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors, and additionally request the
// body frame roll, pitch, and yaw angles from the PID attitude
// controllers implemented in the Crazyflie 2.0 firmware.
#define MOTOR_MODE 6
#define RATE_MODE 7
#define ANGLE_MODE 8
// These constants define the controller used for computing the response in the
// "calculateControlOutput" function
// The following is a short description about each mode:
// LQR_RATE_MODE LQR controller based on the state vector:
// [position,velocity,angles]
//
// LQR_ANGLE_MODE LQR controller based on the state vector:
// [position,velocity]
//
#define LQR_RATE_MODE 1 // (DEFAULT)
#define LQR_ANGLE_MODE 2
// Namespacing the package
using namespace d_fall_pps;
// ----------------------------------------------------------------------------------
// V V A RRRR III A BBBB L EEEEE SSSS
// V V A A R R I A A B B L E S
// V V A A RRRR I A A BBBB L EEE SSS
// V V AAAAA R R I AAAAA B B L E S
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
// Variables for controller
float cf_mass; // Crazyflie mass in grams
std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
float control_frequency = 200.0; // Frequency at which the controller is running
float gravity_force = 0.0; // The weight of the Crazyflie in Newtons, i.e., mg
float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
// The LQR Controller parameters for "LQR_RATE_MODE"
std::vector<float> gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
std::vector<float> gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
std::vector<float> gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
std::vector<float> gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08, 0.00, 0.00, 0.00};
// ROS Publisher for debugging variables
ros::Publisher debugPublisher;
// Variable for the namespaces for the paramter services
// > For the paramter service of this agent
std::string namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
std::string namespace_to_coordinator_parameter_service;
// The ID of this agent, i.e., the ID of this compute
int my_agentID = 0;
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
// The "CrazyflieData" type used for the "request" variable is a
// structure as defined in the file "CrazyflieData.msg" which has the following
// properties:
// string crazyflieName The name given to the Crazyflie in the Vicon software
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
// F U U NN N C T I O O NN N
// FFF U U N N N C T I O O N N N
// F U U N NN C T I O O N NN
// F UUU N N CCCC T III OOO N N
//
// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
// P P R R O O T O O T Y Y P P E S
// PPPP RRRR O O T O O T Y PPPP EEE SSS
// P R R O O T O O T Y P E S
// P R R OOO T OOO T Y P EEEEE SSSS
// ----------------------------------------------------------------------------------
// These function prototypes are not strictly required for this code to complile, but
// adding the function prototypes here means the the functions can be written below in
// any order. If the function prototypes are not included then the function need to
// written below in an order that ensure each function is implemented before it is
// called from another function, hence why the "main" function is at the bottom.
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR