# Measured with same wheel diam
# Kp = 1
# timestamp yaw yawGoal motorInput
pid_response1=[
[23421,0.7549,0.0000,-0.7549],
[23461,0.7547,0.0000,-0.7547],
[23501,0.7376,0.0000,-0.7376],
[23541,0.6797,0.0000,-0.6797],
[23582,0.6226,0.0000,-0.6226],
[23622,0.5656,0.0000,-0.5656],
[23662,0.5107,0.0000,-0.5107],
[23702,0.4611,0.0000,-0.4611],
[23742,0.4150,0.0000,-0.4150],
[23782,0.3749,0.0000,-0.3749],
[23822,0.3382,0.0000,-0.3382],
[23862,0.3064,0.0000,-0.3064],
[23903,0.2768,0.0000,-0.2768],
[23943,0.2522,0.0000,-0.2522],
[23983,0.2310,0.0000,-0.2310],
[24023,0.2110,0.0000,-0.2110],
[24062,0.1923,0.0000,-0.1923],
[24102,0.1763,0.0000,-0.1763],
[24142,0.1581,0.0000,-0.1581],
[24182,0.1447,0.0000,-0.1447],
[24223,0.1318,0.0000,-0.1318],
[24263,0.1195,0.0000,-0.1195],
[24303,0.1090,0.0000,-0.1090],
[24343,0.0983,0.0000,-0.0983],
[24383,0.0901,0.0000,-0.0901],
[24423,0.0797,0.0000,-0.0797],
[24463,0.0737,0.0000,-0.0737],
[24504,0.0685,0.0000,-0.0685],
[24544,0.0643,0.0000,-0.0643],
[24584,0.0595,0.0000,-0.0595],
[24624,0.0569,0.0000,-0.0569],
[24664,0.0523,0.0000,-0.0523],
[24704,0.0476,0.0000,-0.0476],
[24743,0.0427,0.0000,-0.0427],
[24783,0.0389,0.0000,-0.0389],
[24823,0.0366,0.0000,-0.0366],
[24864,0.0359,0.0000,-0.0359],
[24904,0.0348,0.0000,-0.0348],
[24944,0.0332,0.0000,-0.0332],
[24984,0.0314,0.0000,-0.0314],
[25024,0.0294,0.0000,-0.0294],
[25064,0.0278,0.0000,-0.0278],
[25104,0.0259,0.0000,-0.0259],
[25145,0.0212,0.0000,-0.0212],
[25185,0.0202,0.0000,-0.0202],
[25225,0.0179,0.0000,-0.0179],
[25265,0.0150,0.0000,-0.0150],
[25305,0.0129,0.0000,-0.0129],
[25345,0.0122,0.0000,-0.0122],
[25384,0.0096,0.0000,-0.0096],
[25424,0.0062,0.0000,-0.0062],
[25464,0.0034,0.0000,-0.0034],
[25505,0.0025,0.0000,-0.0025],
[25545,0.0025,0.0000,-0.0025],
[25585,0.0020,0.0000,-0.0020],
[25625,-0.0023,0.0000,0.0023],
[25665,-0.0050,0.0000,0.0050],
[25705,-0.0052,0.0000,0.0052],
[25745,-0.0051,0.0000,0.0051],
[25785,-0.0051,0.0000,0.0051],
[25826,-0.0050,0.0000,0.0050],
[25866,-0.0050,0.0000,0.0050],
[25906,-0.0050,0.0000,0.0050],
[25946,-0.0050,0.0000,0.0050],
[25986,-0.0050,0.0000,0.0050],
[26025,-0.0050,0.0000,0.0050],
[26065,-0.0050,0.0000,0.0050],
[26106,-0.0050,0.0000,0.0050],
[26146,-0.0050,0.0000,0.0050],
[26186,-0.0050,0.0000,0.0050],
[26226,-0.0048,0.0000,0.0048],
[26266,-0.0050,0.0000,0.0050],
[26306,-0.0048,0.0000,0.0048],
[26346,-0.0048,0.0000,0.0048],
[26387,-0.0048,0.0000,0.0048],
[26427,-0.0048,0.0000,0.0048],
[26467,-0.0048,0.0000,0.0048],
[26507,-0.0048,0.0000,0.0048],
[26547,-0.0048,0.0000,0.0048],
[26587,-0.0048,0.0000,0.0048],
[26627,-0.0048,0.0000,0.0048],
[26667,-0.0048,0.0000,0.0048],
[26707,-0.0048,0.0000,0.0048],
[26747,-0.0048,0.0000,0.0048],
[26787,-0.0048,0.0000,0.0048],
[26827,-0.0048,0.0000,0.0048],
[26867,-0.0048,0.0000,0.0048],
[26907,-0.0048,0.0000,0.0048],
[26947,-0.0048,0.0000,0.0048],
[26987,-0.0048,0.0000,0.0048],
[27027,-0.0047,0.0000,0.0047],
[27068,-0.0047,0.0000,0.0047],
[27108,-0.0047,0.0000,0.0047],
[27148,-0.0047,0.0000,0.0047],
[27188,-0.0047,0.0000,0.0047],
[27228,-0.0047,0.0000,0.0047],
[27268,-0.0047,0.0000,0.0047],
[27308,-0.0047,0.0000,0.0047],
[27347,-0.0047,0.0000,0.0047],
[27388,-0.0047,0.0000,0.0047],
[27428,-0.0047,0.0000,0.0047],
[27468,-0.0047,0.0000,0.0047],
[27508,-0.0047,0.0000,0.0047],
[27548,-0.0047,0.0000,0.0047],
[27588,-0.0047,0.0000,0.0047],
[27628,-0.0047,0.0000,0.0047],
[27668,-0.0047,0.0000,0.0047],
[27709,-0.0047,0.0000,0.0047],
[27749,-0.0047,0.0000,0.0047],
[27789,-0.0047,0.0000,0.0047],
[27829,-0.0047,0.0000,0.0047],
[27869,-0.0047,0.0000,0.0047],
[27909,-0.0047,0.0000,0.0047],
[27949,-0.0047,0.0000,0.0047],
[27988,-0.0046,0.0000,0.0046],
[28028,-0.0046,0.0000,0.0046],
[28069,-0.0046,0.0000,0.0046],
[28109,-0.0046,0.0000,0.0046],
[28149,-0.0046,0.0000,0.0046],
[28189,-0.0046,0.0000,0.0046],
[28229,-0.0046,0.0000,0.0046],
[28269,-0.0046,0.0000,0.0046],
[28310,-0.0046,0.0000,0.0046],
[28350,-0.0046,0.0000,0.0046],
[28390,-0.0046,0.0000,0.0046],
[28430,-0.0046,0.0000,0.0046],
[28470,-0.0046,0.0000,0.0046],
[28510,-0.0046,0.0000,0.0046],
[28550,-0.0046,0.0000,0.0046],
[28590,-0.0046,0.0000,0.0046],
[28630,-0.0046,0.0000,0.0046],
[28670,-0.0046,0.0000,0.0046],
[28710,-0.0046,0.0000,0.0046],
[28750,-0.0046,0.0000,0.0046],
[28790,-0.0046,0.0000,0.0046],
[28830,-0.0046,0.0000,0.0046],
[28870,-0.0045,0.0000,0.0045],
[28910,-0.0045,0.0000,0.0045],
[28950,-0.0045,0.0000,0.0045],
[28991,-0.0045,0.0000,0.0045],
[29031,-0.0045,0.0000,0.0045],
[29071,-0.0045,0.0000,0.0045],
[29111,-0.0045,0.0000,0.0045],
[29151,-0.0045,0.0000,0.0045],
[29191,-0.0045,0.0000,0.0045],
[29231,-0.0044,0.0000,0.0044],
[29272,-0.0044,0.0000,0.0044],
[29312,-0.0044,0.0000,0.0044],
[29351,-0.0044,0.0000,0.0044],
[29391,-0.0044,0.0000,0.0044],
[29431,-0.0044,0.0000,0.0044],
[29471,-0.0044,0.0000,0.0044],
[29511,-0.0044,0.0000,0.0044],
[29551,-0.0044,0.0000,0.0044],
[29591,-0.0044,0.0000,0.0044],
[29632,-0.0044,0.0000,0.0044],
[29672,-0.0044,0.0000,0.0044],
[29712,-0.0044,0.0000,0.0044],
[29752,-0.0044,0.0000,0.0044],
[29792,-0.0044,0.0000,0.0044],
[29832,-0.0044,0.0000,0.0044],
[29872,-0.0044,0.0000,0.0044],
[29913,-0.0044,0.0000,0.0044],
[29953,-0.0044,0.0000,0.0044],
[29992,-0.0044,0.0000,0.0044],
[30032,-0.0044,0.0000,0.0044],
[30072,-0.0044,0.0000,0.0044],
[30112,-0.0044,0.0000,0.0044],
[30152,-0.0044,0.0000,0.0044],
[30192,-0.0044,0.0000,0.0044],
[30233,-0.0044,0.0000,0.0044],
[30273,-0.0044,0.0000,0.0044],
[30313,-0.0044,0.0000,0.0044],
[30353,-0.0044,0.0000,0.0044],
[30393,-0.0044,0.0000,0.0044],
[30433,-0.0044,0.0000,0.0044],
[30473,-0.0044,0.0000,0.0044],
[30513,-0.0044,0.0000,0.0044],
[30554,-0.0044,0.0000,0.0044],
[30594,-0.0044,0.0000,0.0044],
[30633,-0.0044,0.0000,0.0044],
[30673,-0.0044,0.0000,0.0044],
[30713,-0.0044,0.0000,0.0044],
[30753,-0.0044,0.0000,0.0044],
[30793,-0.0044,0.0000,0.0044],
[30833,-0.0044,0.0000,0.0044],
[30874,-0.0043,0.0000,0.0043],
[30914,-0.0043,0.0000,0.0043],
[30954,-0.0043,0.0000,0.0043],
[30994,-0.0043,0.0000,0.0043],
[31034,-0.0043,0.0000,0.0043],
[31074,-0.0043,0.0000,0.0043],
[31114,-0.0043,0.0000,0.0043],
[31154,-0.0043,0.0000,0.0043],
[31194,-0.0043,0.0000,0.0043],
[31235,-0.0043,0.0000,0.0043],
[31275,-0.0043,0.0000,0.0043],
[31314,-0.0043,0.0000,0.0043],
[31354,-0.0043,0.0000,0.0043],
[31394,-0.0043,0.0000,0.0043],
[31434,-0.0043,0.0000,0.0043],
[31475,-0.0043,0.0000,0.0043],
[31515,-0.0043,0.0000,0.0043],
[31555,-0.0043,0.0000,0.0043],
[31595,-0.0043,0.0000,0.0043],
[31635,-0.0043,0.0000,0.0043],
[31675,-0.0043,0.0000,0.0043],
[31715,-0.0043,0.0000,0.0043],
[31755,-0.0041,0.0000,0.0041],
[31796,-0.0041,0.0000,0.0041],
[31836,-0.0041,0.0000,0.0041],
[31876,-0.0041,0.0000,0.0041],
[31916,-0.0041,0.0000,0.0041],
[31955,-0.0041,0.0000,0.0041],
[31995,-0.0041,0.0000,0.0041],
[32035,-0.0041,0.0000,0.0041],
[32075,-0.0041,0.0000,0.0041],
[32115,-0.0041,0.0000,0.0041],
[32156,-0.0041,0.0000,0.0041],
[32196,-0.0041,0.0000,0.0041],
[32236,-0.0041,0.0000,0.0041],
[32276,-0.0041,0.0000,0.0041],
[32316,-0.0041,0.0000,0.0041],
[32356,-0.0041,0.0000,0.0041],
[32396,-0.0040,0.0000,0.0040],
[32437,-0.0040,0.0000,0.0040],
[32477,-0.0040,1.5708,1.2092],
[32517,0.0042,1.5708,1.2092],
[32557,0.0820,1.5708,1.2092],
[32596,0.1714,1.5708,1.2092],
[32636,0.2637,1.5708,1.2092],
[32676,0.3568,1.5708,1.2092],
[32716,0.4507,1.5708,1.1201],
[32757,0.5446,1.5708,1.0262],
[32797,0.6331,1.5708,0.9377],
[32837,0.7189,1.5708,0.8519],
[32877,0.7989,1.5708,0.7719],
[32917,0.8710,1.5708,0.6998],
[32957,0.9370,1.5708,0.6338],
[32997,0.9930,1.5708,0.5778],
[33037,1.0476,1.5708,0.5232],
[33077,1.0956,1.5708,0.4752],
[33118,1.1414,1.5708,0.4294],
[33158,1.1821,1.5708,0.3887],
[33198,1.2158,1.5708,0.3550],
[33238,1.2491,1.5708,0.3217],
[33277,1.2795,1.5708,0.2913],
[33317,1.3023,1.5708,0.2685],
[33357,1.3257,1.5708,0.2451],
[33398,1.3492,1.5708,0.2216],
[33438,1.3715,1.5708,0.1993],
[33478,1.3911,1.5708,0.1797],
[33518,1.4061,1.5708,0.1647],
[33558,1.4202,1.5708,0.1506],
[33598,1.4346,1.5708,0.1362],
[33638,1.4464,1.5708,0.1244],
[33678,1.4584,1.5708,0.1124],
[33718,1.4677,1.5708,0.1031],
[33759,1.4734,1.5708,0.0974],
[33799,1.4806,1.5708,0.0902],
[33839,1.4891,1.5708,0.0817],
[33879,1.4943,1.5708,0.0765],
[33918,1.5010,1.5708,0.0698],
[33958,1.5071,1.5708,0.0637],
[33998,1.5135,1.5708,0.0573],
[34039,1.5179,1.5708,0.0529],
[34079,1.5199,1.5708,0.0509],
[34119,1.5227,1.5708,0.0481],
[34159,1.5275,1.5708,0.0433],
[34199,1.5313,1.5708,0.0395],
[34239,1.5345,1.5708,0.0363],
[34279,1.5377,1.5708,0.0331],
[34319,1.5400,1.5708,0.0308],
[34360,1.5424,1.5708,0.0284],
[34400,1.5442,1.5708,0.0266],
[34440,1.5471,1.5708,0.0237],
[34480,1.5497,1.5708,0.0210],
[34520,1.5522,1.5708,0.0186],
[34560,1.5531,1.5708,0.0177],
[34599,1.5532,1.5708,0.0176],
[34639,1.5532,1.5708,0.0176],
[34680,1.5532,1.5708,0.0176],
[34720,1.5532,1.5708,0.0176],
[34760,1.5532,1.5708,0.0176],
[34800,1.5532,1.5708,0.0176],
[34840,1.5532,1.5708,0.0176],
[34880,1.5532,1.5708,0.0176],
[34920,1.5532,1.5708,0.0176],
[34960,1.5532,1.5708,0.0176],
[35001,1.5532,1.5708,0.0176],
[35041,1.5532,1.5708,0.0176],
[35081,1.5532,1.5708,0.0176],
[35121,1.5532,1.5708,0.0176],
[35161,1.5532,1.5708,0.0176],
[35201,1.5532,1.5708,0.0176],
[35240,1.5532,1.5708,0.0176],
[35281,1.5532,1.5708,0.0176],
[35321,1.5532,1.5708,0.0176],
[35361,1.5532,1.5708,0.0176],
[35401,1.5532,1.5708,0.0176],
[35441,1.5532,1.5708,0.0176],
[35481,1.5532,0.7854,-0.7678],
[35521,1.5530,0.7854,-0.7676],
[35561,1.5092,0.7854,-0.7238],
[35601,1.4494,0.7854,-0.6640],
[35642,1.3881,0.7854,-0.6027],
[35682,1.3310,0.7854,-0.5456],
[35722,1.2758,0.7854,-0.4904],
[35762,1.2299,0.7854,-0.4445],
[35802,1.1839,0.7854,-0.3985],
[35842,1.1430,0.7854,-0.3576],
[35881,1.1091,0.7854,-0.3237],
[35922,1.0771,0.7854,-0.2917],
[35962,1.0506,0.7854,-0.2652],
[36002,1.0260,0.7854,-0.2406],
[36042,1.0043,0.7854,-0.2189],
[36082,0.9841,0.7854,-0.1987],
[36122,0.9647,0.7854,-0.1793],
[36162,0.9486,0.7854,-0.1632],
[36202,0.9330,0.7854,-0.1476],
[36243,0.9204,0.7854,-0.1350],
[36283,0.9085,0.7854,-0.1231],
[36323,0.8967,0.7854,-0.1113],
[36363,0.8856,0.7854,-0.1003],
[36403,0.8777,0.7854,-0.0923],
[36443,0.8683,0.7854,-0.0829],
[36483,0.8598,0.7854,-0.0744],
[36523,0.8557,0.7854,-0.0703],
[36563,0.8498,0.7854,-0.0644],
[36603,0.8447,0.7854,-0.0593],
[36643,0.8391,0.7854,-0.0537],
[36683,0.8345,0.7854,-0.0491],
[36723,0.8274,0.7854,-0.0420],
[36763,0.8215,0.7854,-0.0361],
[36803,0.8206,0.7854,-0.0352],
[36843,0.8189,0.7854,-0.0335],
[36884,0.8168,0.7854,-0.0314],
[36924,0.8135,0.7854,-0.0281],
[36964,0.8107,0.7854,-0.0253],
[37004,0.8085,0.7854,-0.0231],
[37044,0.8069,0.7854,-0.0215],
[37084,0.8041,0.7854,-0.0187],
[37124,0.8018,0.7854,-0.0164],
[37164,0.7991,0.7854,-0.0137],
[37204,0.7951,0.7854,-0.0097],
[37244,0.7943,0.7854,-0.0089],
[37284,0.7926,0.7854,-0.0072],
[37324,0.7911,0.7854,-0.0057],
[37364,0.7893,0.7854,-0.0039],
[37404,0.7884,0.7854,-0.0030],
[37444,0.7871,0.7854,-0.0017],
[37484,0.7863,0.7854,-0.0009],
[37524,0.7860,0.7854,-0.0006],
[37565,0.7860,0.7854,-0.0006],
[37605,0.7860,0.7854,-0.0006],
[37645,0.7860,0.7854,-0.0006],
[37685,0.7860,0.7854,-0.0006],
[37725,0.7860,0.7854,-0.0006],
[37765,0.7860,0.7854,-0.0006],
[37805,0.7860,0.7854,-0.0006],
[37844,0.7860,0.7854,-0.0006],
[37884,0.7860,0.7854,-0.0006],
[37925,0.7860,0.7854,-0.0006],
[37965,0.7860,0.7854,-0.0006],
[38005,0.7860,0.7854,-0.0006],
[38045,0.7860,0.7854,-0.0006],
[38085,0.7860,0.7854,-0.0006],
[38125,0.7860,0.7854,-0.0006],
[38165,0.7860,0.7854,-0.0006],
[38206,0.7860,0.7854,-0.0006],
[38246,0.7860,0.7854,-0.0006],
[38286,0.7860,0.7854,-0.0006],
[38326,0.7860,0.7854,-0.0006],
[38366,0.7860,0.7854,-0.0006],
[38406,0.7860,0.7854,-0.0006],
[38446,0.7860,0.7854,-0.0006]]

