copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/frc971/control_loops/matlab/drivetrain_controller.m b/frc971/control_loops/matlab/drivetrain_controller.m
new file mode 100644
index 0000000..11a5a4b
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_controller.m
@@ -0,0 +1,231 @@
+close all;
+load 'drivetrain_spin_low'
+load 'drivetrain_strait_low'
+m = 68;
+rb = 0.617998644 / 2.0;
+J = 7;
+stall_current = 133.0;
+R = 12.0 / stall_current / 4 / 0.43;
+Km = (12.0 - R * 2.7) / (4650.0 / 60.0 * 2.0 * pi);
+Kt = 0.008;
+r = 0.04445; % 3.5 inches diameter
+G_low = 60.0 / 15.0 * 50.0 / 15.0;
+G_high = 45.0 / 30.0 * 50.0 / 15.0;
+dt = 0.01;
+
+G = G_low;
+
+msp = (1.0 / m + rb ^ 2.0 / J);
+msn = (1.0 / m - rb ^ 2.0 / J);
+tc = -Km * Kt * G ^ 2.0 / (R * r ^ 2.0);
+mp = G * Kt / (R * r);
+
+A = [0 1 0 0; 0 msp*tc 0 msn*tc; 0 0 0 1; 0 msn*tc 0 msp*tc];
+B = [0 0; msp * mp msn * mp; 0 0; msn * mp msp * mp];
+C = [1 0 0 0; 0 0 1 0];
+D = [0 0; 0 0];
+
+dm = c2d(ss(A, B, C, D), dt);
+
+hp = .8;
+lp = .85;
+K = place(dm.a, dm.b, [hp, hp, lp, lp]);
+
+hlp = 0.07;
+llp = 0.09;
+L = place(dm.a', dm.c', [hlp, hlp, llp, llp])';
+
+% Plot what we computed
+
+fd = fopen('/home/aschuh/frc971/2012/trunk/src/atom_code/control_loops/Drivetrain.mat', 'w');
+n = 1;
+sm = [];
+writeMatHeader(fd, size(dm.a, 1), size(dm.b, 2));
+writeMat(fd, dm.a, 'A');
+writeMat(fd, dm.b, 'B');
+writeMat(fd, dm.c, 'C');
+writeMat(fd, dm.d, 'D');
+writeMat(fd, L, 'L');
+writeMat(fd, K, 'K');
+writeMat(fd, [12; 12], 'U_max');
+writeMat(fd, [-12; -12], 'U_min');
+writeMatFooter(fd);
+fclose(fd);
+
+full_model = dss([dm.a (-dm.b * K); eye(4) (dm.a - dm.b * K - L * dm.c)], [0, 0; 0, 0; 0, 0; 0, 0; L], [C, [0, 0, 0, 0; 0, 0, 0, 0]], 0, eye(8), 0.01);
+
+n = 1;
+sm_strait = [];
+t = drivetrain_strait_low(1, 1) + dt * (n - 1);
+x = [drivetrain_strait_low(1, 2); 0; drivetrain_strait_low(1, 3); 0];
+while t < drivetrain_strait_low(end, 1)
+ sm_strait(n, 1) = t;
+ sm_strait(n, 2) = (x(1,1) + x(3,1)) / 2.0;
+ t = t + dt;
+ x = dm.a * x + dm.b * [drivetrain_strait_low(n, 4); drivetrain_strait_low(n, 5)];
+ n = n + 1;
+end
+
+figure;
+plot(drivetrain_strait_low(:, 1), (drivetrain_strait_low(:, 2) + drivetrain_strait_low(:, 3)) / 2.0, sm_strait(:, 1), sm_strait(:, 2));
+legend('actual', 'sim');
+
+n = 1;
+sm_spin = [];
+t = drivetrain_spin_low(1, 1) + dt * (n - 1);
+x = [drivetrain_spin_low(1, 2); 0; drivetrain_spin_low(1, 3); 0];
+while t < drivetrain_spin_low(end, 1)
+ sm_spin(n, 1) = t;
+ sm_spin(n, 2) = (x(1,1) - x(3,1)) / 2.0;
+ t = t + dt;
+ x = dm.a * x + dm.b * [drivetrain_spin_low(n, 4); drivetrain_spin_low(n, 5)];
+ n = n + 1;
+end
+
+figure;
+plot(drivetrain_spin_low(:, 1), (drivetrain_spin_low(:, 2) - drivetrain_spin_low(:, 3)) / 2.0, sm_spin(:, 1), sm_spin(:, 2));
+legend('actual', 'sim');
+
+%figure;
+%nyquist(full_model);
+
+
+%%
+t = 0;
+x = [0; 0; 0; 0;];
+while t < logging(end, 1)
+ sm(n, 1) = t;
+ sm(n, 2) = x(1,1);
+ sm(n, 3) = x(3,1);
+ t = t + dt;
+ x = dm.a * x + dm.b * [12.0; 12.0];
+ n = n + 1;
+end
+
+figure;
+plot(logging(:, 1), logging(:, 2), sm(:, 1), sm(:, 2));
+legend('actual', 'sim');
+
+%% Simulation of a small turn angle with a large distance to travel
+tf = 2;
+x = [0; 0; 0.1; 0;];
+r = [10; 0; 10; 0];
+
+smt = zeros(tf / dt, 8);
+t = 0;
+xhat = x;
+n = 1;
+% 1 means scale
+% 2 means just limit to 12 volts
+% 3 means preserve the difference in power
+captype = 1;
+while n <= size(smt, 1)
+ smt(n, 1) = t;
+ smt(n, 2) = x(1,1);
+ smt(n, 3) = x(3,1);
+ t = t + dt;
+
+ u = K * (r - xhat);
+ smt(n, 4) = u(1,1);
+ smt(n, 5) = u(2,1);
+
+ if captype == 1
+ if sum(abs(u) > 12.0)
+ % We have a problem!
+ % Check to see if it's a big steering power problem,
+ % or a big drive error.
+ turnPower = (u(1, 1) - u(2, 1));
+ drivePower = (u(1, 1) + u(2, 1));
+ scaleFactor = 12.0 / max(abs(u));
+ smt(n, 8) = 1.0 / scaleFactor;
+ % Only start scaling the turn power up if we are far out of
+ % range.
+ if abs(turnPower) < 0.5 * abs(drivePower)
+ % Turn power is swamped.
+ deltaTurn = turnPower / 2.0 / scaleFactor * 0.5;
+ u(1, 1) = u(1, 1) + deltaTurn;
+ u(2, 1) = u(2, 1) - deltaTurn;
+ scaleFactor = 12.0 / max(abs(u));
+ else
+ if 0.5 * abs(turnPower) > abs(drivePower)
+ % Drive power is swamped.
+ deltaDrive = drivePower / 2.0 / scaleFactor * 0.5;
+ u(1, 1) = u(1, 1) + deltaDrive;
+ u(2, 1) = u(2, 1) + deltaDrive;
+ scaleFactor = 12.0 / max(abs(u));
+ end
+ end
+ u = u * scaleFactor;
+ end
+ else
+ if captype == 2
+ if u(1, 1) > 12.0
+ u(1, 1) = 12.0;
+ end
+ if u(1, 1) < -12.0
+ u(1, 1) = -12.0;
+ end
+ if u(2, 1) > 12.0
+ u(2, 1) = 12.0;
+ end
+ if u(2, 1) < -12.0
+ u(2, 1) = -12.0;
+ end
+ else
+ if captype == 3
+ if u(1, 1) > 12.0
+ u(2, 1) = u(2, 1) - (u(1, 1) - 12.0);
+ else
+ if u(1, 1) < -12.0
+ u(2, 1) = u(2, 1) - (u(1, 1) + 12.0);
+ end
+ end
+ if u(2, 1) > 12.0
+ u(1, 1) = u(1, 1) - (u(2, 1) - 12.0);
+ else
+ if u(2, 1) < -12.0
+ u(1, 1) = u(1, 1) - (u(2, 1) + 12.0);
+ end
+ end
+ if u(1, 1) > 12.0
+ u(1, 1) = 12.0;
+ end
+ if u(1, 1) < -12.0
+ u(1, 1) = -12.0;
+ end
+ if u(2, 1) > 12.0
+ u(2, 1) = 12.0;
+ end
+ if u(2, 1) < -12.0
+ u(2, 1) = -12.0;
+ end
+ end
+ end
+
+ end
+ smt(n, 6) = u(1,1);
+ smt(n, 7) = u(2,1);
+ xhat = dm.a * xhat + dm.b * u + L * (dm.c * x - dm.c * xhat);
+ x = dm.a * x + dm.b * u;
+
+ n = n + 1;
+end
+
+figure;
+subplot(6, 1, 1);
+plot(smt(:, 1), smt(:, 2) + smt(:, 3));
+legend('dist');
+subplot(6, 1, 2);
+plot(smt(:, 1), smt(:, 2) - smt(:, 3));
+legend('angle');
+subplot(3, 1, 2);
+plot(smt(:, 1), smt(:, 4), smt(:, 1), smt(:, 5));
+legend('lu', 'ru');
+subplot(3, 1, 3);
+plot(smt(:, 1), smt(:, 6), smt(:, 1), smt(:, 7));
+legend('lu_{real}', 'ru_{real}');
+
+%figure;
+%plot(smt(:, 1), smt(:, 8))
+%legend('Scale Factor');
+
diff --git a/frc971/control_loops/matlab/drivetrain_spin_fast.csv b/frc971/control_loops/matlab/drivetrain_spin_fast.csv
new file mode 100644
index 0000000..5768209
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_spin_fast.csv
@@ -0,0 +1,215 @@
+12038,8.037056,-7.571709,1.000000
+12039,8.037056,-7.571709,1.000000
+12040,8.037056,-7.571709,1.000000
+12041,8.037254,-7.572700,1.000000
+12042,8.044792,-7.577064,1.000000
+12043,8.053718,-7.583610,1.000000
+12044,8.066016,-7.593330,1.000000
+12045,8.078909,-7.602652,1.000000
+12046,8.094183,-7.613760,1.000000
+12047,8.109655,-7.624670,1.000000
+12048,8.126118,-7.637167,1.000000
+12049,8.141789,-7.650060,1.000000
+12050,8.156070,-7.662556,1.000000
+12051,8.174716,-7.677433,1.000000
+12052,8.190188,-7.690128,1.000000
+12053,8.208635,-7.704608,1.000000
+12054,8.225694,-7.717303,1.000000
+12055,8.242951,-7.729800,1.000000
+12056,8.261200,-7.745073,1.000000
+12057,8.276077,-7.758363,1.000000
+12058,8.289565,-7.771058,1.000000
+12059,8.308012,-7.787720,1.000000
+12060,8.327253,-7.805771,1.000000
+12061,8.345105,-7.819457,1.000000
+12062,8.360379,-7.830565,1.000000
+12063,8.377041,-7.844252,1.000000
+12064,8.396083,-7.860914,1.000000
+12065,8.413539,-7.877378,1.000000
+12066,8.430597,-7.895230,1.000000
+12067,8.445871,-7.910107,1.000000
+12068,8.460153,-7.923397,1.000000
+12069,8.478600,-7.938274,1.000000
+12070,8.497444,-7.954341,1.000000
+12071,8.516288,-7.970408,1.000000
+12072,8.535132,-7.987268,1.000000
+12073,8.549215,-8.002541,1.000000
+12074,8.563299,-8.018410,1.000000
+12075,8.581944,-8.037651,1.000000
+12076,8.605152,-8.057288,1.000000
+12077,8.623203,-8.070578,1.000000
+12078,8.639865,-8.087637,1.000000
+12079,8.658709,-8.105489,1.000000
+12080,8.676958,-8.122945,1.000000
+12087,8.815213,-8.259811,1.000000
+12088,8.835247,-8.279250,1.000000
+12089,8.852702,-8.298094,1.000000
+12090,8.876307,-8.320310,1.000000
+12091,8.897333,-8.338758,1.000000
+12092,8.922921,-8.360577,1.000000
+12093,8.948311,-8.383983,1.000000
+12094,8.968147,-8.403621,1.000000
+12095,8.988974,-8.426035,1.000000
+12096,9.009405,-8.447656,1.000000
+12097,9.035191,-8.474633,1.000000
+12098,9.055027,-8.494865,1.000000
+12099,9.076450,-8.516288,1.000000
+12100,9.101641,-8.540686,1.000000
+12101,9.121675,-8.560323,1.000000
+12102,9.144090,-8.583729,1.000000
+12103,9.171066,-8.609119,1.000000
+12104,9.195663,-8.630542,1.000000
+12105,9.218672,-8.653155,1.000000
+12106,9.240690,-8.671602,1.000000
+12107,9.265881,-8.693421,1.000000
+12108,9.290676,-8.717621,1.000000
+12109,9.315868,-8.741424,1.000000
+12110,9.343043,-8.767210,1.000000
+12111,9.365655,-8.788038,1.000000
+12112,9.392632,-8.813626,1.000000
+12113,9.417427,-8.836635,1.000000
+12114,9.441230,-8.857066,1.000000
+12115,9.468801,-8.881464,1.000000
+12116,9.499150,-8.906656,1.000000
+12117,9.521564,-8.928475,1.000000
+12118,9.551120,-8.953666,1.000000
+12119,9.574724,-8.974692,1.000000
+12120,9.604081,-9.003454,1.000000
+12121,9.631455,-9.030629,1.000000
+12122,9.656646,-9.054829,1.000000
+12123,9.686400,-9.084979,1.000000
+12124,9.711789,-9.111559,1.000000
+12125,9.737576,-9.136552,1.000000
+12126,9.761776,-9.158570,1.000000
+12127,9.788554,-9.182770,1.000000
+12128,9.815927,-9.208754,1.000000
+12129,9.845681,-9.235334,1.000000
+12130,9.872261,-9.259534,1.000000
+12131,9.896460,-9.282543,1.000000
+12132,9.924627,-9.308528,1.000000
+12133,9.954976,-9.336893,1.000000
+12134,9.982746,-9.362283,1.000000
+12135,10.007739,-9.385293,1.000000
+12136,10.038881,-9.414055,1.000000
+12137,10.065064,-9.437262,1.000000
+12138,10.097595,-9.464437,1.000000
+12139,10.123977,-9.488042,1.000000
+12140,10.155912,-9.517201,1.000000
+12141,10.180905,-9.539813,1.000000
+12142,10.213039,-9.568377,1.000000
+12143,10.238429,-9.592973,1.000000
+12144,10.270166,-9.622529,1.000000
+12145,10.296548,-9.646728,1.000000
+12146,10.326897,-9.673705,1.000000
+12147,10.356253,-9.700285,1.000000
+12148,10.388784,-9.728650,1.000000
+12149,10.419133,-9.755230,1.000000
+12150,10.449878,-9.783992,1.000000
+12151,10.481020,-9.814935,1.000000
+12152,10.508592,-9.843499,1.000000
+12153,10.536759,-9.875633,1.000000
+12154,10.568099,-9.908560,1.000000
+12155,10.594679,-9.934347,1.000000
+12156,10.624036,-9.961323,1.000000
+12157,10.653195,-9.988300,1.000000
+12158,10.682949,-10.015078,1.000000
+12159,10.716074,-10.045824,1.000000
+12160,10.745828,-10.074189,1.000000
+12161,10.773003,-10.098587,1.000000
+12162,10.804542,-10.127944,1.000000
+12163,10.831518,-10.152342,1.000000
+12164,10.865041,-10.183881,1.000000
+12165,10.893406,-10.211254,1.000000
+12166,10.924945,-10.240413,1.000000
+12167,10.960054,-10.269968,1.000000
+12168,10.989411,-10.294168,1.000000
+12169,11.020950,-10.322334,1.000000
+12170,11.057249,-10.352881,1.000000
+12171,11.086210,-10.379263,1.000000
+12172,11.116162,-10.407231,1.000000
+12173,11.144130,-10.434605,1.000000
+12174,11.176859,-10.466342,1.000000
+12175,11.212564,-10.498873,1.000000
+12176,11.240929,-10.524461,1.000000
+12177,11.271872,-10.553818,1.000000
+12178,11.306387,-10.587737,1.000000
+12179,11.337926,-10.620863,1.000000
+12180,11.364704,-10.651410,1.000000
+12186,11.552945,-10.839849,1.000000
+12187,11.580517,-10.865041,1.000000
+12188,11.612056,-10.895588,1.000000
+12189,11.642603,-10.925738,1.000000
+12190,11.676125,-10.957674,1.000000
+12191,11.707268,-10.987031,1.000000
+12192,11.736823,-11.013214,1.000000
+12193,11.773122,-11.045943,1.000000
+12194,11.802479,-11.072920,1.000000
+12195,11.834415,-11.103268,1.000000
+12196,11.868532,-11.136394,1.000000
+12197,11.900865,-11.167338,1.000000
+12198,11.931808,-11.197488,1.000000
+12199,11.964141,-11.227440,1.000000
+12200,11.997068,-11.256797,1.000000
+12201,12.026623,-11.283576,1.000000
+12202,12.063121,-11.316701,1.000000
+12203,12.092676,-11.344273,1.000000
+12204,12.128778,-11.377200,1.000000
+12205,12.158333,-11.404574,1.000000
+12206,12.193641,-11.437699,1.000000
+12207,12.223394,-11.465668,1.000000
+12208,12.259297,-11.499785,1.000000
+12209,12.292621,-11.532514,1.000000
+12210,12.321581,-11.563458,1.000000
+12211,12.357087,-11.601345,1.000000
+12212,12.389618,-11.637049,1.000000
+12213,12.418776,-11.669580,1.000000
+12214,12.454481,-11.707466,1.000000
+12215,12.482648,-11.737021,1.000000
+12216,12.517757,-11.769155,1.000000
+12217,12.547511,-11.796528,1.000000
+12218,12.584802,-11.830844,1.000000
+12219,12.614754,-11.859209,1.000000
+12220,12.647284,-11.890153,1.000000
+12221,12.679815,-11.920502,1.000000
+12222,12.715520,-11.954818,1.000000
+12223,12.749042,-11.986158,1.000000
+12224,12.781969,-12.017896,1.000000
+12225,12.811723,-12.045071,1.000000
+12226,12.844849,-12.075816,1.000000
+12227,12.878371,-12.106363,1.000000
+12228,12.913084,-12.137902,1.000000
+12229,12.947995,-12.169639,1.000000
+12230,12.982707,-12.200385,1.000000
+12231,13.019602,-12.233510,1.000000
+12232,13.051736,-12.264057,1.000000
+12233,13.086448,-12.296191,1.000000
+12234,13.117590,-12.326143,1.000000
+12235,13.156072,-12.363236,1.000000
+12236,13.186222,-12.393982,1.000000
+12237,13.219745,-12.428496,1.000000
+12238,13.255846,-12.467771,1.000000
+12239,13.285401,-12.499310,1.000000
+12240,13.320907,-12.536998,1.000000
+12241,13.353834,-12.570322,1.000000
+12242,13.387754,-12.601861,1.000000
+12243,13.419689,-12.629234,1.000000
+12244,13.457774,-12.663550,1.000000
+12245,13.492090,-12.695089,1.000000
+12246,13.522438,-12.723652,1.000000
+12247,13.558738,-12.758563,1.000000
+12248,13.591269,-12.790895,1.000000
+12249,13.621419,-12.819261,1.000000
+12250,13.659305,-12.854965,1.000000
+12251,13.691241,-12.883132,1.000000
+12252,13.726350,-12.914076,1.000000
+12253,13.760864,-12.944623,1.000000
+12254,13.798751,-12.979335,-1.000000
+12255,13.829694,-13.008295,-1.000000
+12256,13.866391,-13.044000,-1.000000
+12257,13.894557,-13.069985,-1.000000
+12258,13.918360,-13.087242,-1.000000
+12259,13.925898,-13.091209,-1.000000
+12260,13.924311,-13.085060,-1.000000
+12261,13.919551,-13.075340,-1.000000
+12262,13.915980,-13.064629,-1.000000
+12263,13.913005,-13.050942,-1.000000
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/drivetrain_spin_low.csv b/frc971/control_loops/matlab/drivetrain_spin_low.csv
new file mode 100644
index 0000000..a96acbe
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_spin_low.csv
@@ -0,0 +1,280 @@
+120.220000,16.662837,-14.612612,12.000000,-12.000000
+120.230000,16.662837,-14.612612,12.000000,-12.000000
+120.240000,16.662837,-14.612612,12.000000,-12.000000
+120.250000,16.662837,-14.612612,12.000000,-12.000000
+120.260000,16.662837,-14.612612,12.000000,-12.000000
+120.270000,16.665217,-14.617571,12.000000,-12.000000
+120.280000,16.673152,-14.624117,12.000000,-12.000000
+120.290000,16.682474,-14.632051,12.000000,-12.000000
+120.300000,16.692194,-14.640977,12.000000,-12.000000
+120.310000,16.702112,-14.650895,12.000000,-12.000000
+120.320000,16.713220,-14.660020,12.000000,-12.000000
+120.330000,16.724526,-14.671326,12.000000,-12.000000
+120.340000,16.736626,-14.682632,12.000000,-12.000000
+120.350000,16.748527,-14.693939,12.000000,-12.000000
+120.360000,16.761024,-14.705245,12.000000,-12.000000
+120.370000,16.773719,-14.716552,12.000000,-12.000000
+120.380000,16.786017,-14.728255,12.000000,-12.000000
+120.390000,16.799109,-14.740156,12.000000,-12.000000
+120.400000,16.812002,-14.751661,12.000000,-12.000000
+120.410000,16.824895,-14.763364,12.000000,-12.000000
+120.420000,16.837193,-14.775861,12.000000,-12.000000
+120.430000,16.850087,-14.787960,12.000000,-12.000000
+120.440000,0.0,-0.0,12.000000,-12.000000
+120.450000,0.0,-0.0,12.000000,-12.000000
+120.460000,0.0,-0.0,12.000000,-12.000000
+120.470000,0.0,-0.0,12.000000,-12.000000
+120.480000,16.913759,-14.847864,12.000000,-12.000000
+120.490000,16.926653,-14.860361,12.000000,-12.000000
+120.500000,16.939546,-14.873453,12.000000,-12.000000
+120.510000,16.952439,-14.886147,12.000000,-12.000000
+120.520000,16.965134,-14.899041,12.000000,-12.000000
+120.530000,16.978821,-14.911537,12.000000,-12.000000
+120.540000,16.991714,-14.924232,12.000000,-12.000000
+120.550000,17.004409,-14.936729,12.000000,-12.000000
+120.560000,17.018294,-14.950019,12.000000,-12.000000
+120.570000,17.030592,-14.963110,12.000000,-12.000000
+120.580000,17.043882,-14.976400,12.000000,-12.000000
+120.590000,17.057569,-14.989690,12.000000,-12.000000
+120.600000,17.071850,-15.003178,12.000000,-12.000000
+120.610000,17.085537,-15.016270,12.000000,-12.000000
+120.620000,17.099422,-15.029758,12.000000,-12.000000
+120.630000,17.113109,-15.043048,12.000000,-12.000000
+120.640000,17.127192,-15.056735,12.000000,-12.000000
+120.650000,17.141672,-15.070818,12.000000,-12.000000
+120.660000,17.155756,-15.084505,12.000000,-12.000000
+120.670000,17.170434,-15.098192,12.000000,-12.000000
+120.680000,17.185113,-15.112275,12.000000,-12.000000
+120.690000,17.199990,-15.126359,12.000000,-12.000000
+120.700000,17.214073,-15.140442,12.000000,-12.000000
+120.710000,17.228553,-15.155120,12.000000,-12.000000
+120.720000,17.243430,-15.170196,12.000000,-12.000000
+120.730000,17.258307,-15.184279,12.000000,-12.000000
+120.740000,17.273183,-15.198561,12.000000,-12.000000
+120.750000,17.288259,-15.212842,12.000000,-12.000000
+120.760000,17.303731,-15.227521,12.000000,-12.000000
+120.770000,17.319202,-15.242398,12.000000,-12.000000
+120.780000,17.334674,-15.256878,12.000000,-12.000000
+120.790000,17.350345,-15.272151,12.000000,-12.000000
+120.800000,17.365618,-15.287425,12.000000,-12.000000
+120.810000,17.381288,-15.302500,12.000000,-12.000000
+120.820000,17.396959,-15.317972,12.000000,-12.000000
+120.830000,17.412431,-15.333444,12.000000,-12.000000
+120.840000,17.428101,-15.349313,12.000000,-12.000000
+120.850000,17.443176,-15.364388,12.000000,-12.000000
+120.860000,17.458846,-15.379265,12.000000,-12.000000
+120.870000,17.474318,-15.394340,12.000000,-12.000000
+120.880000,17.489988,-15.409812,12.000000,-12.000000
+120.890000,17.505064,-15.424887,12.000000,-12.000000
+120.900000,17.520932,-15.439962,12.000000,-12.000000
+120.910000,17.536404,-15.455236,12.000000,-12.000000
+120.920000,17.552074,-15.469914,12.000000,-12.000000
+120.930000,17.567745,-15.485188,12.000000,-12.000000
+120.940000,17.584208,-15.501255,12.000000,-12.000000
+120.950000,17.600275,-15.515735,12.000000,-12.000000
+120.960000,17.616541,-15.531207,12.000000,-12.000000
+120.970000,17.633004,-15.546282,12.000000,-12.000000
+120.980000,17.649865,-15.561555,12.000000,-12.000000
+120.990000,17.666527,-15.576234,12.000000,-12.000000
+121.000000,17.682792,-15.591111,12.000000,-12.000000
+121.010000,17.699256,-15.606384,12.000000,-12.000000
+121.020000,17.715521,-15.621658,12.000000,-12.000000
+121.030000,17.731786,-15.636534,12.000000,-12.000000
+121.040000,17.747853,-15.652006,12.000000,-12.000000
+121.050000,17.763722,-15.666883,12.000000,-12.000000
+121.060000,17.779789,-15.682355,12.000000,-12.000000
+121.070000,17.796253,-15.698025,12.000000,-12.000000
+121.080000,17.812716,-15.713696,12.000000,-12.000000
+121.090000,17.828585,-15.728771,12.000000,-12.000000
+121.100000,17.845247,-15.744441,12.000000,-12.000000
+121.110000,17.861512,-15.760310,12.000000,-12.000000
+121.120000,17.877778,-15.775980,12.000000,-12.000000
+121.130000,17.894241,-15.791650,12.000000,-12.000000
+121.140000,17.910507,-15.807519,12.000000,-12.000000
+121.150000,17.926970,-15.822991,12.000000,-12.000000
+121.160000,17.943037,-15.838066,12.000000,-12.000000
+121.170000,17.959104,-15.853736,12.000000,-12.000000
+121.180000,17.975568,-15.869605,12.000000,-12.000000
+121.190000,17.992032,-15.884680,12.000000,-12.000000
+121.200000,18.008694,-15.900747,12.000000,-12.000000
+121.210000,18.024364,-15.916814,12.000000,-12.000000
+121.220000,18.040828,-15.932286,12.000000,-12.000000
+121.230000,18.057291,-15.947559,12.000000,-12.000000
+121.240000,18.073755,-15.963825,12.000000,-12.000000
+121.250000,18.090615,-15.979495,12.000000,-12.000000
+121.260000,18.107476,-15.995959,12.000000,-12.000000
+121.270000,18.123939,-16.011232,12.000000,-12.000000
+121.280000,18.140998,-16.027299,12.000000,-12.000000
+121.290000,18.157462,-16.042573,12.000000,-12.000000
+121.300000,18.174719,-16.058838,12.000000,-12.000000
+121.310000,18.191183,-16.074707,12.000000,-12.000000
+121.320000,18.208242,-16.090575,12.000000,-12.000000
+121.330000,18.225102,-16.106444,12.000000,-12.000000
+121.340000,18.242161,-16.122511,12.000000,-12.000000
+121.350000,18.258823,-16.138181,12.000000,-12.000000
+121.360000,18.276080,-16.154248,12.000000,-12.000000
+121.370000,18.292742,-16.169918,12.000000,-12.000000
+121.380000,18.309801,-16.185588,12.000000,-12.000000
+121.390000,18.326463,-16.201655,12.000000,-12.000000
+121.400000,18.343521,-16.217524,12.000000,-12.000000
+121.410000,18.359985,-16.232996,12.000000,-12.000000
+121.420000,18.377242,-16.249063,12.000000,-12.000000
+121.430000,18.394301,-16.265130,12.000000,-12.000000
+121.440000,18.411558,-16.280800,12.000000,-12.000000
+121.450000,0.0,-0.0,12.000000,-12.000000
+121.460000,0.0,-0.0,12.000000,-12.000000
+121.470000,0.0,-0.0,12.000000,-12.000000
+121.480000,18.480587,-16.345068,12.000000,-12.000000
+121.490000,18.497447,-16.360738,12.000000,-12.000000
+121.500000,18.514506,-16.377202,12.000000,-12.000000
+121.510000,18.531564,-16.392475,12.000000,-12.000000
+121.520000,18.548623,-16.408542,12.000000,-12.000000
+121.530000,18.565880,-16.424609,12.000000,-12.000000
+121.540000,18.582542,-16.441073,12.000000,-12.000000
+121.550000,18.599601,-16.456743,12.000000,-12.000000
+121.560000,18.616858,-16.473009,12.000000,-12.000000
+121.570000,18.633520,-16.489274,12.000000,-12.000000
+121.580000,18.650381,-16.505738,12.000000,-12.000000
+121.590000,18.666646,-16.522003,12.000000,-12.000000
+121.600000,18.683705,-16.537673,12.000000,-12.000000
+121.610000,18.700764,-16.553542,12.000000,-12.000000
+121.620000,18.718219,-16.570204,12.000000,-12.000000
+121.630000,18.734683,-16.585676,12.000000,-12.000000
+121.640000,18.752138,-16.602140,12.000000,-12.000000
+121.650000,18.768602,-16.618008,12.000000,-12.000000
+121.660000,18.785661,-16.634075,12.000000,-12.000000
+121.670000,18.802521,-16.649745,12.000000,-12.000000
+121.680000,18.819977,-16.666011,12.000000,-12.000000
+121.690000,18.836440,-16.681879,12.000000,-12.000000
+121.700000,18.853301,-16.697748,12.000000,-12.000000
+121.710000,18.870161,-16.713617,12.000000,-12.000000
+121.720000,18.887815,-16.729882,12.000000,-12.000000
+121.730000,18.904675,-16.745949,12.000000,-12.000000
+121.740000,18.921932,-16.762214,12.000000,-12.000000
+121.750000,18.938594,-16.777686,12.000000,-12.000000
+121.760000,18.955653,-16.793555,12.000000,-12.000000
+121.770000,18.972910,-16.809423,12.000000,-12.000000
+121.780000,18.990564,-16.825689,12.000000,-12.000000
+121.790000,19.007623,-16.841359,12.000000,-12.000000
+121.800000,19.025078,-16.857426,12.000000,-12.000000
+121.810000,19.041740,-16.873096,12.000000,-12.000000
+121.820000,19.058998,-16.889163,12.000000,-12.000000
+121.830000,19.076056,-16.905032,12.000000,-12.000000
+121.840000,19.094107,-16.921694,12.000000,-12.000000
+121.850000,19.110372,-16.937364,12.000000,-12.000000
+121.860000,19.128423,-16.953828,12.000000,-12.000000
+121.870000,19.144886,-16.969300,12.000000,-12.000000
+121.880000,19.162540,-16.986160,12.000000,-12.000000
+121.890000,19.179004,-17.001433,12.000000,-12.000000
+121.900000,19.196459,-17.017897,12.000000,-12.000000
+121.910000,19.213716,-17.033964,12.000000,-12.000000
+121.920000,19.231172,-17.050428,12.000000,-12.000000
+121.930000,19.248231,-17.066098,12.000000,-12.000000
+121.940000,19.265885,-17.082562,12.000000,-12.000000
+121.950000,19.283340,-17.099025,12.000000,-12.000000
+121.960000,19.301192,-17.115886,12.000000,-12.000000
+121.970000,19.318251,-17.131754,12.000000,-12.000000
+121.980000,19.335905,-17.148218,12.000000,-12.000000
+121.990000,19.352765,-17.164285,12.000000,-12.000000
+122.000000,19.370022,-17.180947,12.000000,-12.000000
+122.010000,19.387081,-17.197411,12.000000,-12.000000
+122.020000,19.404933,-17.214073,12.000000,-12.000000
+122.030000,19.421199,-17.229942,12.000000,-12.000000
+122.040000,19.439448,-17.247000,12.000000,-12.000000
+122.050000,19.455514,-17.262472,12.000000,-12.000000
+122.060000,19.472772,-17.279333,12.000000,-12.000000
+122.070000,19.489632,-17.295400,12.000000,-12.000000
+122.080000,19.507087,-17.311665,12.000000,-12.000000
+122.090000,19.523948,-17.327930,12.000000,-12.000000
+122.100000,19.541403,-17.343799,12.000000,-12.000000
+122.110000,19.558462,-17.360064,12.000000,-12.000000
+122.120000,19.576116,-17.376925,12.000000,-12.000000
+122.130000,19.592976,-17.392992,12.000000,-12.000000
+122.140000,19.610432,-17.409654,12.000000,-12.000000
+122.150000,19.627491,-17.425125,12.000000,-12.000000
+122.160000,19.644946,-17.441788,12.000000,-12.000000
+122.170000,19.661608,-17.457458,12.000000,-12.000000
+122.180000,19.679262,-17.473525,12.000000,-12.000000
+122.190000,19.696321,-17.489195,12.000000,-12.000000
+122.200000,19.713379,-17.505262,12.000000,-12.000000
+122.210000,19.730438,-17.520932,12.000000,-12.000000
+122.220000,19.748687,-17.537198,12.000000,-12.000000
+122.230000,19.765349,-17.552868,12.000000,-12.000000
+122.240000,19.783003,-17.569728,12.000000,-12.000000
+122.250000,19.799665,-17.585398,12.000000,-12.000000
+122.260000,19.817517,-17.601862,12.000000,-12.000000
+122.270000,19.834378,-17.617136,12.000000,-12.000000
+122.280000,19.851635,-17.633401,12.000000,-12.000000
+122.290000,19.869090,-17.649270,12.000000,-12.000000
+122.300000,19.886347,-17.665535,12.000000,-12.000000
+122.310000,19.903604,-17.681404,12.000000,-12.000000
+122.320000,19.921258,-17.698066,12.000000,-12.000000
+122.330000,19.938119,-17.713934,12.000000,-12.000000
+122.340000,19.955177,-17.729803,12.000000,-12.000000
+122.350000,19.972435,-17.746068,12.000000,-12.000000
+122.360000,19.989692,-17.762532,12.000000,-12.000000
+122.370000,20.005957,-17.778400,12.000000,-12.000000
+122.380000,20.023611,-17.794864,12.000000,-12.000000
+122.390000,20.040075,-17.810534,12.000000,-12.000000
+122.400000,20.057530,-17.826998,12.000000,-12.000000
+122.410000,20.074589,-17.842867,12.000000,-12.000000
+122.420000,20.092243,-17.859925,12.000000,-12.000000
+122.430000,20.109103,-17.875199,12.000000,-12.000000
+122.440000,20.126360,-17.891663,12.000000,-12.000000
+122.450000,0.0,-0.0,12.000000,-12.000000
+122.460000,0.0,-0.0,12.000000,-12.000000
+122.470000,0.0,-0.0,12.000000,-12.000000
+122.480000,20.195587,-17.957319,12.000000,-12.000000
+122.490000,20.212249,-17.973188,12.000000,-12.000000
+122.500000,20.229506,-17.989651,12.000000,-12.000000
+122.510000,20.246565,-18.005520,12.000000,-12.000000
+122.520000,20.263822,-18.022182,12.000000,-12.000000
+122.530000,20.281079,-18.038051,12.000000,-12.000000
+122.540000,20.298733,-18.054911,12.000000,-12.000000
+122.550000,20.314998,-18.070185,12.000000,-12.000000
+122.560000,20.333049,-18.087243,12.000000,-12.000000
+122.570000,20.350108,-18.102914,12.000000,-12.000000
+122.580000,20.367761,-18.119576,12.000000,-12.000000
+122.590000,20.385018,-18.135643,12.000000,-12.000000
+122.600000,20.402276,-18.152106,12.000000,-12.000000
+122.610000,20.419929,-18.167777,12.000000,-12.000000
+122.620000,20.437782,-18.184835,12.000000,-12.000000
+122.630000,20.455039,-18.200704,12.000000,-12.000000
+122.640000,20.472296,-18.216969,12.000000,-12.000000
+122.650000,20.489355,-18.233036,12.000000,-12.000000
+122.660000,20.507604,-18.249698,12.000000,-12.000000
+122.670000,20.524861,-18.265170,12.000000,-12.000000
+122.680000,20.542118,-18.281634,12.000000,-12.000000
+122.690000,20.559177,-18.297106,12.000000,-12.000000
+122.700000,20.576235,-18.313371,12.000000,-12.000000
+122.710000,20.593492,-18.329240,12.000000,-12.000000
+122.720000,20.610948,-18.346100,12.000000,-12.000000
+122.730000,20.628205,-18.361770,12.000000,-12.000000
+122.740000,20.646454,-18.378631,12.000000,-12.000000
+122.750000,20.662918,-18.393904,12.000000,-12.000000
+122.760000,20.681166,-18.410765,12.000000,-12.000000
+122.770000,20.697829,-18.426435,12.000000,-12.000000
+122.780000,20.715681,-18.442700,12.000000,-12.000000
+122.790000,20.732739,-18.458767,12.000000,-12.000000
+122.800000,20.749997,-18.474636,12.000000,-12.000000
+122.810000,20.767254,-18.491100,12.000000,-12.000000
+122.820000,20.784709,-18.507762,12.000000,-12.000000
+122.830000,20.802958,-18.525217,12.000000,-12.000000
+122.840000,20.820017,-18.541681,12.000000,-12.000000
+122.850000,20.835290,-18.555962,12.000000,-12.000000
+122.860000,20.852547,-18.572228,12.000000,-12.000000
+122.870000,0.0,-0.0,12.000000,-12.000000
+122.880000,0.0,-0.0,12.000000,-12.000000
+122.890000,0.0,-0.0,12.000000,-12.000000
+122.900000,0.0,-0.0,12.000000,-12.000000
+122.910000,20.938833,-18.652959,12.000000,-12.000000
+122.920000,20.956289,-18.669026,12.000000,-12.000000
+122.930000,20.973744,-18.685292,12.000000,-12.000000
+122.940000,20.991596,-18.702350,12.000000,-12.000000
+122.950000,21.008258,-18.718021,12.000000,-12.000000
+122.960000,21.025912,-18.734683,12.000000,-12.000000
+122.970000,21.042772,-18.750948,12.000000,-12.000000
+122.980000,21.061021,-18.768205,12.000000,-12.000000
+122.990000,21.077882,-18.783875,12.000000,-12.000000
+123.000000,21.094941,-18.800141,12.000000,-12.000000
+123.010000,21.111999,-18.816406,12.000000,-12.000000
diff --git a/frc971/control_loops/matlab/drivetrain_spin_low.mat b/frc971/control_loops/matlab/drivetrain_spin_low.mat
new file mode 100644
index 0000000..457083f
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_spin_low.mat
Binary files differ
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low.csv b/frc971/control_loops/matlab/drivetrain_strait_low.csv
new file mode 100644
index 0000000..6966f5b
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low.csv
@@ -0,0 +1,103 @@
+11.590000,15.318369,-16.398228,12.000000,12.000000
+11.600000,15.318369,-16.398228,12.000000,12.000000
+11.610000,15.318369,-16.398228,12.000000,12.000000
+11.620000,15.318369,-16.398228,12.000000,12.000000
+11.630000,15.318369,-16.398228,12.000000,12.000000
+11.640000,15.321146,-16.395649,12.000000,12.000000
+11.650000,15.329278,-16.388707,12.000000,12.000000
+11.660000,15.338403,-16.380772,12.000000,12.000000
+11.670000,15.347924,-16.372045,12.000000,12.000000
+11.680000,15.358635,-16.362325,12.000000,12.000000
+11.690000,15.369545,-16.351812,12.000000,12.000000
+11.700000,15.381050,-16.341101,12.000000,12.000000
+11.710000,15.392555,-16.329993,12.000000,12.000000
+11.720000,15.404258,-16.319083,12.000000,12.000000
+11.730000,15.415961,-16.308173,12.000000,12.000000
+11.740000,15.428061,-16.294685,12.000000,12.000000
+11.750000,15.440954,-16.283379,12.000000,12.000000
+11.760000,15.453847,-16.268502,12.000000,12.000000
+11.770000,15.466939,-16.255014,12.000000,12.000000
+11.780000,15.479038,-16.241327,12.000000,12.000000
+11.790000,15.491138,-16.228235,12.000000,12.000000
+11.800000,15.503635,-16.216136,12.000000,12.000000
+11.810000,15.515933,-16.204234,12.000000,12.000000
+11.820000,15.528628,-16.192134,12.000000,12.000000
+11.830000,15.541521,-16.179043,12.000000,12.000000
+11.840000,15.554811,-16.165951,12.000000,12.000000
+11.850000,15.568299,-16.152859,12.000000,12.000000
+11.860000,15.581589,-16.139569,12.000000,12.000000
+11.870000,15.595673,-16.126081,12.000000,12.000000
+11.880000,15.610153,-16.112791,12.000000,12.000000
+11.890000,15.624435,-16.098708,12.000000,12.000000
+11.900000,15.638716,-16.084228,12.000000,12.000000
+11.910000,15.653395,-16.069946,12.000000,12.000000
+11.920000,15.668272,-16.055466,12.000000,12.000000
+11.930000,15.683347,-16.040787,12.000000,12.000000
+11.940000,15.698620,-16.026307,12.000000,12.000000
+11.950000,15.714092,-16.011430,12.000000,12.000000
+11.960000,15.728969,-15.996554,12.000000,12.000000
+11.970000,15.744243,-15.981478,12.000000,12.000000
+11.980000,15.759715,-15.966205,12.000000,12.000000
+11.990000,15.774988,-15.950931,12.000000,12.000000
+12.000000,15.789865,-15.935459,12.000000,12.000000
+12.010000,15.805535,-15.920186,12.000000,12.000000
+12.020000,15.821205,-15.904516,12.000000,12.000000
+12.030000,15.836876,-15.888845,12.000000,12.000000
+12.040000,15.852744,-15.873175,12.000000,12.000000
+12.050000,15.868613,-15.857703,12.000000,12.000000
+12.060000,15.884680,-15.841835,12.000000,12.000000
+12.070000,15.900747,-15.826164,12.000000,12.000000
+12.080000,15.916615,-15.810296,12.000000,12.000000
+12.090000,15.932881,-15.794427,12.000000,12.000000
+12.100000,15.949146,-15.778162,12.000000,12.000000
+12.110000,15.965213,-15.762293,12.000000,12.000000
+12.120000,15.981677,-15.746226,12.000000,12.000000
+12.130000,0.0,-0.0,12.000000,12.000000
+12.140000,0.0,-0.0,12.000000,12.000000
+12.150000,0.0,-0.0,12.000000,12.000000
+12.160000,16.048127,-15.680173,12.000000,12.000000
+12.170000,16.064194,-15.663709,12.000000,12.000000
+12.180000,16.081649,-15.647047,12.000000,12.000000
+12.190000,16.098113,-15.630385,12.000000,12.000000
+12.200000,16.114973,-15.613525,12.000000,12.000000
+12.210000,16.131834,-15.597061,12.000000,12.000000
+12.220000,16.148892,-15.580399,12.000000,12.000000
+12.230000,16.165951,-15.563340,12.000000,12.000000
+12.240000,16.183010,-15.546877,12.000000,12.000000
+12.250000,16.200069,-15.530016,12.000000,12.000000
+12.260000,16.217326,-15.512958,12.000000,12.000000
+12.270000,16.234583,-15.495700,12.000000,12.000000
+12.280000,16.251840,-15.478840,12.000000,12.000000
+12.290000,16.268899,-15.461980,12.000000,12.000000
+12.300000,16.286156,-15.445119,12.000000,12.000000
+12.310000,16.303215,-15.428061,12.000000,12.000000
+12.320000,16.320273,-15.411002,12.000000,12.000000
+12.330000,16.337729,-15.393943,12.000000,12.000000
+12.340000,16.354589,-15.376884,12.000000,12.000000
+12.350000,16.371648,-15.359825,12.000000,12.000000
+12.360000,16.389103,-15.342767,12.000000,12.000000
+12.370000,16.406162,-15.325708,12.000000,12.000000
+12.380000,16.423221,-15.308649,12.000000,12.000000
+12.390000,16.440676,-15.291590,12.000000,12.000000
+12.400000,16.457934,-15.274333,12.000000,12.000000
+12.410000,16.475191,-15.257671,12.000000,12.000000
+12.420000,16.492646,-15.240216,12.000000,12.000000
+12.430000,16.509903,-15.222959,12.000000,12.000000
+12.440000,16.527557,-15.205900,12.000000,12.000000
+12.450000,16.544616,-15.189040,12.000000,12.000000
+12.460000,16.561873,-15.171782,12.000000,12.000000
+12.470000,16.579130,-15.154922,12.000000,12.000000
+12.480000,16.596982,-15.137665,12.000000,12.000000
+12.490000,16.614239,-15.120209,12.000000,12.000000
+12.500000,16.631100,-15.103151,12.000000,12.000000
+12.510000,16.648159,-15.085894,12.000000,12.000000
+12.520000,16.666011,-15.068636,12.000000,12.000000
+12.530000,16.683069,-15.051379,12.000000,12.000000
+12.540000,16.700327,-15.034321,12.000000,12.000000
+12.550000,16.717782,-15.017262,12.000000,12.000000
+12.560000,16.734841,-15.000203,12.000000,12.000000
+12.570000,16.752495,-14.982946,12.000000,12.000000
+12.580000,16.769752,-14.965887,12.000000,12.000000
+12.590000,16.787009,-14.948828,12.000000,12.000000
+12.600000,16.804464,-14.931175,12.000000,12.000000
+12.610000,16.821920,-14.914314,12.000000,12.000000
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low.mat b/frc971/control_loops/matlab/drivetrain_strait_low.mat
new file mode 100644
index 0000000..e6a4973
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low.mat
Binary files differ
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.csv b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.csv
new file mode 100644
index 0000000..0091dd2
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.csv
@@ -0,0 +1,185 @@
+31783,-2.373349,-1.651328,1.000000
+31784,-2.373349,-1.651328,1.000000
+31785,-2.373349,-1.651328,1.000000
+31786,-2.373349,-1.651328,1.000000
+31787,-2.370771,-1.651328,1.000000
+31788,-2.364622,-1.649344,1.000000
+31789,-2.356489,-1.639624,1.000000
+31790,-2.348158,-1.632880,1.000000
+31791,-2.336257,-1.622367,1.000000
+31792,-2.325744,-1.612449,1.000000
+31793,-2.312652,-1.599755,1.000000
+31794,-2.301941,-1.589440,1.000000
+31795,-2.288651,-1.577142,1.000000
+31796,-2.278138,-1.567224,1.000000
+31797,-2.263856,-1.553934,1.000000
+31798,-2.251558,-1.542231,1.000000
+31799,-2.236681,-1.526957,1.000000
+31800,-2.223589,-1.513072,1.000000
+31801,-2.211093,-1.499386,1.000000
+31802,-2.198200,-1.486294,1.000000
+31803,-2.185505,-1.473004,1.000000
+31804,-2.173603,-1.462491,1.000000
+31805,-2.160115,-1.450193,1.000000
+31806,-2.145238,-1.436506,1.000000
+31807,-2.131353,-1.423613,1.000000
+31808,-2.117666,-1.410521,1.000000
+31809,-2.103980,-1.397231,1.000000
+31810,-2.089500,-1.383148,1.000000
+31811,-2.076011,-1.369660,1.000000
+31812,-2.063316,-1.357163,1.000000
+31813,-2.047646,-1.341493,1.000000
+31814,-2.034356,-1.328600,1.000000
+31815,-2.017496,-1.312533,1.000000
+31816,-2.002619,-1.297854,1.000000
+31817,-1.987544,-1.282977,1.000000
+31818,-1.974055,-1.269886,1.000000
+31819,-1.957195,-1.253819,1.000000
+31820,-1.941525,-1.238347,1.000000
+31821,-1.927441,-1.224859,1.000000
+31822,-1.910383,-1.208197,1.000000
+31823,-1.896299,-1.194311,1.000000
+31824,-1.879042,-1.177253,1.000000
+31825,-1.862578,-1.161384,1.000000
+31826,-1.846908,-1.145515,1.000000
+31827,-1.829254,-1.128060,1.000000
+31828,-1.814378,-1.114175,1.000000
+31829,-1.798906,-1.098306,1.000000
+31830,-1.782442,-1.081644,1.000000
+31831,-1.765978,-1.065776,1.000000
+31832,-1.751697,-1.051097,1.000000
+31833,-1.735233,-1.035229,1.000000
+31834,-1.718571,-1.019162,1.000000
+31835,-1.700124,-1.000913,1.000000
+31836,-1.683461,-0.984846,1.000000
+31837,-1.666799,-0.968382,1.000000
+31838,-1.650137,-0.952117,1.000000
+31839,-1.633277,-0.935851,1.000000
+31840,-1.616020,-0.919189,1.000000
+31841,-1.599358,-0.902527,1.000000
+31842,-1.582696,-0.885469,1.000000
+31843,-1.565835,-0.868608,1.000000
+31844,-1.548975,-0.851748,1.000000
+31845,-1.533305,-0.836474,1.000000
+31846,-1.516643,-0.820209,1.000000
+31847,-1.497997,-0.801762,1.000000
+31848,-1.481137,-0.784901,1.000000
+31849,-1.463880,-0.767843,1.000000
+31850,-1.446424,-0.750784,1.000000
+31851,-1.430952,-0.735709,1.000000
+31852,-1.411711,-0.717261,1.000000
+31853,-1.396240,-0.701789,1.000000
+31854,-1.377197,-0.682747,1.000000
+31855,-1.359543,-0.665688,1.000000
+31856,-1.342286,-0.648630,1.000000
+31857,-1.325029,-0.631174,1.000000
+31864,-1.204626,-0.512358,1.000000
+31865,-1.187171,-0.495299,1.000000
+31866,-1.169517,-0.478240,1.000000
+31867,-1.152260,-0.460785,1.000000
+31868,-1.134606,-0.443726,1.000000
+31869,-1.117150,-0.426667,1.000000
+31870,-1.097116,-0.407228,1.000000
+31871,-1.081446,-0.391756,1.000000
+31872,-1.062205,-0.373111,1.000000
+31873,-1.044551,-0.355457,1.000000
+31874,-1.027096,-0.337803,1.000000
+31875,-1.009045,-0.320546,1.000000
+31876,-0.993574,-0.304876,1.000000
+31877,-0.974134,-0.286032,1.000000
+31878,-0.958861,-0.270560,1.000000
+31879,-0.939422,-0.250724,1.000000
+31880,-0.921371,-0.233467,1.000000
+31881,-0.903916,-0.216408,1.000000
+31882,-0.886659,-0.199349,1.000000
+31883,-0.870592,-0.183679,1.000000
+31884,-0.850954,-0.164439,1.000000
+31885,-0.834887,-0.148768,1.000000
+31886,-0.815448,-0.129726,1.000000
+31887,-0.797795,-0.112667,1.000000
+31888,-0.779942,-0.095013,1.000000
+31889,-0.762090,-0.077955,1.000000
+31890,-0.746420,-0.061689,1.000000
+31891,-0.727179,-0.042647,1.000000
+31892,-0.709525,-0.025390,1.000000
+31893,-0.693657,-0.009720,1.000000
+31894,-0.674416,0.009323,1.000000
+31895,-0.658349,0.025390,1.000000
+31896,-0.640695,0.042449,1.000000
+31897,-0.621455,0.061491,1.000000
+31898,-0.603999,0.078748,1.000000
+31899,-0.586147,0.096402,1.000000
+31900,-0.567898,0.113659,1.000000
+31901,-0.550443,0.130718,1.000000
+31902,-0.532590,0.148570,1.000000
+31903,-0.514936,0.165827,1.000000
+31904,-0.497283,0.183282,1.000000
+31905,-0.481216,0.199151,1.000000
+31906,-0.463562,0.216607,1.000000
+31907,-0.445710,0.234062,1.000000
+31908,-0.428056,0.251518,1.000000
+31909,-0.408815,0.270560,1.000000
+31910,-0.390765,0.288412,1.000000
+31911,-0.373706,0.305471,1.000000
+31912,-0.357441,0.320943,1.000000
+31913,-0.337803,0.340580,1.000000
+31914,-0.321934,0.356052,1.000000
+31915,-0.302495,0.375491,1.000000
+31916,-0.284643,0.392550,1.000000
+31917,-0.266791,0.410005,1.000000
+31918,-0.248939,0.427659,1.000000
+31919,-0.231285,0.444916,1.000000
+31920,-0.213234,0.462570,1.000000
+31921,-0.195581,0.479827,1.000000
+31922,-0.178125,0.497084,1.000000
+31923,-0.160273,0.514738,1.000000
+31924,-0.142818,0.532194,1.000000
+31925,-0.126949,0.548261,1.000000
+31926,-0.109097,0.566113,1.000000
+31927,-0.089459,0.585552,1.000000
+31928,-0.071607,0.602809,1.000000
+31929,-0.053557,0.620463,1.000000
+31930,-0.035308,0.638315,1.000000
+31931,-0.017852,0.655572,1.000000
+31932,0.000198,0.673424,1.000000
+31933,0.016265,0.688896,1.000000
+31934,0.036101,0.708732,1.000000
+31935,0.054152,0.726187,1.000000
+31936,0.071607,0.743841,1.000000
+31937,0.089658,0.761693,1.000000
+31938,0.107113,0.779149,1.000000
+31939,0.124965,0.796803,1.000000
+31940,0.141429,0.812671,1.000000
+31941,0.158884,0.830325,1.000000
+31942,0.177927,0.849368,1.000000
+31943,0.195581,0.867418,1.000000
+31944,0.213433,0.884874,1.000000
+31945,0.229500,0.900742,1.000000
+31946,0.249336,0.919784,1.000000
+31947,0.266989,0.937240,1.000000
+31948,0.285040,0.954695,1.000000
+31949,0.301107,0.970564,1.000000
+31950,0.320943,0.989805,1.000000
+31951,0.338200,1.007260,1.000000
+31952,0.355854,1.024517,1.000000
+31953,0.372119,1.040584,1.000000
+31954,0.391360,1.059627,1.000000
+31955,0.409609,1.077479,1.000000
+31956,0.426866,1.094934,1.000000
+31957,0.444718,1.112191,1.000000
+31964,0.566906,1.232793,1.000000
+31965,0.584362,1.250248,-1.000000
+31966,0.602412,1.267902,-1.000000
+31967,0.620066,1.285556,-1.000000
+31968,0.637720,1.302615,-1.000000
+31969,0.656762,1.317690,-1.000000
+31970,0.664895,1.321062,-1.000000
+31971,0.667077,1.320070,-1.000000
+31972,0.669655,1.316698,-1.000000
+31973,0.666482,1.313524,-1.000000
+31974,0.660928,1.307177,-1.000000
+31975,0.652002,1.297656,-1.000000
+31976,0.641687,1.287143,-1.000000
+31977,0.630777,1.276630,-1.000000
+31978,0.619273,1.268101,-1.000000
+31979,0.608760,1.256992,-1.000000
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.mat b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.mat
new file mode 100644
index 0000000..e9cd690
--- /dev/null
+++ b/frc971/control_loops/matlab/drivetrain_strait_low_wgaps.mat
Binary files differ
diff --git a/frc971/control_loops/matlab/writeMat.m b/frc971/control_loops/matlab/writeMat.m
new file mode 100644
index 0000000..b1541f5
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMat.m
@@ -0,0 +1,16 @@
+function writeMat(fd, matrix, name)
+ %fprintf(fd, '%s = init_matrix(%d, %d);\n', name, size(matrix, 1), size(matrix, 2));
+ fprintf(fd, '%s << ', name);
+ first_loop = 1;
+ for i=1:size(matrix, 1)
+ for j=1:size(matrix, 2)
+ if first_loop
+ first_loop = 0;
+ else
+ fprintf(fd, ', ');
+ end
+ fprintf(fd, '%.10f', matrix(i, j));
+ end
+ end
+ fprintf(fd, '; \\\n');
+end
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/writeMatFlash.m b/frc971/control_loops/matlab/writeMatFlash.m
new file mode 100644
index 0000000..10104d0
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatFlash.m
@@ -0,0 +1,16 @@
+function writeMatFlash(fd, matrix, name)
+ %fprintf(fd, '%s = init_matrix(%d, %d);\n', name, size(matrix, 1), size(matrix, 2));
+ fprintf(fd, 'flash_matrix(%s, ', name);
+ first_loop = 1;
+ for i=1:size(matrix, 1)
+ for j=1:size(matrix, 2)
+ if first_loop
+ first_loop = 0;
+ else
+ fprintf(fd, ', ');
+ end
+ fprintf(fd, '%.10f', matrix(i, j));
+ end
+ end
+ fprintf(fd, ');\n');
+end
diff --git a/frc971/control_loops/matlab/writeMatFooter.m b/frc971/control_loops/matlab/writeMatFooter.m
new file mode 100644
index 0000000..b23664e
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatFooter.m
@@ -0,0 +1,3 @@
+function writeMatFooter(fd)
+ fprintf(fd, '\n');
+end
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/writeMatFooterFlash.m b/frc971/control_loops/matlab/writeMatFooterFlash.m
new file mode 100644
index 0000000..5326a94
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatFooterFlash.m
@@ -0,0 +1,3 @@
+function writeMatFooterFlash(fd)
+ fprintf(fd, '\n');
+end
diff --git a/frc971/control_loops/matlab/writeMatHeader.m b/frc971/control_loops/matlab/writeMatHeader.m
new file mode 100644
index 0000000..5c100f3
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatHeader.m
@@ -0,0 +1,4 @@
+function writeMatHeader(fd, number_of_states, number_of_outputs)
+ fprintf(fd, 'typedef StateFeedbackLoop<%d, %d> MatrixClass;\n', number_of_states, number_of_outputs);
+ fprintf(fd, '#define MATRIX_INIT ');
+end
\ No newline at end of file
diff --git a/frc971/control_loops/matlab/writeMatHeaderFlash.m b/frc971/control_loops/matlab/writeMatHeaderFlash.m
new file mode 100644
index 0000000..88a6cc6
--- /dev/null
+++ b/frc971/control_loops/matlab/writeMatHeaderFlash.m
@@ -0,0 +1,4 @@
+function writeMatHeaderFlash(fd, number_of_states, number_of_outputs)
+ fprintf(fd, 'typedef StateFeedbackLoop<%d, %d> MatrixClass;\n', number_of_states, number_of_outputs);
+ fprintf(fd, '#define MATRIX_INIT ');
+end