Commit 5f4f4887 authored by Quirin Apfel's avatar Quirin Apfel
Browse files

Stand Do

parent a0e89b2a
......@@ -68,42 +68,32 @@ float pi_control(float e)
*/
float pid_control(float e)
{
float K = 0.4f;
static float K = 0.4f;
const float T_i = 0.1f;//0.18
const float T_d = 0.025;
float ee = e;
if(e > 10.0 || e < -10.0){
//K = 2.4;
ee = e*e*e;
/*if(e<0){
ee = -ee;
}*/
if(e < 0.5 && e > -0.5){
K = 0.4f;
}
if(e > 1.5 || e < -1.5){
if(e > 2.0 || e < -2.0){
K = 100.0;
}
static float e_old = 0;
/*
float ee = e*e;
if(e<0){
ee = -ee;
}
*/
sum_error += e/F_CONTROL;
float derivative = (e - e_old)*F_CONTROL;
float new_out = K*(ee + sum_error/T_i + T_d*derivative);
float new_out = K*(e + sum_error/T_i + T_d*derivative);
if(new_out>OUT_MAX){
new_out = OUT_MAX;
sum_error -= e/F_CONTROL;
sum_error = OUT_MAX/300.0;
}else if(new_out<OUT_MIN){
new_out = OUT_MIN;
sum_error -= e/F_CONTROL;
sum_error = OUT_MIN/300.0;
}
return new_out;
......
224,01.369096,-0.400222,-0.91005584276,1.846543,0.942433,2.23650366,9242838,0,2.126537,0.662298,1.838.1360.31.10,3.553530,-0.764727,-1.,23,49,71,2.780,2.788769,2.870379,-688021.,,22..3167039,2.185300,0.602448,1.52,234 55573.068,-.7,497.78436,22.78326252.,16954348,-0.050290-5100.3664826,-0220427096,-0.639735,-1.525014872.3,2.186409,0.600652,1.52971675471,-0393373184095876280.3852207,0243260392,2.478396,0.307997,00.0222.78631546.36, 42 ,18092,663600314,2.59,2.153950,0.632409,1.49979737892.25205 5 402422526.3,227,741412,1.914592041,.785409,2.050942,0.73,2.4 771242.4879.4983,3.528429,-0.743099,-1.3,0.6,39 095395435312.733.86 8358,3952.785286,1.986757,0.798530,2.13275907,25,3079. 092.667339,4.741645,-2.074306,-100.0000002.667339,4.800632,-2.133293,-100.0000002.665225,4.830326,-2.162987,-102.608880,2.158116,0.450764,1.2869372.608880,2.125026,0.483854,1.3760292.608880,2.126125,0.482755,1.3830482.608880,2.145015,0.463865,1.3469882.608880,2.191061,0.417819,1.2448342.608880,2.251131,0.357749,1.1078212.608880,2.326437,0.282443,0.9327362.608880,2.425172,0.183708,0.6994472.609446,2.533212,0.075668,0.4416642.609446,2.650690,-0.041243,0.1602522.609446,2.764196,-0.154749,-0.1152572.609446,2.881580,-0.272133,-0.4024222.609446,3.000819,-0.391373,-0.6964242.609446,3.124006,-0.514560,-1.0023632.609446,3.236774,-0.627328,-1.2855532.609446,3.327202,-0.717756,-1.5169362.609446,3.386762,-0.777316,-1.6754272.609446,3.351821,-0.742375,-1.6064152.610030,3.238090,-0.628644,-1.3460342.610030,3.065614,-0.455585,-0.9398042.610030,2.876167,-0.266138,-0.4904532.610030,2.711238,-0.101209,-0.0966482.610030,2.550027,0.060003,0.2914592.610030,2.409077,0.200953,0.6337582.610030,2.273803,0.336227,0.9651412.610030,2.141429,0.468601,1.2922102.610030,2.028766,0.581264,1.5742272.610030,1.937675,0.672355,1.8062932.610629,1.898177,0.711853,1.9153252.610629,1.900310,0.710318,1.9258482.610629,1.934534,0.676094,1.8572322.610629,1.981493,0.629136,1.7571152.610629,2.007485,0.603144,1.7067972.610629,2.022633,0.587995,1.6822002.610629,2.028240,0.582388,1.6803912.610629,2.023984,0.586644,1.7023392.610629,2.049616,0.561012,1.6520422.610629,2.135411,0.475218,1.4556402.611243,2.261398,0.349231,1.1602552.611243,2.406188,0.205055,0.8183332.611243,2.536150,0.075093,0.5079282.611243,2.648720,-0.037477,0.2370102.611243,2.747217,-0.135974,-0.0021032.611243,2.845812,-0.234570,-0.2434232.611243,2.955626,-0.344383,-0.5138622.611243,3.092642,-0.481400,-0.8523312.611243,3.225060,-0.613817,-1.1824092.611243,3.338589,-0.727346,-1.4694262.611871,3.396414,-0.785172,-1.6239102.611871,3.359370,-0.747499,-1.5484452.611871,3.253965,-0.642093,-1.3083142.611871,3.094484,-0.482613,-0.9352132.611871,2.921566,-0.309695,-0.5264042.611871,2.769281,-0.157410,-0.1640682.611871,2.619023,-0.007152,0.1964092.611871,2.484528,0.127344,0.5217442.611871,2.364050,0.247821,0.8158472.611871,2.243876,0.367995,1.1116242.612513,2.147100,0.464771,1.3531822.612513,2.052734,0.559780,1.5923982.612513,2.005033,0.607481,1.7190302.612513,2.034166,0.578347,1.6606762.612513,2.074134,0.538379,1.5755212.612513,2.156087,0.456427,1.3879632.612513,2.268191,0.344323,1.1258002.612513,2.369936,0.242578,0.8864632.612513,2.467508,0.145005,0.6551902.612513,2.557378,0.055135,0.4406042.613168,2.655629,-0.043115,0.2039412.613168,2.771793,-0.158625,-0.0764542.613168,2.904470,-0.291302,-0.4007062.613168,3.050941,-0.437774,-0.7609932.613168,3.196759,-0.583591,-1.1226262.613168,3.329645,-0.716477,-1.4558822.613168,3.438372,-0.825204,-1.7333312.613168,3.498945,-0.885777,-1.8964232.613168,3.488174,-0.875007,-1.8880742.613168,3.431168,-0.818000,-1.7676172.613835,3.332764,-0.719597,-1.5458412.613835,3.248360,-0.634526,-1.3543612.613835,3.171248,-0.557413,-1.1804402.613835,3.097693,-0.483859,-1.0135862.613835,3.055701,-0.441867,-0.9216422.613835,3.025781,-0.411946,-0.8580732.613835,2.987634,-0.373800,-0.7739962.613835,2.930823,-0.316988,-0.6439902.613835,2.812902,-0.199067,-0.3649602.613835,2.656336,-0.042501,0.0099492.614512,2.479162,0.134672,0.4378582.614512,2.290695,0.323817,0.8982832.614512,2.117379,0.497134,1.3241852.614512,1.961940,0.652573,1.7102902.614512,1.841786,0.772726,2.0141132.614512,1.841749,0.772764,2.0296582.614512,1.908159,0.706353,1.8844012.614512,2.066652,0.547860,1.5149742.614512,2.215659,0.398854,1.1653362.614512,2.339328,0.275185,0.8740342.615201,2.436227,0.178286,0.6450422.615201,2.492842,0.122359,0.5132642.615201,2.526640,0.088561,0.4339202.615201,2.559042,0.056159,0.3572802.615201,2.585852,0.029349,0.2935232.615201,2.680870,-0.065669,0.0641652.615201,2.819058,-0.203857,-0.2715632.615201,2.985875,-0.370674,-0.6793382.615201,3.171070,-0.555869,-1.1349232.615201,3.320858,-0.705657,-1.5085282.603714,3.420262,-0.816548,-1.8855192.603727,3.464356,-0.860643,-2.0085592.603727,3.418548,-0.814821,-1.9148842.603727,3.325529,-0.721802,-1.7060752.603727,3.202570,-0.598844,-1.4229512.603727,3.107080,-0.503353,-1.2038402.603727,3.019231,-0.415504,-1.0013132.603727,2.977957,-0.374230,-0.9097402.603727,2.921515,-0.317788,-0.7806352.603727,2.887407,-0.283680,-0.7044492.603727,2.840356,-0.236629,-0.5962602.603740,2.756398,-0.152671,-0.3978142.603740,2.643852,-0.040113,-0.1284762.603740,2.495737,0.108002,0.2291602.603740,2.332788,0.270952,0.6256582.603740,2.164801,0.438938,1.0376052.603740,2.010335,0.593405,1.4201922.603740,1.871601,0.732139,1.7677972.603740,1.751091,0.852649,2.0740742.603740,1.692165,0.911574,2.2337262.603740,1.715879,0.887861,2.1945712.603752,1.805879,0.797860,1.9945262.603752,1.913925,0.689827,1.7490432.603752,2.005332,0.598420,1.5416342.603752,2.078837,0.524915,1.3757212.603752,2.132120,0.471631,1.2572732.603752,2.158240,0.445512,1.2034972.603752,2.161029,0.442723,1.2056582.603752,2.160779,0.442973,1.2151182.603752,2.180255,0.423496,1.1768432.603752,2.244772,0.358980,1.0291842.603763,2.369290,0.234462,0.7350302.603763,2.537062,0.066702,0.3337392.603763,2.695550,-0.091787,-0.0484702.603763,2.841467,-0.237704,-0.4034252.603763,2.963936,-0.360172,-0.7045522.603763,3.062587,-0.458824,-0.9504932.603763,3.141076,-0.537313,-1.1496112.603763,3.195525,-0.591762,-1.2921252.603763,3.206843,-0.603079,-1.3313482.603763,3.169152,-0.565389,-1.2521982.603775,3.091123,-0.487359,-1.0746752.603775,2.998754,-0.394979,-0.8608622.603775,2.890150,-0.286375,-0.6059402.603775,2.789560,-0.185785,-0.3682402.603775,2.688693,-0.084918,-0.1278572.603775,2.598309,0.005466,0.0891742.603775,2.503265,0.100510,0.3192902.603775,2.372862,0.230913,0.6368752.603775,2.227424,0.376351,0.9934532.603775,2.080837,0.522938,1.3557212.603786,1.951285,0.652490,1.6796952.603786,1.901284,0.702502,1.8137732.603786,1.923014,0.680772,1.7752372.603786,1.999433,0.604353,1.6039192.603786,2.109587,0.494199,1.3494342.603786,2.192195,0.411590,1.1594052.603786,2.274492,0.329294,0.9684802.603786,2.307562,0.296224,0.8950352.603786,2.335043,0.268743,0.8344562.603786,2.338657,0.265128,0.8310842.603796,2.383766,0.220020,0.7272242.603796,2.489538,0.114258,0.4756802.603796,2.640827,-0.037031,0.1118462.603796,2.819831,-0.216035,-0.3220842.603796,2.964069,-0.360273,-0.6754602.603796,3.105344,-0.501547,-1.0245502.603796,3.225164,-0.621368,-1.3245482.603796,3.313890,-0.710094,-1.5516922.603796,3.378307,-0.774511,-1.7217822.603796,3.379183,-0.775387,-1.7393922.603806,3.324182,-0.720385,-1.6217962.603806,3.225048,-0.621241,-1.3962762.603806,3.120152,-0.516346,-1.1548532.603806,3.010952,-0.407145,-0.9009142.603806,2.910432,-0.306625,-0.6658002.603806,2.815988,-0.212182,-0.4433792.603806,2.718957,-0.115151,-0.2128082.603806,2.616784,-0.012977,0.0321502.603806,2.498974,0.104832,0.3169902.603806,2.379374,0.224433,0.6085192.603816,2.257571,0.346236,0.9077722.603816,2.146963,0.456853,1.1823902.603816,2.052287,0.551529,1.4206432.603816,1.983007,0.620810,1.5993322.603816,1.983387,0.620429,1.6108282.603816,2.027121,0.576695,1.5174012.603816,2.116304,0.487512,1.3131102.603816,2.197386,0.406430,1.1266432.603816,2.281278,0.322538,0.9317532.603816,2.320475,0.283341,0.8433462.603826,2.375554,0.228262,0.7157222.603826,2.410196,0.193630,0.6364772.603826,2.449057,0.154768,0.5463052.603826,2.556132,0.047693,0.2902792.603826,2.675347,-0.071522,0.0027332.603826,2.834589,-0.230764,-0.3840642.603826,2.982595,-0.378770,-0.7468542.603826,3.112545,-0.508719,-1.0689072.603826,3.237023,-0.633198,-1.3803192.603826,3.323755,-0.719929,-1.6028742.603834,3.395297,-0.791472,-1.7904052.603834,3.414918,-0.811084,-1.853696
\ No newline at end of file
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