From d3ff7e618a0cda1c4bfb16e38fe9e435545c2812 Mon Sep 17 00:00:00 2001 From: Paul Beuchat <beuchatp@control.ee.ethz.ch> Date: Wed, 18 Apr 2018 11:38:17 +0200 Subject: [PATCH] Final attempt at tuning the full state LQR, no success --- pps_ws/src/d_fall_pps/param/ClientConfig.yaml | 2 +- .../src/d_fall_pps/param/DemoController.yaml | 6 +- .../d_fall_pps/src/DemoControllerService.cpp | 62 +++++++++++-------- 3 files changed, 40 insertions(+), 30 deletions(-) diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml index 7c810af8..a32012ed 100755 --- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml +++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml @@ -1,6 +1,6 @@ safeController: "SafeControllerService/RateController" demoController: "DemoControllerService/DemoController" -strictSafety: true +strictSafety: false angleMargin: 0.6 battery_threshold_while_flying: 2.8 # in V battery_threshold_while_motors_off: 3.30 # in V diff --git a/pps_ws/src/d_fall_pps/param/DemoController.yaml b/pps_ws/src/d_fall_pps/param/DemoController.yaml index 8b0e4f10..dcde9927 100644 --- a/pps_ws/src/d_fall_pps/param/DemoController.yaml +++ b/pps_ws/src/d_fall_pps/param/DemoController.yaml @@ -1,5 +1,5 @@ # Mass of the crazyflie -mass : 31 +mass : 30 # Frequency of the controller, in hertz control_frequency : 200 @@ -27,7 +27,7 @@ follow_in_a_line_agentIDs : [1, 2, 3] shouldPublishDebugMessage : false # Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not -shouldDisplayDebugInfo : true +shouldDisplayDebugInfo : false # A flag for which LQR controller mode to use, defined as: @@ -50,7 +50,7 @@ lqr_controller_mode : 3 # each of (x,y,z,roll,pitch,yaw) # 3 - Quad-rotor Model Based Method # Uses the model of the quad-rotor and the previous inputs -estimator_method : 2 +estimator_method : 1 # The LQR Controller parameters for "mode = 1" diff --git a/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp b/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp index 5f0bea33..6a3ab506 100644 --- a/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp +++ b/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp @@ -471,10 +471,41 @@ void calculateControlOutput_viaLQRforMotors(float stateErrorBody[12], Controller motor4_thrustAdjustment -= gainMatrixMotor4[i] * stateErrorBody[i]; } - //motor1_thrustAdjustment = -gravity_force_quarter*0.9; - //motor2_thrustAdjustment = -gravity_force_quarter*0.9; - //motor3_thrustAdjustment = -gravity_force_quarter*0.9; - //motor4_thrustAdjustment = -gravity_force_quarter*0.9; + DebugMsg debugMsg; + + // Fill the debugging message with the data provided by Vicon + //debugMsg.vicon_x = request.ownCrazyflie.x; + //debugMsg.vicon_y = request.ownCrazyflie.y; + //debugMsg.vicon_z = request.ownCrazyflie.z; + //debugMsg.vicon_roll = request.ownCrazyflie.roll; + //debugMsg.vicon_pitch = request.ownCrazyflie.pitch; + //debugMsg.vicon_yaw = request.ownCrazyflie.yaw; + + // debugMsg.value_1 = thrustAdjustment; + // ...................... + // debugMsg.value_10 = your_variable_name; + + debugMsg.value_1 = stateErrorBody[6]; + debugMsg.value_2 = stateErrorBody[9]; + + debugMsg.value_3 = motor1_thrustAdjustment; + debugMsg.value_4 = motor2_thrustAdjustment; + debugMsg.value_5 = motor3_thrustAdjustment; + debugMsg.value_6 = motor4_thrustAdjustment; + + + + + // Publish the "debugMsg" + debugPublisher.publish(debugMsg); + + + + + motor1_thrustAdjustment = -gravity_force_quarter*0.9; + motor2_thrustAdjustment = -gravity_force_quarter*0.9; + motor3_thrustAdjustment = -gravity_force_quarter*0.9; + motor4_thrustAdjustment = -gravity_force_quarter*0.9; // UPDATE THE "RETURN" THE VARIABLE NAMED "response" @@ -497,28 +528,7 @@ void calculateControlOutput_viaLQRforMotors(float stateErrorBody[12], Controller // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE; - DebugMsg debugMsg; - - // Fill the debugging message with the data provided by Vicon - //debugMsg.vicon_x = request.ownCrazyflie.x; - //debugMsg.vicon_y = request.ownCrazyflie.y; - //debugMsg.vicon_z = request.ownCrazyflie.z; - //debugMsg.vicon_roll = request.ownCrazyflie.roll; - //debugMsg.vicon_pitch = request.ownCrazyflie.pitch; - //debugMsg.vicon_yaw = request.ownCrazyflie.yaw; - - // debugMsg.value_1 = thrustAdjustment; - // ...................... - // debugMsg.value_10 = your_variable_name; - - debugMsg.value_1 = motor1_thrustAdjustment; - debugMsg.value_2 = motor2_thrustAdjustment; - debugMsg.value_3 = motor3_thrustAdjustment; - debugMsg.value_4 = motor4_thrustAdjustment; - - - // Publish the "debugMsg" - debugPublisher.publish(debugMsg); + // An alternate debugging technique is to print out data directly to the -- GitLab