To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 5d2e5ad6 authored by elokdae's avatar elokdae
Browse files

Merged DeePC developments so far

parent d05fcff5
......@@ -56,7 +56,7 @@
<property name="bottomMargin">
<number>6</number>
</property>
<item row="0" column="3">
<item row="0" column="4">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
......@@ -108,7 +108,7 @@
</property>
</widget>
</item>
<item row="1" column="2">
<item row="2" column="2">
<layout class="QHBoxLayout" name="horizontalLayout_2">
<property name="leftMargin">
<number>0</number>
......@@ -134,7 +134,7 @@
</item>
</layout>
</item>
<item row="1" column="1">
<item row="2" column="1">
<layout class="QHBoxLayout" name="horizontalLayout_3">
<property name="leftMargin">
<number>0</number>
......@@ -160,7 +160,7 @@
</item>
</layout>
</item>
<item row="1" column="0">
<item row="2" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_4">
<property name="leftMargin">
<number>0</number>
......@@ -186,6 +186,35 @@
</item>
</layout>
</item>
<item row="0" column="3">
<widget class="QPushButton" name="custom_button_4">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Button 4</string>
</property>
</widget>
</item>
<item row="2" column="3">
<widget class="QLineEdit" name="lineEdit_custom_4">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="0">
......
......@@ -110,7 +110,7 @@ private slots:
void on_custom_button_1_clicked();
void on_custom_button_2_clicked();
void on_custom_button_3_clicked();
void on_custom_button_4_clicked();
......
......@@ -145,8 +145,6 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ui->action_showHideController_picker->trigger();
// > For the tuning controller
ui->action_showHideController_tuning->trigger();
// > For the template controller
ui->action_showHideController_template->trigger();
}
......
......@@ -178,7 +178,12 @@ void TemplateControllerTab::on_custom_button_3_clicked()
#endif
}
void TemplateControllerTab::on_custom_button_4_clicked()
{
#ifdef CATKIN_MAKE
publish_custom_button_command(4,ui->lineEdit_custom_4);
#endif
}
......
......@@ -163,11 +163,49 @@ std::vector<float> yaml_gainMatrixRollRate = { 0.00,-6.20, 0.00,
std::vector<float> yaml_gainMatrixPitchRate = { 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00};
std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30};
// Thrust perturbation parameters
float yaml_thrustPerturbAmp_in_grams = 0.0;
float yaml_thrustPerturbFreq = 0.0;
// Roll rate perturbation parameters
float yaml_rollRatePerturbAmp_in_deg = 0.0;
float yaml_rollRatePerturbFreq = 0.0;
// Pitch rate perturbation parameters
float yaml_pitchRatePerturbAmp_in_deg = 0.0;
float yaml_pitchRatePerturbFreq = 0.0;
// Yaw rate perturbation parameters
float yaml_yawRatePerturbAmp_in_deg = 0.0;
float yaml_yawRatePerturbFreq = 0.0;
// The weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = 0.0;
// Thrust perturbation in Newtons
float m_thrustPerturbAmp_in_newtons = 0.0;
// Roll rate perturbation in rad/s
float m_rollRatePerturbAmp_in_rad = 0.0;
// Pitch rate perturbation in rad/s
float m_pitchRatePerturbAmp_in_rad = 0.0;
// Yaw rate perturbation in rad/s
float m_yawRatePerturbAmp_in_rad = 0.0;
// Enable thrust perturbation
bool m_thrustPerturbEnable = false;
// Enable roll rate perturbation
bool m_rollRatePerturbEnable = false;
// Enable pitch rate perturbation
bool m_pitchRatePerturbEnable = false;
// Enable yaw rate perturbation
bool m_yawRatePerturbEnable = false;
// The location error of the Crazyflie at the "previous" time step
float m_previous_stateErrorInertial[9];
......@@ -175,7 +213,8 @@ float m_previous_stateErrorInertial[9];
// with units [meters,meters,meters,radians]
std::vector<float> m_setpoint{0.0,0.0,0.4,0.0};
// Time
float m_time = 0.0;
// ROS Publisher for debugging variables
......
......@@ -26,7 +26,7 @@ mocap_timeout_duration: 2.0
# Flag for whether the take-off should be performed
# with the default controller
shouldPerfom_takeOff_with_defaultController: true
shouldPerfom_takeOff_with_defaultController: false
# Flag for whether the landing should be performed
# with the default controller
......
# Mass of the crazyflie
# Mass of the crazyflie, in grams
mass : 28
# Frequency of the controller, in hertz
......@@ -26,3 +26,27 @@ gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.0
gainMatrixRollRate : [ 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00]
gainMatrixPitchRate : [ 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
# Thrust perturbation magnitude, in grams
thrustPerturbAmp : 20
# Thrust perturbation frequency, in hertz
thrustPerturbFreq: 0.1
# Roll rate perturbation magnitude, in deg/s
rollRatePerturbAmp : 60
# Roll rate perturbation frequency, in hertz
rollRatePerturbFreq: 0.1
# Pitch rate perturbation magnitude, in deg/s
pitchRatePerturbAmp : 60
# Pitch rate perturbation frequency, in hertz
pitchRatePerturbFreq: 0.1
# Yaw rate perturbation magnitude, in deg/s
yawRatePerturbAmp : 180
# Yaw rate perturbation frequency, in hertz
yawRatePerturbFreq: 0.1
\ No newline at end of file
......@@ -257,6 +257,14 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.pitch = pitchRate_forResponse;
response.controlOutput.yaw = yawRate_forResponse;
// Roll/pitch/yaw rate perturbation
if (m_rollRatePerturbEnable)
response.controlOutput.roll += m_rollRatePerturbAmp_in_rad * sin(2 * PI * yaml_rollRatePerturbFreq * m_time);
if (m_pitchRatePerturbEnable)
response.controlOutput.pitch += m_pitchRatePerturbAmp_in_rad * sin(2 * PI * yaml_pitchRatePerturbFreq * m_time);
if (m_yawRatePerturbEnable)
response.controlOutput.yaw += m_yawRatePerturbAmp_in_rad * sin(2 * PI * yaml_yawRatePerturbFreq * m_time);
// Put the thrust commands into the "response" variable.
// The thrust adjustment computed by the controller need to be added to the
// the feed-forward thrust that "counter-acts" gravity.
......@@ -268,6 +276,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// value is divided by four.
float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
float thrust_request_per_motor = thrustAdjustment / 4.0 + feed_forward_thrust_per_motor;
// Thrust perturbation
if (m_thrustPerturbEnable)
thrust_request_per_motor += m_thrustPerturbAmp_in_newtons / 4.0 * sin(2 * PI * yaml_thrustPerturbFreq * m_time);
// > NOTE: the function "computeMotorPolyBackward" converts the input argument
// from Newtons to the 16-bit command expected by the Crazyflie.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrust_request_per_motor);
......@@ -284,6 +297,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
// ADVANCE TIME
m_time += 1 / yaml_control_frequency;
......@@ -602,6 +617,13 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 1 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 1
if (m_thrustPerturbEnable)
m_thrustPerturbEnable = false;
else
{
m_time = 0.0;
m_thrustPerturbEnable = true;
}
break;
}
......@@ -612,6 +634,13 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 2 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 2
if (m_rollRatePerturbEnable)
m_rollRatePerturbEnable = false;
else
{
m_time = 0.0;
m_rollRatePerturbEnable = true;
}
break;
}
......@@ -622,6 +651,30 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 3 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 3
if (m_pitchRatePerturbEnable)
m_pitchRatePerturbEnable = false;
else
{
m_time = 0.0;
m_pitchRatePerturbEnable = true;
}
break;
}
// > FOR CUSTOM BUTTON 4
case 4:
{
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 4 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 3
if (m_yawRatePerturbEnable)
m_yawRatePerturbEnable = false;
else
{
m_time = 0.0;
m_yawRatePerturbEnable = true;
}
break;
}
......@@ -749,6 +802,22 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
// Thrust perturbation parameters
yaml_thrustPerturbAmp_in_grams = getParameterFloat(nodeHandle_for_paramaters, "thrustPerturbAmp");
yaml_thrustPerturbFreq = getParameterFloat(nodeHandle_for_paramaters, "thrustPerturbFreq");
// Roll rate perturbation parameters
yaml_rollRatePerturbAmp_in_deg = getParameterFloat(nodeHandle_for_paramaters, "rollRatePerturbAmp");
yaml_rollRatePerturbFreq = getParameterFloat(nodeHandle_for_paramaters, "rollRatePerturbFreq");
// Pitch rate perturbation parameters
yaml_pitchRatePerturbAmp_in_deg = getParameterFloat(nodeHandle_for_paramaters, "pitchRatePerturbAmp");
yaml_pitchRatePerturbFreq = getParameterFloat(nodeHandle_for_paramaters, "pitchRatePerturbFreq");
// Yaw rate perturbation parameters
yaml_yawRatePerturbAmp_in_deg = getParameterFloat(nodeHandle_for_paramaters, "yawRatePerturbAmp");
yaml_yawRatePerturbFreq = getParameterFloat(nodeHandle_for_paramaters, "yawRatePerturbFreq");
// > DEBUGGING: Print out one of the parameters that was loaded to
// debug if the fetching of parameters worked correctly
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] DEBUGGING: the fetched TemplateController/mass = " << yaml_cf_mass_in_grams);
......@@ -761,6 +830,19 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
// gravity (i.e., mg) in units of [Newtons]
m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
// > Compute the thrust perturbation force in units of [Newtons]
m_thrustPerturbAmp_in_newtons = yaml_thrustPerturbAmp_in_grams * 9.81/1000.0;
// > Compute the roll rate perturbation in units of [rad/s]
m_rollRatePerturbAmp_in_rad = yaml_rollRatePerturbAmp_in_deg * PI/180.0;
// > Compute the pitch rate perturbation in units of [rad/s]
m_pitchRatePerturbAmp_in_rad = yaml_pitchRatePerturbAmp_in_deg * PI/180.0;
// > Compute the yaw rate perturbation in units of [rad/s]
m_yawRatePerturbAmp_in_rad = yaml_yawRatePerturbAmp_in_deg * PI/180.0;
// DEBUGGING: Print out one of the computed quantities
ROS_INFO_STREAM("[TEMPLATE CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
}
......
Markdown is supported
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