Commit 63ab8d92 authored by mastefan's avatar mastefan
Browse files

Tested sequence landing and GUI integrator

parent d93b0161
......@@ -354,7 +354,7 @@ private:
ros::Publisher dronexSetpointPublisher;
ros::Subscriber dronexSetpointSubscriber;
ros::Subscriber dronexSetpointToGUISubscriber;
ros::Publisher droneXIntegrParamPublisher;
// > For the PICKER CONTROLLER
......@@ -454,6 +454,7 @@ private:
void highlightTuningControllerTab();
void highlightPickerControllerTab();
void highlightDroneXControllerTab();
void on_set_Integr_Param_button_clicked();
bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
......
......@@ -112,12 +112,12 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
//TODO add Publisher and Subscriber for DroneX
// Publisher and Subscriber for DroneX
dronexButtonPressedPublisher = nodeHandle.advertise<std_msgs::Int32>("DroneXControllerService/ButtonPressed", 1);
dronexSetpointPublisher = nodeHandle.advertise<Setpoint>("DroneXControllerService/Setpoint", 1);
dronexSetpointSubscriber = nodeHandle.subscribe("DroneXControllerService/Setpoint", 1, &MainWindow::dronexSetpointCallback, this);
dronexSetpointToGUISubscriber = nodeHandle.subscribe("DroneXControllerService/SetpointToGUI", 1, &MainWindow::dronexSetpointCallback, this);
droneXIntegrParamPublisher = nodeHandle.advertise<Setpoint>("DroneXControllerService/IntegrParams", 1);
// SET ALL SLIDERS AND DIALS TO DEFAULT VALUES
ui->picker_z_slider->setValue( 40 );
......@@ -1618,7 +1618,16 @@ void MainWindow::on_enable_droneX_controller_clicked()
}
void MainWindow::on_set_Integr_Param_button_clicked()
{
Setpoint msg;
msg.x = (ui->current_setpoint_x_student->text()).toFloat();
msg.y = (ui->current_setpoint_y_student->text()).toFloat();
msg.z = (ui->current_setpoint_z_student->text()).toFloat();
this->droneXIntegrParamPublisher.publish(msg);
}
......
......@@ -405,7 +405,7 @@
</attribute>
<layout class="QGridLayout" name="gridLayout_22">
<item row="0" column="0">
<layout class="QGridLayout" name="gridLayout_21" rowstretch="0,0,0,0,0">
<layout class="QGridLayout" name="gridLayout_21" rowstretch="0,0">
<property name="leftMargin">
<number>0</number>
</property>
......@@ -416,7 +416,7 @@
<number>6</number>
</property>
<item row="1" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_14" stretch="1,0,1">
<layout class="QHBoxLayout" name="horizontalLayout_14" stretch="1,0,0">
<item>
<layout class="QVBoxLayout" name="verticalLayout_10" stretch="0,0,0">
<item>
......@@ -531,6 +531,43 @@
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_62">
<property name="text">
<string>Integrator params</string>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_17">
<property name="topMargin">
<number>0</number>
</property>
<item>
<widget class="QLineEdit" name="x_integr">
<property name="statusTip">
<string/>
</property>
<property name="whatsThis">
<string/>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="y_integr"/>
</item>
<item>
<widget class="QLineEdit" name="z_integr"/>
</item>
</layout>
</item>
<item>
<widget class="QPushButton" name="set_Integr_Param_button">
<property name="text">
<string>Set</string>
</property>
</widget>
</item>
<item>
<spacer name="verticalSpacer_8">
<property name="orientation">
......
......@@ -229,8 +229,8 @@ float m_mass_total_grams;
// > The setpoints for (x,y,z) position and yaw angle, in that order
float m_setpoint[4] = {0.0,0.0,0.4,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
float m_setpoint[4] = {0.0,0.0,0.0,0.0};
float m_setpoint_for_controller[4] = {0.0,0.0,0.0,0.0};
// > Small adjustments to the x-y setpoint
float m_xAdjustment = 0.0f;
......
......@@ -205,7 +205,7 @@ void buttonPressed_land(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Start flight-sequence LA...");
// OLD:flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
// NEW:
flightSequence = 1;
flightSequence = SEQUENCE_LAND_ON_MOTHERSHIP;
}
void buttonPressed_abort(){
......@@ -231,6 +231,14 @@ void buttonPressed_integrator_reset(){
integratorFlag = DRONEX_INTEGRATOR_RESET;
}
void integratorCallback(const Setpoint& integrParams ) {
integrator_var[0] = integrParams.x;
integrator_var[1] = integrParams.y;
integrator_var[2] = integrParams.z;
}
......@@ -356,7 +364,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//ROS_INFO("DRONEX_STATE_APPROACH");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.2;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.6;
if(abs(request.ownCrazyflie.x-dronexSetpoint.x) < tol_eps[0] && abs(request.ownCrazyflie.y-dronexSetpoint.y) < tol_eps[1] &&
abs(request.ownCrazyflie.z-dronexSetpoint.z) < tol_eps[2] ){
......@@ -371,7 +379,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
//ROS_INFO("DRONEX_STATE_GROUND");
// Variable for choosing flight sequence off
flightSequence = 0;
flightSequence = SEQUENCE_NONE;
// Flags of landing sequence reset
tookOffFlag = false;
......@@ -393,7 +401,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
//ROS_INFO("DRONEX_STATE_ON_MOTHERSHIP");
// Variable for choosing flight sequence off
flightSequence = 0;
flightSequence = SEQUENCE_NONE;
// Flags of landing sequence reset
tookOffFlag = false;
......@@ -515,15 +523,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if (flightSequence == 1){
if (flightSequence == SEQUENCE_LAND_ON_MOTHERSHIP){
flying_state = DRONEX_STATE_TAKING_OFF;
//ROS_INFO_STREAM("Flight sequence: Landing on mothership");
if(tookOffFlag){
flying_state = DRONEX_STATE_APPROACH;
if(approachedFlag){
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP; // @ Stefano: Was macht DRONEX_LAND? müessti das nid DRONEX_STATE_LAND_ON_MOTHERSHIP si?
// Valentin
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
}
}
......@@ -1603,22 +1610,11 @@ int main(int argc, char* argv[]) {
dronexSetpointToGUIPublisher = nodeHandle.advertise<Setpoint>("SetpointToGUI", 1);
// Subscribe to get Coordinates of Dronex
//ros::Subscriber dronexCoordinateSubscriber = nodeHandle.subscribe("dronexCoordinate", 1, dronexCoordinateCallback);
// publish flyingState
flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
//ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, dronexCoordinateCallback);
// get Integrator params from GUI
ros::Subscriber droneXIntegrParamSubscriber = nodeHandle.subscribe("IntegrParams", 1, integratorCallback);
......
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