Commit fdb019d5 authored by Tobias's avatar Tobias

final commit by TK

parent 63aa325f
This diff is collapsed.
0,0,0,0,0,0,0,0,0,0,0,0
0.0102491,0.33115,-0.00610016,-0.0164237,0.0738702,-0.215738,0.307408,-0.0163027,-0.0613293,-0.213034,-0.320688,-0.015635
0.0156948,0.548559,-0.0100623,-0.0315152,0.108752,-0.371753,0.515573,-0.0311044,-0.0855198,-0.366961,-0.542644,-0.029185
0.0181043,0.678679,-0.01293,-0.0455306,0.116241,-0.483746,0.649121,-0.0437021,-0.0848683,-0.476503,-0.687187,-0.0416027
0.0183799,0.74119,-0.0144775,-0.059004,0.108439,-0.55959,0.726846,-0.0552897,-0.0714276,-0.551423,-0.776004,-0.0531425
0.016709,0.749566,-0.0157192,-0.071332,0.0947248,-0.608056,0.759907,-0.0657499,-0.0536201,-0.601802,-0.816955,-0.0630986
0.0133048,0.728695,-0.0179059,-0.0833119,0.0817649,-0.639669,0.767176,-0.0752978,-0.0379079,-0.634895,-0.831088,-0.0721977
0.0101083,0.690945,-0.0214814,-0.0945284,0.0654671,-0.657536,0.759745,-0.0837621,-0.0212369,-0.659449,-0.828929,-0.0805574
0.00703688,0.655945,-0.0262512,-0.105487,0.0484927,-0.676874,0.754464,-0.0921367,-0.00585237,-0.683006,-0.828926,-0.0877035
0.00310893,0.625908,-0.0328796,-0.11604,0.0337968,-0.695057,0.753926,-0.0990857,0.00690825,-0.707491,-0.832467,-0.094473
-0.00172129,0.622579,-0.0418367,-0.126063,0.0241599,-0.725695,0.773552,-0.105719,0.0141896,-0.741232,-0.85875,-0.101243
-0.00625429,0.626615,-0.0521429,-0.135575,0.0178352,-0.753553,0.796053,-0.111591,0.0172088,-0.776563,-0.887983,-0.107574
-0.0112212,0.647063,-0.061675,-0.144343,0.0157638,-0.795159,0.836872,-0.11655,0.016289,-0.824546,-0.933403,-0.113549
-0.0173832,0.690175,-0.0720571,-0.152858,0.0126016,-0.843657,0.895816,-0.121843,0.0171061,-0.88387,-0.995427,-0.119195
-0.0235139,0.726095,-0.0812017,-0.161274,0.00734033,-0.886065,0.947417,-0.1271,0.0198496,-0.934414,-1.04995,-0.124185
-0.0292797,0.774641,-0.0911686,-0.169193,0.00268378,-0.93103,1.00802,-0.131565,0.0211917,-0.989,-1.11495,-0.129133
-0.0335945,0.824774,-0.0997727,-0.176926,-0.00465665,-0.976913,1.06992,-0.135447,0.0236954,-1.0446,-1.18104,-0.133668
-0.0390284,0.886678,-0.108565,-0.184139,-0.00879995,-1.03129,1.1456,-0.13936,0.0240863,-1.11148,-1.25813,-0.13829
-0.0447113,0.948583,-0.116696,-0.190893,-0.0134233,-1.08517,1.22272,-0.143203,0.0251156,-1.17903,-1.3351,-0.143161
-0.0503426,1.02691,-0.124571,-0.197737,-0.0187311,-1.14953,1.31652,-0.145907,0.0268979,-1.25386,-1.42589,-0.147539
-0.0555968,1.10851,-0.132235,-0.20419,-0.0241867,-1.21269,1.41351,-0.148782,0.0281625,-1.33082,-1.51931,-0.151934
-0.06267,1.22406,-0.140249,-0.21051,-0.0304241,-1.3001,1.54132,-0.151078,0.0322101,-1.42333,-1.64074,-0.156312
-0.0693283,1.35071,-0.149655,-0.216453,-0.0354582,-1.38094,1.67353,-0.15336,0.0344112,-1.51427,-1.76715,-0.160419
-0.075411,1.50332,-0.160054,-0.22221,-0.0414277,-1.479,1.83075,-0.15605,0.0370552,-1.62086,-1.91738,-0.164961
-0.0798565,1.67676,-0.171626,-0.227946,-0.0497364,-1.58327,2.00482,-0.157775,0.0404053,-1.73421,-2.08289,-0.169419
-0.0854385,1.87201,-0.183342,-0.23356,-0.0551509,-1.70406,2.19989,-0.159825,0.0419207,-1.86417,-2.26796,-0.174131
-0.0904145,2.09579,-0.195789,-0.238989,-0.0573721,-1.83537,2.42092,-0.161763,0.0401129,-2.0087,-2.47628,-0.179028
-0.0949707,2.35152,-0.208139,-0.243977,-0.0618237,-1.97795,2.66956,-0.162878,0.0400508,-2.16966,-2.71068,-0.184151
-0.0984003,2.64298,-0.222037,-0.248849,-0.0670611,-2.14211,2.95192,-0.164507,0.0399324,-2.34997,-2.97508,-0.189716
-0.103947,2.96369,-0.236692,-0.253441,-0.0692593,-2.32219,3.26247,-0.166124,0.039454,-2.54827,-3.26669,-0.195427
-0.111253,3.31712,-0.251863,-0.257564,-0.0713712,-2.51577,3.6022,-0.167244,0.0410897,-2.76235,-3.58384,-0.201508
-0.114073,3.70724,-0.267666,-0.261463,-0.0752917,-2.72716,3.9756,-0.168147,0.0403239,-2.99537,-3.93063,-0.208385
-0.119461,4.14047,-0.284295,-0.265097,-0.0765575,-2.95727,4.38615,-0.168828,0.0400684,-3.24851,-4.31037,-0.215511
-0.123101,4.61138,-0.302012,-0.268562,-0.0808665,-3.20383,4.83222,-0.169204,0.0417898,-3.52118,-4.71939,-0.223259
-0.12811,5.14148,-0.31961,-0.271386,-0.0814347,-3.48018,5.33039,-0.168994,0.0420225,-3.82268,-5.17272,-0.231316
-0.131699,5.7222,-0.337633,-0.274027,-0.0827606,-3.77738,5.87517,-0.168074,0.0421608,-4.1449,-5.6647,-0.240221
-0.13427,6.35289,-0.355722,-0.276357,-0.0845575,-4.09872,6.46583,-0.16692,0.0427041,-4.4927,-6.19453,-0.249575
-0.135783,7.05724,-0.37344,-0.278318,-0.0872989,-4.45475,7.12452,-0.164627,0.0444159,-4.87616,-6.77942,-0.259435
-0.138587,7.83012,-0.393415,-0.279713,-0.0891131,-4.84558,7.84567,-0.160552,0.0479335,-5.29688,-7.41702,-0.270433
-0.142043,8.6669,-0.414108,-0.280668,-0.0852037,-5.27056,8.62546,-0.154722,0.0477987,-5.75278,-8.10254,-0.281918
-0.143981,9.58513,-0.435992,-0.280864,-0.0842111,-5.7419,9.47741,-0.147018,0.050687,-6.25357,-8.8522,-0.294582
-0.144358,10.584,-0.461476,-0.280539,-0.0823197,-6.25593,10.3999,-0.13677,0.0528863,-6.79893,-9.66266,-0.308543
-0.144229,11.678,-0.48834,-0.279586,-0.0778197,-6.82876,11.4078,-0.123671,0.0540743,-7.40212,-10.5481,-0.324181
-0.146251,12.8781,-0.519525,-0.277348,-0.07553,-7.46994,12.5063,-0.106309,0.0618632,-8.06243,-11.5174,-0.341175
-0.147054,14.2085,-0.553085,-0.27389,-0.0691245,-8.20029,13.7182,-0.0823515,0.0668096,-8.79944,-12.5895,-0.360111
-0.147225,15.6554,-0.590507,-0.26876,-0.0626766,-9.0294,15.0304,-0.0488709,0.074009,-9.61135,-13.7554,-0.381658
-0.146807,17.2614,-0.631641,-0.261995,-0.0524977,-10.0009,16.4743,-0.00180931,0.0802051,-10.5166,-15.0496,-0.405527
-0.143862,19.0214,-0.675103,-0.253335,-0.0435475,-11.1359,18.0386,0.0641391,0.0883456,-11.5259,-16.471,-0.432252
-0.141613,20.9352,-0.723162,-0.242859,-0.0317618,-12.4465,19.707,0.151233,0.0986358,-12.6334,-18.0174,-0.461313
-0.138493,22.9775,-0.776682,-0.230615,-0.0171915,-13.9116,21.4434,0.256757,0.109996,-13.8332,-19.663,-0.492044
-0.132898,25.1245,-0.833448,-0.216869,-0.000620678,-15.478,23.2074,0.372333,0.122178,-15.0991,-21.3797,-0.524659
-0.126851,27.3364,-0.895176,-0.201539,0.0182513,-17.0848,24.9518,0.489708,0.137457,-16.3991,-23.1275,-0.558119
-0.116638,29.6261,-0.958359,-0.185111,0.0382522,-18.7222,26.691,0.603891,0.153639,-17.7414,-24.9118,-0.591861
-0.105714,29.8106,-1.01907,-0.166998,0.0619962,-16.2754,26.1326,0.711572,0.171837,-17.4167,-24.4351,-0.625464
-0.0920611,30.5681,-1.07891,-0.148656,0.0893948,-14.4651,26.0679,0.810604,0.190486,-17.4695,-24.4801,-0.657137
-0.0742325,31.7955,-1.13529,-0.130274,0.119071,-13.1868,26.4,0.900518,0.210075,-17.8241,-24.9455,-0.687644
-0.0528052,33.4115,-1.19047,-0.112581,0.152573,-12.3552,27.0514,0.98087,0.229593,-18.4323,-25.753,-0.71587
-0.0312387,35.3782,-1.24364,-0.0962825,0.189584,-11.9123,27.9846,1.0516,0.253437,-19.2456,-26.8605,-0.741754
-0.00678755,37.6368,-1.30153,-0.0812549,0.229013,-11.8028,29.1509,1.11351,0.279424,-20.2422,-28.2211,-0.766308
0.0107815,29.8581,-1.3673,-0.0670323,0.275579,-8.28285,23.6618,1.16696,0.311616,-15.3013,-22.9437,-0.786996
-0.00592279,24.2049,-1.45343,-0.0532909,0.344544,-5.80159,19.7649,1.21298,0.362335,-11.559,-19.2743,-0.807244
-0.0447858,20.183,-1.56109,-0.0376649,0.433267,-4.12677,17.0589,1.25263,0.420775,-8.76631,-16.8167,-0.826701
-0.0886191,17.4322,-1.68684,-0.0190717,0.532022,-3.0754,15.2524,1.28618,0.478308,-6.73087,-15.2816,-0.843653
-0.122264,15.71,-1.82842,0.0037253,0.631748,-2.53226,14.143,1.31371,0.527133,-5.30559,-14.464,-0.860943
-0.145299,14.779,-1.97288,0.0311686,0.726456,-2.37146,13.5484,1.33604,0.570191,-4.35446,-14.154,-0.883846
-0.137985,14.5513,-2.11609,0.0636601,0.808849,-2.55815,13.4023,1.35377,0.595056,-3.84249,-14.3008,-0.908929
-0.102101,14.913,-2.25234,0.101207,0.880743,-3.02857,13.6312,1.36651,0.601148,-3.72226,-14.8198,-0.937483
-0.0231249,15.7786,-2.37149,0.144055,0.878867,-3.72545,14.1726,1.37482,0.634137,-3.92229,-15.6278,-0.972516
0.154187,17.267,-2.46798,0.191397,0.813025,-4.7033,15.1313,1.37831,0.62459,-4.47714,-16.8247,-1.01371
0.403563,19.3393,-2.5347,0.242553,0.699216,-5.9342,16.4834,1.37798,0.580524,-5.35976,-18.3814,-1.06229
0.67288,21.8562,-2.5679,0.296044,0.599379,-7.35141,18.1235,1.3743,0.494246,-6.48339,-20.1873,-1.11886
0.922843,24.7284,-2.55783,0.349529,0.531735,-8.90375,19.9965,1.36694,0.388911,-7.81896,-22.1709,-1.18358
1.1336,27.8458,-2.50126,0.400465,0.501518,-10.5254,22.0126,1.35587,0.281578,-9.25905,-24.2356,-1.25539
1.29226,30.8915,-2.38076,0.446209,0.490881,-12.0352,23.9147,1.34092,0.199666,-10.5438,-25.9795,-1.32977
1.36998,33.792,-2.21623,0.484842,0.485153,-13.3651,25.6221,1.32316,0.173093,-11.4765,-27.3925,-1.40231
1.31453,28.8022,-2.071,0.516715,0.491719,-12.7387,22.8726,1.30584,0.244256,-10.4882,-24.4714,-1.46809
1.15633,24.9841,-1.96334,0.544791,0.473661,-12.0959,20.7059,1.28018,0.399875,-9.74619,-22.2987,-1.52755
0.955328,22.1624,-1.88617,0.570202,0.439707,-11.4977,19.0758,1.24769,0.587012,-9.2352,-20.759,-1.58208
0.747628,20.2175,-1.83461,0.598068,0.454092,-10.9536,17.9338,1.21821,0.717563,-8.94585,-19.77,-1.63835
0.622608,18.7634,-1.68585,0.62991,0.476538,-10.4049,17.1073,1.19722,0.730425,-8.77747,-19.0893,-1.69602
0.536539,17.7536,-1.45636,0.659421,0.487648,-9.9009,16.5838,1.18907,0.67877,-8.74761,-18.6882,-1.75655
0.444922,16.9981,-1.18868,0.69016,0.486909,-9.41325,16.2286,1.19668,0.624399,-8.74973,-18.4123,-1.81869
0.325948,16.2286,-0.923376,0.73307,0.496099,-8.91306,15.8644,1.22152,0.579225,-8.70271,-18.0939,-1.88038
0.20445,15.3859,-0.679033,0.791203,0.502148,-8.40697,15.4393,1.26232,0.54228,-8.58316,-17.7004,-1.93967
0.117468,14.4563,-0.463841,0.862178,0.500059,-7.91033,14.9567,1.31771,0.488341,-8.40754,-17.2232,-1.99747
0.0715397,13.4271,-0.285737,0.94378,0.471143,-7.42136,14.3781,1.385,0.434971,-8.16013,-16.6507,-2.05198
0.0616931,12.2846,-0.156039,1.03212,0.432033,-6.95487,13.7019,1.46176,0.373742,-7.85937,-15.9864,-2.1027
0.07837,11.0141,-0.0795821,1.12398,0.388543,-6.50174,12.9049,1.54529,0.310685,-7.51307,-15.2192,-2.1503
0.111988,9.64412,-0.0580199,1.21668,0.344238,-6.07479,11.9999,1.63319,0.252229,-7.11734,-14.3724,-2.19186
0.154301,8.20589,-0.0848949,1.30802,0.302357,-5.68447,11.012,1.72272,0.204104,-6.69778,-13.4698,-2.22871
0.201579,6.71169,-0.152777,1.39622,0.254241,-5.32233,9.94445,1.81025,0.177966,-6.28119,-12.5146,-2.26231
0.244372,5.19469,-0.256733,1.47983,0.210091,-4.99994,8.82262,1.89367,0.171328,-5.87616,-11.5364,-2.29219
0.274807,3.64755,-0.395755,1.55774,0.176548,-4.70696,7.634,1.97218,0.184018,-5.48348,-10.5309,-2.31839
0.299071,2.07704,-0.561362,1.62882,0.154047,-4.43701,6.39003,2.04512,0.206971,-5.10594,-9.50294,-2.34069
0.324446,0.510855,-0.745511,1.69156,0.146022,-4.20392,5.12005,2.11119,0.227269,-4.75908,-8.47177,-2.35978
0.365686,-1.04616,-0.937473,1.74532,0.155884,-3.92127,3.85519,2.17304,0.218943,-4.38488,-7.41354,-2.37014
0.387074,-2.57193,-1.12508,1.78834,0.181689,-3.54567,2.64643,2.23135,0.211423,-3.96518,-6.32787,-2.37087
0.3813,-4.0916,-1.31247,1.81921,0.210909,-3.07195,1.46504,2.28542,0.223785,-3.49164,-5.20658,-2.36261
0.353584,-5.6626,-1.48575,1.83488,0.242645,-2.50899,0.276491,2.33394,0.251439,-2.98829,-4.02013,-2.34877
0.316039,-7.37024,-1.63986,1.83063,0.277102,-1.84904,-0.968702,2.37555,0.280607,-2.42554,-2.73221,-2.32901
0.283572,-9.28786,-1.79413,1.81008,0.30415,-1.08168,-2.33001,2.41101,0.299502,-1.79841,-1.30294,-2.30459
0.263049,-11.2368,-1.97419,1.78349,0.318955,-0.268084,-3.708,2.44163,0.30682,-1.15622,0.160314,-2.27628
0.25514,-13.2225,-2.15903,1.74628,0.326487,0.552069,-5.09862,2.46953,0.29936,-0.506387,1.65564,-2.24487
0.250871,-15.3139,-2.33441,1.69498,0.325962,1.3618,-6.54351,2.49581,0.288181,0.153085,3.21043,-2.2116
0.25011,-17.4739,-2.48553,1.63059,0.319348,2.14792,-8.0221,2.52038,0.274521,0.811288,4.80272,-2.17702
0.247856,-19.6642,-2.60136,1.55526,0.30896,2.90496,-9.51548,2.54386,0.265333,1.46138,6.41132,-2.14156
0.243423,-21.8853,-2.66931,1.47365,0.296609,3.63917,-11.0281,2.56592,0.261505,2.11163,8.04563,-2.10569
0.239969,-24.0709,-2.68875,1.38738,0.284142,4.33554,-12.5193,2.58611,0.261087,2.74799,9.66392,-2.06925
0.236044,-26.2467,-2.65166,1.30165,0.273056,5.00906,-14.015,2.60505,0.263871,3.40208,11.2948,-2.03133
0.231538,-28.4067,-2.56715,1.21685,0.264514,5.66346,-15.5103,2.62242,0.269665,4.08106,12.9347,-1.99111
0.22511,-22.5063,-2.45934,1.13547,0.259216,4.31627,-12.4847,2.63747,0.278102,2.80809,10.0512,-1.94939
0.220013,-18.0324,-2.34956,1.05781,0.258467,3.40441,-10.4124,2.65092,0.282634,2.01382,8.11409,-1.90735
0.218367,-14.6126,-2.25025,0.985706,0.261441,2.78593,-9.01527,2.66208,0.280721,1.56373,6.83797,-1.86462
0.222993,-11.9789,-2.15595,0.919244,0.263796,2.36569,-8.09903,2.67097,0.275674,1.36333,6.02733,-1.82206
0.231094,-9.93616,-2.05906,0.858413,0.267372,2.07642,-7.51446,2.67739,0.268962,1.33568,5.53658,-1.77973
0.239361,-8.31469,-1.9529,0.802957,0.271272,1.85854,-7.14053,2.68116,0.264466,1.42372,5.24069,-1.73885
0.244829,-6.98813,-1.8317,0.753017,0.275097,1.65992,-6.87716,2.68156,0.264102,1.53271,5.05604,-1.70299
0.248602,-5.85651,-1.69025,0.708296,0.27933,1.46136,-6.64018,2.67953,0.265813,1.60611,4.91787,-1.67285
0.25137,-4.8825,-1.52723,0.668497,0.282858,1.31227,-6.40168,2.67668,0.267605,1.67169,4.81754,-1.64693
0.253152,-4.04203,-1.35393,0.633348,0.285046,1.2249,-6.16602,2.67294,0.27049,1.73554,4.74205,-1.62401
0.255378,-3.31888,-1.17297,0.603449,0.287714,1.19707,-5.94057,2.66693,0.272616,1.79771,4.68552,-1.6043
This diff is collapsed.
......@@ -341,7 +341,7 @@ Eigen::MatrixXf readMatrix(std::string filename);
std::string getcwd_string( void );
void distributed_control_command( Controller::Request &request, Controller::Response &response );
void centralized_control_command( Controller::Request &request, Controller::Response &response);
void display_control_agent1( Controller::Request &request, Controller::Response &response) ;
std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load , float roll, float pitch, float yaw );
void desired_eulerrates_to_bodyrates( Controller::Request &request , float roll_rate , float pitch_rate, float (&bodyrates)[2]);
......
......@@ -679,22 +679,6 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
// response.controlOutput.onboardControllerType = ANGLE_MODE;
}
/*void write_vector_to_csv(std::ofstream filehandle, int len, std::vector<float> x)
{
for(int i=0 ; i < len ; i++)
{
if (i < len-1)
{
filehandle<< x(i) << ",";
}
else
{
filehandle<< x(i) ;
}
}
filehandle<< "\n" ;
return;
}*/
void desired_eulerrates_to_bodyrates( Controller::Request &request , float roll_rate , float pitch_rate, float (&bodyrates)[2])
{
......@@ -712,98 +696,6 @@ void desired_eulerrates_to_bodyrates( Controller::Request &request , float roll_
}
void display_control_agent1( Controller::Request &request, Controller::Response &response)
{
/*
// Control output
Eigen::Matrix<float,12,1> u = centralized_controller.K * estimator_state_prev_only_agent1;
// Estimator update
estimator_state_prev_only_agent1 = centralized_controller.Phi * estimator_state_prev_only_agent1 + centralized_controller.Gamma * y_only_agent_1;
// extract motor command from u:
float thrustSum = 0.0;
float outRoll = 0.0;
float outPitch = 0.0;
float outYaw = 0.0;
if (my_agentID == 1 )
{
// Controller for crazyflie 01
thrustSum = u(0,0);
outRoll = u(1,0);
outPitch = u(2,0);
outYaw = u(3,0);
}
// Debugging
if (outer_loop_counter % 50 == 0)
{
float est_error_roll = estimator_state_prev_only_agent1(0)-request.otherCrazyflies[0].roll;
float est_error_pitch = estimator_state_prev_only_agent1(1)-request.otherCrazyflies[0].pitch;
float est_error_yaw = estimator_state_prev_only_agent1(2)-request.otherCrazyflies[0].yaw;
ROS_INFO_STREAM("[STUDENT CONTROLLER] ESTIMATION ERRORS:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll = " << est_error_roll);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch = " << est_error_pitch);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw = " << est_error_yaw);
ROS_INFO_STREAM("[STUDENT CONTROLLER] ESTIMATES:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: roll estimate = " << estimator_state_prev_only_agent1(0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: pitch estimate = " << estimator_state_prev_only_agent1(1) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: yaw estimate = " <<estimator_state_prev_only_agent1(2) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi estimate = " << estimator_state_prev_only_agent1(3) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable theta estimate = " << estimator_state_prev_only_agent1(4) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi_dot estimate = " << estimator_state_prev_only_agent1(5) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable ttheta_dot estimate = " << estimator_state_prev_only_agent1(6) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] COMMANDS:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired yaw-rate computed = " << outYaw );
ROS_INFO_STREAM("[STUDENT CONTROLLER] errors:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter1 = " << y_only_agent_1(0,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter1 = " << y_only_agent_1(1,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter1 = " << y_only_agent_1(2,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter1 = " << y_only_agent_1(3,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter1 = " << y_only_agent_1(4,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter1 = " << y_only_agent_1(5,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter2 = " << y_only_agent_1(6,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter2 = " << y_only_agent_1(7,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter2 = " << y_only_agent_1(8,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter2 = " << y_only_agent_1(9,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter2 = " << y_only_agent_1(10,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter2 = " << y_only_agent_1(11,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter3 = " << y_only_agent_1(12,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter3 = " << y_only_agent_1(13,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter3 = " << y_only_agent_1(14,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter3 = " << y_only_agent_1(15,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter3 = " << y_only_agent_1(16,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter3 = " << y_only_agent_1(17,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for load = " << y_only_agent_1(18,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for load = " << y_only_agent_1(19,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for load = " << y_only_agent_1(20,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for load = " << y_only_agent_1(21,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for load = " << y_only_agent_1(22,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for load = " << y_only_agent_1(23,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_only_agent_1^T = " << y_only_agent_1.transpose() );
}
// PREPARE AND RETURN THE VARIABLE "response"
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.yaw = outYaw;
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
// choosing the Crazyflie onBoard controller type.
// it can either be Motor, Rate or Angle based
// response.controlOutput.onboardControllerType = MOTOR_MODE;
response.controlOutput.onboardControllerType = RATE_MODE;
// response.controlOutput.onboardControllerType = ANGLE_MODE; */
}
void get_y_from_CFData( Controller::Request &request )
{
......
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