# Measured with same wheel diam
# Kp = 10
# timestamp yaw yawGoal motorInput
pid_response3=[
[23181,0.7701,0.0000,-1.2092],
[23221,0.7688,0.0000,-1.2092],
[23262,0.7507,0.0000,-1.2092],
[23302,0.6719,0.0000,-1.2092],
[23342,0.5807,0.0000,-1.2092],
[23382,0.4882,0.0000,-1.2092],
[23421,0.3923,0.0000,-1.2092],
[23461,0.2989,0.0000,-1.2092],
[23501,0.2044,0.0000,-1.2092],
[23541,0.1092,0.0000,-1.0921],
[23582,0.0135,0.0000,-0.1353],
[23622,-0.0713,0.0000,0.7126],
[23662,-0.1135,0.0000,1.1348],
[23702,-0.0840,0.0000,0.8396],
[23742,-0.0103,0.0000,0.1034],
[23782,0.0580,0.0000,-0.5798],
[23822,0.1010,0.0000,-1.0103],
[23862,0.0638,0.0000,-0.6379],
[23903,-0.0105,0.0000,0.1045],
[23943,-0.0663,0.0000,0.6630],
[23983,-0.0660,0.0000,0.6598],
[24023,-0.0239,0.0000,0.2386],
[24062,0.0216,0.0000,-0.2157],
[24102,0.0542,0.0000,-0.5421],
[24142,0.0377,0.0000,-0.3774],
[24182,-0.0026,0.0000,0.0255],
[24223,-0.0291,0.0000,0.2908],
[24263,-0.0280,0.0000,0.2800],
[24303,-0.0137,0.0000,0.1371],
[24343,0.0070,0.0000,-0.0704],
[24383,0.0172,0.0000,-0.1720],
[24423,0.0157,0.0000,-0.1574],
[24463,0.0046,0.0000,-0.0462],
[24504,-0.0080,0.0000,0.0797],
[24544,-0.0123,0.0000,0.1226],
[24584,-0.0113,0.0000,0.1129],
[24624,-0.0055,0.0000,0.0555],
[24664,0.0017,0.0000,-0.0167],
[24704,0.0042,0.0000,-0.0423],
[24743,0.0039,0.0000,-0.0387],
[24783,0.0028,0.0000,-0.0276],
[24823,0.0013,0.0000,-0.0130],
[24863,-0.0003,0.0000,0.0030],
[24904,-0.0009,0.0000,0.0091],
[24944,-0.0007,0.0000,0.0067],
[24984,-0.0007,0.0000,0.0067],
[25024,-0.0006,0.0000,0.0055],
[25064,-0.0006,0.0000,0.0055],
[25104,-0.0006,0.0000,0.0055],
[25144,-0.0006,0.0000,0.0056],
[25185,-0.0006,0.0000,0.0056],
[25225,-0.0006,0.0000,0.0056],
[25265,-0.0002,0.0000,0.0020],
[25305,-0.0001,0.0000,0.0008],
[25345,-0.0002,0.0000,0.0020],
[25384,-0.0002,0.0000,0.0021],
[25424,-0.0003,0.0000,0.0033],
[25464,-0.0003,0.0000,0.0033],
[25505,-0.0003,0.0000,0.0033],
[25545,-0.0002,0.0000,0.0021],
[25585,-0.0002,0.0000,0.0021],
[25625,-0.0002,0.0000,0.0021],
[25665,-0.0002,0.0000,0.0021],
[25705,-0.0002,0.0000,0.0021],
[25745,-0.0002,0.0000,0.0021],
[25785,-0.0003,0.0000,0.0034],
[25826,-0.0003,0.0000,0.0034],
[25866,-0.0003,0.0000,0.0034],
[25906,-0.0003,0.0000,0.0034],
[25946,-0.0003,0.0000,0.0034],
[25986,-0.0003,0.0000,0.0034],
[26025,-0.0005,0.0000,0.0047],
[26065,-0.0005,0.0000,0.0047],
[26106,-0.0005,0.0000,0.0047],
[26146,-0.0005,0.0000,0.0046],
[26186,-0.0006,0.0000,0.0058],
[26226,-0.0006,0.0000,0.0059],
[26266,-0.0006,0.0000,0.0059],
[26306,-0.0006,0.0000,0.0059],
[26346,-0.0006,0.0000,0.0059],
[26386,-0.0006,0.0000,0.0059],
[26427,-0.0006,0.0000,0.0058],
[26467,-0.0006,0.0000,0.0059],
[26507,-0.0006,0.0000,0.0058],
[26547,-0.0006,0.0000,0.0058],
[26587,-0.0007,0.0000,0.0070],
[26627,-0.0007,0.0000,0.0070],
[26667,-0.0007,0.0000,0.0070],
[26706,-0.0007,0.0000,0.0070],
[26746,-0.0007,0.0000,0.0070],
[26787,-0.0007,0.0000,0.0070],
[26827,-0.0007,0.0000,0.0070],
[26867,-0.0006,0.0000,0.0058],
[26907,-0.0007,0.0000,0.0070],
[26947,-0.0006,0.0000,0.0058],
[26987,-0.0006,0.0000,0.0058],
[27027,-0.0006,0.0000,0.0059],
[27068,-0.0006,0.0000,0.0059],
[27108,-0.0006,0.0000,0.0059],
[27148,-0.0007,0.0000,0.0072],
[27188,-0.0007,0.0000,0.0072],
[27228,-0.0006,0.0000,0.0060],
[27268,-0.0006,0.0000,0.0060],
[27308,-0.0006,0.0000,0.0060],
[27347,-0.0005,0.0000,0.0047],
[27387,-0.0005,0.0000,0.0047],
[27428,-0.0005,0.0000,0.0047],
[27468,-0.0005,0.0000,0.0047],
[27508,-0.0003,0.0000,0.0035],
[27548,-0.0003,0.0000,0.0035],
[27588,-0.0003,0.0000,0.0035],
[27628,-0.0003,0.0000,0.0034],
[27668,-0.0003,0.0000,0.0034],
[27709,-0.0004,0.0000,0.0035],
[27749,-0.0004,0.0000,0.0035],
[27789,-0.0002,0.0000,0.0023],
[27829,-0.0002,0.0000,0.0023],
[27869,-0.0002,0.0000,0.0023],
[27909,-0.0002,0.0000,0.0023],
[27949,-0.0004,0.0000,0.0036],
[27988,-0.0002,0.0000,0.0023],
[28029,-0.0001,0.0000,0.0011],
[28069,-0.0001,0.0000,0.0011],
[28109,-0.0001,0.0000,0.0012],
[28149,-0.0001,0.0000,0.0012],
[28189,0.0000,0.0000,-0.0001],
[28229,0.0000,0.0000,-0.0001],
[28269,0.0000,0.0000,-0.0001],
[28309,0.0000,0.0000,-0.0001],
[28349,0.0000,0.0000,-0.0001],
[28390,0.0000,0.0000,-0.0001],
[28430,0.0000,0.0000,-0.0001],
[28470,0.0000,0.0000,-0.0001],
[28510,0.0000,0.0000,-0.0001],
[28550,0.0000,0.0000,-0.0001],
[28590,0.0000,0.0000,-0.0001],
[28630,0.0000,0.0000,-0.0001],
[28669,0.0000,0.0000,-0.0001],
[28710,0.0000,0.0000,-0.0001],
[28750,0.0000,0.0000,-0.0001],
[28790,0.0000,0.0000,-0.0001],
[28830,0.0000,0.0000,-0.0000],
[28870,0.0000,0.0000,-0.0000],
[28910,0.0000,0.0000,-0.0001],
[28951,0.0001,0.0000,-0.0013],
[28991,0.0001,0.0000,-0.0013],
[29031,-0.0006,0.0000,0.0060],
[29071,-0.0011,0.0000,0.0110],
[29111,-0.0010,0.0000,0.0097],
[29151,-0.0010,0.0000,0.0097],
[29191,-0.0010,0.0000,0.0097],
[29231,-0.0010,0.0000,0.0097],
[29271,-0.0010,0.0000,0.0097],
[29312,-0.0010,0.0000,0.0097],
[29351,-0.0010,0.0000,0.0097],
[29391,-0.0010,0.0000,0.0097],
[29431,-0.0010,0.0000,0.0096],
[29471,-0.0010,0.0000,0.0096],
[29511,-0.0010,0.0000,0.0096],
[29551,-0.0010,0.0000,0.0096],
[29591,-0.0010,0.0000,0.0096],
[29632,-0.0010,0.0000,0.0096],
[29672,-0.0010,0.0000,0.0096],
[29712,-0.0008,0.0000,0.0084],
[29752,-0.0008,0.0000,0.0084],
[29792,-0.0008,0.0000,0.0084],
[29832,-0.0008,0.0000,0.0084],
[29872,-0.0008,0.0000,0.0084],
[29913,-0.0008,0.0000,0.0084],
[29953,-0.0008,0.0000,0.0084],
[29992,-0.0008,0.0000,0.0084],
[30032,-0.0008,0.0000,0.0084],
[30072,-0.0008,0.0000,0.0084],
[30112,-0.0008,0.0000,0.0084],
[30152,-0.0008,0.0000,0.0084],
[30192,-0.0008,0.0000,0.0084],
[30233,-0.0008,0.0000,0.0084],
[30273,-0.0007,0.0000,0.0072],
[30313,-0.0007,0.0000,0.0072],
[30353,-0.0007,0.0000,0.0072],
[30393,-0.0007,0.0000,0.0072],
[30433,-0.0007,0.0000,0.0072],
[30473,-0.0007,0.0000,0.0072],
[30513,-0.0007,0.0000,0.0072],
[30553,-0.0007,0.0000,0.0072],
[30594,-0.0007,0.0000,0.0072],
[30633,-0.0007,0.0000,0.0072],
[30673,-0.0007,0.0000,0.0072],
[30713,-0.0007,0.0000,0.0073],
[30753,-0.0007,0.0000,0.0072],
[30793,-0.0006,0.0000,0.0060],
[30833,-0.0006,0.0000,0.0060],
[30873,-0.0006,0.0000,0.0060],
[30914,-0.0006,0.0000,0.0060],
[30954,-0.0006,0.0000,0.0060],
[30994,-0.0006,0.0000,0.0060],
[31034,-0.0006,0.0000,0.0060],
[31074,-0.0006,0.0000,0.0060],
[31114,-0.0005,0.0000,0.0048],
[31154,-0.0005,0.0000,0.0048],
[31195,-0.0005,0.0000,0.0048],
[31235,-0.0005,0.0000,0.0048],
[31275,-0.0005,0.0000,0.0048],
[31314,-0.0005,0.0000,0.0049],
[31354,-0.0005,0.0000,0.0048],
[31394,-0.0004,0.0000,0.0036],
[31434,-0.0004,0.0000,0.0036],
[31474,-0.0004,0.0000,0.0036],
[31514,-0.0004,0.0000,0.0036],
[31555,-0.0004,0.0000,0.0036],
[31595,-0.0004,0.0000,0.0036],
[31635,-0.0004,0.0000,0.0036],
[31675,-0.0004,0.0000,0.0036],
[31715,-0.0004,0.0000,0.0036],
[31755,-0.0004,0.0000,0.0036],
[31796,-0.0002,0.0000,0.0024],
[31835,-0.0002,0.0000,0.0024],
[31876,-0.0002,0.0000,0.0024],
[31916,-0.0002,0.0000,0.0024],
[31955,-0.0002,0.0000,0.0024],
[31995,-0.0002,0.0000,0.0024],
[32035,-0.0002,0.0000,0.0024],
[32075,-0.0002,0.0000,0.0024],
[32116,-0.0002,0.0000,0.0024],
[32156,-0.0002,0.0000,0.0024],
[32196,-0.0002,0.0000,0.0024],
[32236,-0.0002,1.5708,1.2092],
[32276,0.0004,1.5708,1.2092],
[32316,0.0622,1.5708,1.2092],
[32356,0.1462,1.5708,1.2092],
[32396,0.2369,1.5708,1.2092],
[32436,0.3302,1.5708,1.2092],
[32477,0.4253,1.5708,1.2092],
[32517,0.5199,1.5708,1.2092],
[32557,0.6116,1.5708,1.2092],
[32596,0.7043,1.5708,1.2092],
[32636,0.7975,1.5708,1.2092],
[32676,0.8925,1.5708,1.2092],
[32716,0.9880,1.5708,1.2092],
[32757,1.0818,1.5708,1.2092],
[32796,1.1775,1.5708,1.2092],
[32837,1.2741,1.5708,1.2092],
[32877,1.3699,1.5708,1.2092],
[32917,1.4657,1.5708,1.0510],
[32957,1.5618,1.5708,0.0898],
[32997,1.6521,1.5708,-0.8132],
[33037,1.6913,1.5708,-1.2048],
[33077,1.6556,1.5708,-0.8478],
[33118,1.5700,1.5708,0.0084],
[33158,1.4965,1.5708,0.7428],
[33198,1.4879,1.5708,0.8292],
[33238,1.5360,1.5708,0.3484],
[33277,1.5948,1.5708,-0.2405],
[33317,1.6441,1.5708,-0.7329],
[33357,1.6324,1.5708,-0.6158],
[33398,1.5753,1.5708,-0.0449],
[33437,1.5233,1.5708,0.4754],
[33478,1.4882,1.5708,0.8257],
[33518,1.5121,1.5708,0.5873],
[33558,1.5684,1.5708,0.0242],
[33598,1.6198,1.5708,-0.4897],
[33638,1.6529,1.5708,-0.8209],
[33678,1.6204,1.5708,-0.4963],
[33719,1.5573,1.5708,0.1352],
[33759,1.5064,1.5708,0.6444],
[33799,1.5081,1.5708,0.6266],
[33839,1.5507,1.5708,0.2014],
[33879,1.5958,1.5708,-0.2505],
[33918,1.6315,1.5708,-0.6072],
[33958,1.6165,1.5708,-0.4574],
[33998,1.5687,1.5708,0.0209],
[34038,1.5329,1.5708,0.3785],
[34079,1.5315,1.5708,0.3925],
[34119,1.5555,1.5708,0.1527],
[34159,1.5845,1.5708,-0.1375],
[34199,1.6043,1.5708,-0.3355],
[34239,1.5970,1.5708,-0.2622],
[34279,1.5699,1.5708,0.0089],
[34319,1.5482,1.5708,0.2260],
[34359,1.5480,1.5708,0.2277],
[34400,1.5555,1.5708,0.1528],
[34440,1.5737,1.5708,-0.0291],
[34480,1.5868,1.5708,-0.1597],
[34520,1.5866,1.5708,-0.1579],
[34560,1.5770,1.5708,-0.0617],
[34599,1.5649,1.5708,0.0591],
[34639,1.5597,1.5708,0.1111],
[34680,1.5600,1.5708,0.1076],
[34720,1.5680,1.5708,0.0280],
[34760,1.5763,1.5708,-0.0549],
[34800,1.5799,1.5708,-0.0910],
[34840,1.5797,1.5708,-0.0892],
[34880,1.5721,1.5708,-0.0134],
[34920,1.5659,1.5708,0.0488],
[34960,1.5649,1.5708,0.0591],
[35001,1.5652,1.5708,0.0557],
[35041,1.5692,1.5708,0.0159],
[35081,1.5715,1.5708,-0.0065],
[35121,1.5721,1.5708,-0.0134],
[35161,1.5720,1.5708,-0.0117],
[35201,1.5713,1.5708,-0.0048],
[35240,1.5699,0.7854,-1.2092],
[35280,1.5661,0.7854,-1.2092],
[35320,1.4857,0.7854,-1.2092],
[35361,1.3938,0.7854,-1.2092],
[35401,1.2976,0.7854,-1.2092],
[35441,1.2007,0.7854,-1.2092],
[35481,1.1031,0.7854,-1.2092],
[35521,1.0054,0.7854,-1.2092],
[35561,0.9068,0.7854,-1.2092],
[35601,0.8100,0.7854,-0.2463],
[35642,0.7174,0.7854,0.6798],
[35682,0.6697,0.7854,1.1574],
[35722,0.7039,0.7854,0.8147],
[35762,0.7792,0.7854,0.0617],
[35802,0.8426,0.7854,-0.5720],
[35842,0.8811,0.7854,-0.9574],
[35881,0.8400,0.7854,-0.5457],
[35921,0.7702,0.7854,0.1523],
[35961,0.7143,0.7854,0.7112],
[36002,0.7198,0.7854,0.6555],
[36042,0.7667,0.7854,0.1874],
[36082,0.8124,0.7854,-0.2698],
[36122,0.8484,0.7854,-0.6298],
[36162,0.8258,0.7854,-0.4039],
[36202,0.7773,0.7854,0.0811],
[36242,0.7430,0.7854,0.4235],
[36282,0.7460,0.7854,0.3944],
[36323,0.7751,0.7854,0.1027],
[36363,0.8037,0.7854,-0.1832],
[36403,0.8218,0.7854,-0.3637],
[36443,0.8049,0.7854,-0.1946],
[36483,0.7758,0.7854,0.0958],
[36523,0.7579,0.7854,0.2753],
[36562,0.7602,0.7854,0.2520],
[36602,0.7784,0.7854,0.0699],
[36643,0.7971,0.7854,-0.1165],
[36683,0.8059,0.7854,-0.2050],
[36723,0.8010,0.7854,-0.1557],
[36763,0.7836,0.7854,0.0182],
[36803,0.7724,0.7854,0.1295],
[36843,0.7732,0.7854,0.1222],
[36884,0.7796,0.7854,0.0584],
[36924,0.7874,0.7854,-0.0201],
[36964,0.7886,0.7854,-0.0323],
[37004,0.7886,0.7854,-0.0323],
[37044,0.7882,0.7854,-0.0279],
[37084,0.7861,0.7854,-0.0071],
[37124,0.7837,0.7854,0.0169],
[37164,0.7813,0.7854,0.0415],
[37204,0.7818,0.7854,0.0357],
[37244,0.7845,0.7854,0.0088],
[37284,0.7860,0.7854,-0.0063],
[37324,0.7866,0.7854,-0.0120],
[37364,0.7864,0.7854,-0.0100],
[37404,0.7839,0.7854,0.0152],
[37444,0.7828,0.7854,0.0255],
[37484,0.7833,0.7854,0.0206],
[37524,0.7844,0.7854,0.0096],
[37565,0.7860,0.7854,-0.0063],
[37605,0.7862,0.7854,-0.0084],
[37645,0.7857,0.7854,-0.0027],
[37685,0.7831,0.7854,0.0234],
[37725,0.7831,0.7854,0.0234],
[37765,0.7844,0.7854,0.0104],
[37805,0.7850,0.7854,0.0039],
[37844,0.7851,0.7854,0.0031],
[37885,0.7851,0.7854,0.0031],
[37925,0.7852,0.7854,0.0018],
[37965,0.7853,0.7854,0.0010],
[38005,0.7853,0.7854,0.0010],
[38045,0.7853,0.7854,0.0010],
[38085,0.7853,0.7854,0.0010],
[38125,0.7853,0.7854,0.0010],
[38166,0.7853,0.7854,0.0010],
[38206,0.7853,0.7854,0.0010]]

