# Measured with same wheel diam
# Kp = 5
# timestamp yaw yawGoal motorInput
pid_response2=[
[26746,0.7675,0.0000,-1.2092],
[26787,0.7672,0.0000,-1.2092],
[26827,0.7474,0.0000,-1.2092],
[26867,0.6710,0.0000,-1.2092],
[26907,0.5793,0.0000,-1.2092],
[26947,0.4869,0.0000,-1.2092],
[26987,0.3931,0.0000,-1.2092],
[27027,0.2989,0.0000,-1.2092],
[27067,0.2054,0.0000,-1.0269],
[27107,0.1113,0.0000,-0.5563],
[27148,0.0263,0.0000,-0.1316],
[27188,-0.0461,0.0000,0.2306],
[27228,-0.0923,0.0000,0.4617],
[27268,-0.0868,0.0000,0.4338],
[27308,-0.0559,0.0000,0.2794],
[27347,-0.0235,0.0000,0.1176],
[27387,0.0013,0.0000,-0.0064],
[27428,0.0162,0.0000,-0.0810],
[27468,0.0160,0.0000,-0.0798],
[27508,0.0108,0.0000,-0.0542],
[27548,0.0029,0.0000,-0.0145],
[27588,0.0001,0.0000,-0.0003],
[27628,-0.0015,0.0000,0.0076],
[27668,-0.0015,0.0000,0.0076],
[27708,-0.0015,0.0000,0.0076],
[27749,-0.0015,0.0000,0.0077],
[27789,-0.0015,0.0000,0.0077],
[27829,-0.0015,0.0000,0.0077],
[27869,-0.0015,0.0000,0.0077],
[27909,-0.0015,0.0000,0.0077],
[27949,-0.0015,0.0000,0.0077],
[27988,-0.0015,0.0000,0.0077],
[28028,-0.0017,0.0000,0.0083],
[28069,-0.0017,0.0000,0.0084],
[28109,-0.0017,0.0000,0.0084],
[28149,-0.0017,0.0000,0.0084],
[28189,-0.0017,0.0000,0.0084],
[28229,-0.0017,0.0000,0.0084],
[28269,-0.0017,0.0000,0.0084],
[28309,-0.0017,0.0000,0.0084],
[28349,-0.0017,0.0000,0.0084],
[28390,-0.0017,0.0000,0.0084],
[28430,-0.0017,0.0000,0.0084],
[28470,-0.0017,0.0000,0.0084],
[28510,-0.0018,0.0000,0.0091],
[28550,-0.0018,0.0000,0.0091],
[28590,-0.0018,0.0000,0.0091],
[28630,-0.0018,0.0000,0.0091],
[28669,-0.0018,0.0000,0.0091],
[28710,-0.0018,0.0000,0.0091],
[28750,-0.0018,0.0000,0.0091],
[28790,-0.0018,0.0000,0.0091],
[28830,-0.0018,0.0000,0.0091],
[28870,-0.0018,0.0000,0.0091],
[28910,-0.0018,0.0000,0.0091],
[28950,-0.0018,0.0000,0.0091],
[28990,-0.0018,0.0000,0.0091],
[29031,-0.0018,0.0000,0.0091],
[29071,-0.0018,0.0000,0.0091],
[29111,-0.0018,0.0000,0.0092],
[29151,-0.0018,0.0000,0.0092],
[29191,-0.0018,0.0000,0.0092],
[29231,-0.0018,0.0000,0.0092],
[29272,-0.0018,0.0000,0.0092],
[29312,-0.0018,0.0000,0.0092],
[29351,-0.0018,0.0000,0.0092],
[29391,-0.0018,0.0000,0.0092],
[29431,-0.0018,0.0000,0.0092],
[29471,-0.0020,0.0000,0.0098],
[29511,-0.0020,0.0000,0.0098],
[29551,-0.0020,0.0000,0.0098],
[29591,-0.0020,0.0000,0.0098],
[29632,-0.0020,0.0000,0.0098],
[29672,-0.0020,0.0000,0.0098],
[29712,-0.0020,0.0000,0.0098],
[29752,-0.0020,0.0000,0.0098],
[29792,-0.0020,0.0000,0.0098],
[29832,-0.0020,0.0000,0.0098],
[29872,-0.0020,0.0000,0.0098],
[29912,-0.0020,0.0000,0.0098],
[29953,-0.0020,0.0000,0.0098],
[29992,-0.0020,0.0000,0.0098],
[30032,-0.0020,0.0000,0.0099],
[30072,-0.0020,0.0000,0.0099],
[30112,-0.0020,0.0000,0.0099],
[30152,-0.0020,0.0000,0.0099],
[30192,-0.0020,0.0000,0.0099],
[30233,-0.0020,0.0000,0.0099],
[30273,-0.0020,0.0000,0.0099],
[30313,-0.0020,0.0000,0.0099],
[30353,-0.0020,0.0000,0.0099],
[30393,-0.0020,0.0000,0.0099],
[30433,-0.0020,0.0000,0.0099],
[30473,-0.0020,0.0000,0.0099],
[30513,-0.0020,0.0000,0.0099],
[30553,-0.0020,0.0000,0.0099],
[30593,-0.0020,0.0000,0.0099],
[30633,-0.0020,0.0000,0.0099],
[30673,-0.0020,0.0000,0.0099],
[30713,-0.0020,0.0000,0.0099],
[30753,-0.0020,0.0000,0.0099],
[30793,-0.0020,0.0000,0.0099],
[30833,-0.0020,0.0000,0.0099],
[30874,-0.0020,0.0000,0.0099],
[30914,-0.0021,0.0000,0.0105],
[30954,-0.0021,0.0000,0.0105],
[30994,-0.0021,0.0000,0.0105],
[31034,-0.0021,0.0000,0.0105],
[31074,-0.0021,0.0000,0.0105],
[31114,-0.0021,0.0000,0.0105],
[31154,-0.0021,0.0000,0.0105],
[31194,-0.0021,0.0000,0.0105],
[31235,-0.0021,0.0000,0.0105],
[31275,-0.0021,0.0000,0.0105],
[31314,-0.0021,0.0000,0.0105],
[31354,-0.0021,0.0000,0.0105],
[31394,-0.0021,0.0000,0.0105],
[31434,-0.0021,0.0000,0.0105],
[31474,-0.0021,0.0000,0.0105],
[31514,-0.0022,0.0000,0.0111],
[31554,-0.0022,0.0000,0.0111],
[31595,-0.0022,0.0000,0.0111],
[31635,-0.0022,0.0000,0.0111],
[31675,-0.0022,0.0000,0.0111],
[31715,-0.0022,0.0000,0.0111],
[31755,-0.0022,0.0000,0.0111],
[31795,-0.0022,0.0000,0.0112],
[31836,-0.0022,0.0000,0.0111],
[31876,-0.0022,0.0000,0.0111],
[31916,-0.0022,0.0000,0.0112],
[31955,-0.0022,0.0000,0.0111],
[31995,-0.0022,0.0000,0.0111],
[32035,-0.0022,0.0000,0.0111],
[32075,-0.0022,0.0000,0.0111],
[32115,-0.0024,0.0000,0.0118],
[32156,-0.0024,0.0000,0.0118],
[32196,-0.0024,0.0000,0.0118],
[32236,-0.0024,0.0000,0.0118],
[32276,-0.0024,0.0000,0.0118],
[32316,-0.0024,0.0000,0.0118],
[32356,-0.0024,0.0000,0.0118],
[32396,-0.0024,0.0000,0.0118],
[32436,-0.0024,0.0000,0.0118],
[32477,-0.0024,0.0000,0.0118],
[32517,-0.0024,0.0000,0.0118],
[32557,-0.0022,0.0000,0.0112],
[32596,-0.0024,0.0000,0.0118],
[32636,-0.0024,0.0000,0.0118],
[32676,-0.0024,0.0000,0.0118],
[32716,-0.0024,0.0000,0.0118],
[32756,-0.0024,0.0000,0.0118],
[32796,-0.0024,0.0000,0.0118],
[32837,-0.0024,0.0000,0.0118],
[32877,-0.0024,0.0000,0.0118],
[32917,-0.0025,0.0000,0.0124],
[32957,-0.0025,0.0000,0.0124],
[32997,-0.0025,0.0000,0.0124],
[33037,-0.0025,0.0000,0.0124],
[33077,-0.0025,0.0000,0.0124],
[33118,-0.0025,0.0000,0.0124],
[33158,-0.0025,0.0000,0.0124],
[33198,-0.0025,0.0000,0.0124],
[33238,-0.0025,0.0000,0.0124],
[33277,-0.0025,0.0000,0.0124],
[33317,-0.0025,0.0000,0.0124],
[33357,-0.0025,0.0000,0.0124],
[33397,-0.0025,0.0000,0.0124],
[33438,-0.0025,0.0000,0.0124],
[33478,-0.0025,0.0000,0.0124],
[33518,-0.0025,0.0000,0.0124],
[33558,-0.0025,0.0000,0.0124],
[33598,-0.0026,0.0000,0.0130],
[33638,-0.0025,0.0000,0.0124],
[33678,-0.0025,0.0000,0.0124],
[33718,-0.0026,0.0000,0.0130],
[33759,-0.0026,0.0000,0.0130],
[33799,-0.0026,0.0000,0.0130],
[33839,-0.0026,0.0000,0.0130],
[33879,-0.0026,0.0000,0.0130],
[33918,-0.0026,0.0000,0.0130],
[33958,-0.0026,0.0000,0.0130],
[33998,-0.0026,0.0000,0.0130],
[34038,-0.0026,0.0000,0.0130],
[34078,-0.0026,0.0000,0.0130],
[34119,-0.0026,0.0000,0.0130],
[34159,-0.0026,0.0000,0.0130],
[34199,-0.0026,0.0000,0.0130],
[34239,-0.0026,0.0000,0.0130],
[34279,-0.0026,0.0000,0.0130],
[34319,-0.0026,0.0000,0.0130],
[34359,-0.0026,0.0000,0.0130],
[34399,-0.0026,0.0000,0.0130],
[34440,-0.0026,0.0000,0.0130],
[34480,-0.0026,0.0000,0.0130],
[34520,-0.0026,0.0000,0.0130],
[34560,-0.0026,0.0000,0.0130],
[34599,-0.0027,0.0000,0.0136],
[34639,-0.0027,0.0000,0.0136],
[34679,-0.0027,0.0000,0.0136],
[34720,-0.0027,0.0000,0.0136],
[34760,-0.0027,0.0000,0.0136],
[34800,-0.0027,0.0000,0.0136],
[34840,-0.0027,0.0000,0.0136],
[34880,-0.0028,0.0000,0.0142],
[34920,-0.0027,0.0000,0.0136],
[34960,-0.0028,0.0000,0.0142],
[35000,-0.0028,0.0000,0.0142],
[35040,-0.0028,0.0000,0.0142],
[35081,-0.0028,0.0000,0.0142],
[35121,-0.0028,0.0000,0.0142],
[35161,-0.0028,0.0000,0.0142],
[35201,-0.0028,0.0000,0.0142],
[35240,-0.0028,0.0000,0.0142],
[35280,-0.0028,0.0000,0.0142],
[35320,-0.0028,0.0000,0.0142],
[35361,-0.0028,0.0000,0.0142],
[35401,-0.0028,0.0000,0.0142],
[35441,-0.0030,0.0000,0.0148],
[35481,-0.0030,0.0000,0.0149],
[35521,-0.0030,0.0000,0.0148],
[35561,-0.0030,0.0000,0.0149],
[35601,-0.0030,0.0000,0.0149],
[35641,-0.0030,0.0000,0.0149],
[35681,-0.0030,0.0000,0.0148],
[35722,-0.0030,0.0000,0.0148],
[35762,-0.0030,0.0000,0.0148],
[35802,-0.0030,1.5708,1.2092],
[35842,-0.0015,1.5708,1.2092],
[35881,0.0691,1.5708,1.2092],
[35921,0.1553,1.5708,1.2092],
[35961,0.2471,1.5708,1.2092],
[36001,0.3399,1.5708,1.2092],
[36042,0.4348,1.5708,1.2092],
[36082,0.5294,1.5708,1.2092],
[36122,0.6236,1.5708,1.2092],
[36162,0.7172,1.5708,1.2092],
[36202,0.8106,1.5708,1.2092],
[36242,0.9049,1.5708,1.2092],
[36283,0.9997,1.5708,1.2092],
[36322,1.0946,1.5708,1.2092],
[36363,1.1904,1.5708,1.2092],
[36403,1.2863,1.5708,1.2092],
[36443,1.3830,1.5708,0.9390],
[36483,1.4768,1.5708,0.4699],
[36523,1.5604,1.5708,0.0518],
[36562,1.6287,1.5708,-0.2897],
[36602,1.6630,1.5708,-0.4612],
[36643,1.6404,1.5708,-0.3482],
[36683,1.6069,1.5708,-0.1807],
[36723,1.5782,1.5708,-0.0372],
[36763,1.5571,1.5708,0.0684],
[36803,1.5467,1.5708,0.1207],
[36843,1.5477,1.5708,0.1155],
[36883,1.5561,1.5708,0.0736],
[36924,1.5660,1.5708,0.0241],
[36963,1.5696,1.5708,0.0060],
[37004,1.5706,1.5708,0.0008],
[37044,1.5710,1.5708,-0.0009],
[37084,1.5710,1.5708,-0.0009],
[37124,1.5708,1.5708,-0.0000],
[37164,1.5708,1.5708,-0.0000],
[37203,1.5708,1.5708,-0.0000],
[37243,1.5708,1.5708,-0.0000],
[37283,1.5708,1.5708,-0.0000],
[37324,1.5708,1.5708,-0.0000],
[37364,1.5708,1.5708,-0.0000],
[37404,1.5708,1.5708,0.0000],
[37444,1.5708,1.5708,0.0000],
[37484,1.5708,1.5708,0.0000],
[37524,1.5708,1.5708,0.0000],
[37564,1.5708,1.5708,0.0000],
[37605,1.5708,1.5708,0.0000],
[37644,1.5708,1.5708,0.0000],
[37685,1.5706,1.5708,0.0009],
[37725,1.5706,1.5708,0.0009],
[37765,1.5706,1.5708,0.0009],
[37805,1.5706,1.5708,0.0009],
[37844,1.5706,1.5708,0.0009],
[37884,1.5706,1.5708,0.0009],
[37925,1.5706,1.5708,0.0009],
[37965,1.5706,1.5708,0.0009],
[38005,1.5706,1.5708,0.0010],
[38045,1.5706,1.5708,0.0010],
[38085,1.5706,1.5708,0.0010],
[38125,1.5706,1.5708,0.0010],
[38165,1.5706,1.5708,0.0010],
[38205,1.5706,1.5708,0.0010],
[38245,1.5706,1.5708,0.0010],
[38286,1.5706,1.5708,0.0010],
[38326,1.5706,1.5708,0.0010],
[38366,1.5704,1.5708,0.0019],
[38406,1.5704,1.5708,0.0019],
[38446,1.5704,1.5708,0.0019],
[38486,1.5704,1.5708,0.0019],
[38526,1.5704,1.5708,0.0019],
[38566,1.5704,1.5708,0.0019],
[38606,1.5704,1.5708,0.0019],
[38646,1.5704,1.5708,0.0019],
[38686,1.5704,1.5708,0.0019],
[38726,1.5704,1.5708,0.0019],
[38766,1.5704,1.5708,0.0019],
[38807,1.5704,0.7854,-1.2092],
[38846,1.5659,0.7854,-1.2092],
[38887,1.4974,0.7854,-1.2092],
[38927,1.4069,0.7854,-1.2092],
[38967,1.3117,0.7854,-1.2092],
[39007,1.2151,0.7854,-1.2092],
[39047,1.1202,0.7854,-1.2092],
[39087,1.0250,0.7854,-1.1980],
[39127,0.9275,0.7854,-0.7105],
[39168,0.8322,0.7854,-0.2338],
[39207,0.7525,0.7854,0.1647],
[39247,0.7101,0.7854,0.3763],
[39287,0.7172,0.7854,0.3411],
[39327,0.7405,0.7854,0.2246],
[39367,0.7650,0.7854,0.1022],
[39407,0.7858,0.7854,-0.0019],
[39447,0.7988,0.7854,-0.0672],
[39488,0.7994,0.7854,-0.0701],
[39528,0.7985,0.7854,-0.0654],
[39568,0.7940,0.7854,-0.0431],
[39608,0.7888,0.7854,-0.0170],
[39648,0.7870,0.7854,-0.0081],
[39688,0.7854,0.7854,0.0002],
[39728,0.7806,0.7854,0.0238],
[39769,0.7780,0.7854,0.0368],
[39808,0.7782,0.7854,0.0358],
[39848,0.7795,0.7854,0.0292],
[39888,0.7809,0.7854,0.0227],
[39928,0.7816,0.7854,0.0190],
[39968,0.7847,0.7854,0.0035],
[40008,0.7867,0.7854,-0.0063],
[40048,0.7870,0.7854,-0.0081],
[40089,0.7869,0.7854,-0.0073],
[40129,0.7867,0.7854,-0.0067],
[40169,0.7860,0.7854,-0.0030],
[40209,0.7856,0.7854,-0.0008],
[40249,0.7851,0.7854,0.0017],
[40289,0.7851,0.7854,0.0017],
[40329,0.7850,0.7854,0.0021],
[40369,0.7850,0.7854,0.0021],
[40409,0.7850,0.7854,0.0021],
[40450,0.7850,0.7854,0.0021],
[40489,0.7850,0.7854,0.0021],
[40529,0.7850,0.7854,0.0021],
[40569,0.7850,0.7854,0.0021],
[40609,0.7850,0.7854,0.0021],
[40649,0.7850,0.7854,0.0021],
[40689,0.7849,0.7854,0.0025],
[40729,0.7849,0.7854,0.0025],
[40770,0.7849,0.7854,0.0025],
[40810,0.7849,0.7854,0.0025],
[40850,0.7849,0.7854,0.0025],
[40890,0.7849,0.7854,0.0025],
[40930,0.7849,0.7854,0.0025],
[40970,0.7849,0.7854,0.0025],
[41010,0.7849,0.7854,0.0025],
[41050,0.7849,0.7854,0.0025],
[41091,0.7849,0.7854,0.0025],
[41131,0.7849,0.7854,0.0025],
[41170,0.7849,0.7854,0.0025],
[41210,0.7849,0.7854,0.0025],
[41250,0.7849,0.7854,0.0025],
[41290,0.7849,0.7854,0.0025],
[41330,0.7849,0.7854,0.0025],
[41371,0.7849,0.7854,0.0025],
[41411,0.7849,0.7854,0.0025],
[41451,0.7849,0.7854,0.0025],
[41491,0.7849,0.7854,0.0025],
[41531,0.7849,0.7854,0.0025],
[41571,0.7849,0.7854,0.0025],
[41611,0.7849,0.7854,0.0025],
[41651,0.7849,0.7854,0.0025],
[41692,0.7849,0.7854,0.0025],
[41732,0.7849,0.7854,0.0025],
[41772,0.7849,0.7854,0.0025]]

