Revision 5:6da98f3faf33, committed 2017-12-12

Comitter:

bwang

Date:

Tue Dec 12 12:50:09 2017 +0000

Parent:

4:f18f6bc5e1fd

Child:

6:29986fb055a5

Commit message:

initial commit

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_RTCLib.lib	Tue Dec 12 12:50:09 2017 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/nkhorman/code/Adafruit_RTCLib/#2c4e81ecda67

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BNO055.lib	Tue Dec 12 12:50:09 2017 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/StressedDave/code/BNO055/#1f722ffec323

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Tue Dec 12 12:50:09 2017 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/mbed_official/code/SDFileSystem/#8db0d3b02cec

--- a/constants.h	Thu Feb 26 04:49:21 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,118 +0,0 @@
-#ifndef __CONSTANTS_H
-#define __CONSTANTS_H
-
-/*************************************************/
-
-#undef __NATIVE
-#define __USE_THROTTLE
-#undef __SVM
-
-#undef __DEBUG
-#define SKIP 200
-#define DBG_BUF_SZ 2000
-
-//sensor phase in degrees
-#define SENSOR_PHASE 205
-
-//prius brick specific quirks
-#define DB_COEFF 0.9f
-#define DB_OFFSET 0.05f
-
-//throttle settings
-#define THROTTLE_LPF 0.9f
-#define THROTTLE_DB .182f
-
-#define _PH_A D6
-#define _PH_B D13
-#define _PH_C D3
-#define _EN D8
-
-#define _HA D11
-#define _HB D10
-#define _HC D7
-
-#define _THROTTLE A0
-#define _ANALOGA A4
-#define _ANALOGB A5
-#define _IA A1
-#define _IB A2
-
-#define ATAN_TABLE_SIZE 2000
-#define ATAN_UPPER_BOUND 20
-/*************************************************/
-
-const float sinetab[] = { 
-    0.5, 0.508726, 0.51745, 0.526168, 0.534878, 0.543578, 0.552264,
-    0.560935, 0.569587, 0.578217, 0.586824, 0.595404, 0.603956, 0.612476,
-    0.620961, 0.62941, 0.637819, 0.646186, 0.654508, 0.662784, 0.67101,
-    0.679184, 0.687303, 0.695366, 0.703368, 0.711309, 0.719186, 0.726995,
-    0.734736, 0.742405, 0.75, 0.757519, 0.76496, 0.77232, 0.779596,
-    0.786788, 0.793893, 0.800908, 0.807831, 0.81466, 0.821394, 0.82803,
-    0.834565, 0.840999, 0.847329, 0.853553, 0.85967, 0.865677, 0.871572,
-    0.877355, 0.883022, 0.888573, 0.894005, 0.899318, 0.904508, 0.909576,
-    0.914519, 0.919335, 0.924024, 0.928584, 0.933013, 0.93731, 0.941474,
-    0.945503, 0.949397, 0.953154, 0.956773, 0.960252, 0.963592, 0.96679,
-    0.969846, 0.972759, 0.975528, 0.978152, 0.980631, 0.982963, 0.985148,
-    0.987185, 0.989074, 0.990814, 0.992404, 0.993844, 0.995134, 0.996273,
-    0.997261, 0.998097, 0.998782, 0.999315, 0.999695, 0.999924, 1.,
-    0.999924, 0.999695, 0.999315, 0.998782, 0.998097, 0.997261, 0.996273,
-    0.995134, 0.993844, 0.992404, 0.990814, 0.989074, 0.987185, 0.985148,
-    0.982963, 0.980631, 0.978152, 0.975528, 0.972759, 0.969846, 0.96679,
-    0.963592, 0.960252, 0.956773, 0.953154, 0.949397, 0.945503, 0.941474,
-    0.93731, 0.933013, 0.928584, 0.924024, 0.919335, 0.914519, 0.909576,
-    0.904508, 0.899318, 0.894005, 0.888573, 0.883022, 0.877355, 0.871572,
-    0.865677, 0.85967, 0.853553, 0.847329, 0.840999, 0.834565, 0.82803,
-    0.821394, 0.81466, 0.807831, 0.800908, 0.793893, 0.786788, 0.779596,
-    0.77232, 0.76496, 0.757519, 0.75, 0.742405, 0.734736, 0.726995,
-    0.719186, 0.711309, 0.703368, 0.695366, 0.687303, 0.679184, 0.67101,
-    0.662784, 0.654508, 0.646186, 0.637819, 0.62941, 0.620961, 0.612476,
-    0.603956, 0.595404, 0.586824, 0.578217, 0.569587, 0.560935, 0.552264,
-    0.543578, 0.534878, 0.526168, 0.51745, 0.508726, 0.5, 0.491274,
-    0.48255, 0.473832, 0.465122, 0.456422, 0.447736, 0.439065, 0.430413,
-    0.421783, 0.413176, 0.404596, 0.396044, 0.387524, 0.379039, 0.37059,
-    0.362181, 0.353814, 0.345492, 0.337216, 0.32899, 0.320816, 0.312697,
-    0.304634, 0.296632, 0.288691, 0.280814, 0.273005, 0.265264, 0.257595,
-    0.25, 0.242481, 0.23504, 0.22768, 0.220404, 0.213212, 0.206107,
-    0.199092, 0.192169, 0.18534, 0.178606, 0.17197, 0.165435, 0.159001,
-    0.152671, 0.146447, 0.14033, 0.134323, 0.128428, 0.122645, 0.116978,
-    0.111427, 0.105995, 0.100682, 0.0954915, 0.090424, 0.0854812,
-    0.0806647, 0.075976, 0.0714163, 0.0669873, 0.0626901, 0.0585262,
-    0.0544967, 0.050603, 0.0468461, 0.0432273, 0.0397476, 0.0364081,
-    0.0332098, 0.0301537, 0.0272407, 0.0244717, 0.0218476, 0.0193692,
-    0.0170371, 0.0148521, 0.012815, 0.0109262, 0.00918641, 0.00759612,
-    0.00615583, 0.00486597, 0.00372692, 0.00273905, 0.00190265,
-    0.00121797, 0.000685233, 0.000304586, 0.0000761524, 0., 0.0000761524,
-    0.000304586, 0.000685233, 0.00121797, 0.00190265, 0.00273905,
-    0.00372692, 0.00486597, 0.00615583, 0.00759612, 0.00918641,
-    0.0109262, 0.012815, 0.0148521, 0.0170371, 0.0193692, 0.0218476,
-    0.0244717, 0.0272407, 0.0301537, 0.0332098, 0.0364081, 0.0397476,
-    0.0432273, 0.0468461, 0.050603, 0.0544967, 0.0585262, 0.0626901,
-    0.0669873, 0.0714163, 0.075976, 0.0806647, 0.0854812, 0.090424,
-    0.0954915, 0.100682, 0.105995, 0.111427, 0.116978, 0.122645,
-    0.128428, 0.134323, 0.14033, 0.146447, 0.152671, 0.159001, 0.165435,
-    0.17197, 0.178606, 0.18534, 0.192169, 0.199092, 0.206107, 0.213212,
-    0.220404, 0.22768, 0.23504, 0.242481, 0.25, 0.257595, 0.265264,
-    0.273005, 0.280814, 0.288691, 0.296632, 0.304634, 0.312697, 0.320816,
-    0.32899, 0.337216, 0.345492, 0.353814, 0.362181, 0.37059, 0.379039,
-    0.387524, 0.396044, 0.404596, 0.413176, 0.421783, 0.430413, 0.439065,
-    0.447736, 0.456422, 0.465122, 0.473832, 0.48255, 0.491274
-  };
-  
-const float arctan[] = {0,0.572938698,1.145762838,1.718358002,2.290610043,2.862405226,3.433630362,4.004172941,4.57392126,5.142764558,5.710593137,6.27729849,6.842773413,7.406912128,7.969610394,8.53076561,9.090276921,9.648045316,10.20397372,10.75796709,11.30993247,11.85977912,12.40741853,12.95276451,13.49573328,14.03624347,14.5742162,15.10957512,15.64224646,16.17215902,16.69924423,17.22343619,17.74467163,18.26288994,18.77803322,19.29004622,19.79887635,20.30447371,20.80679101,21.30578362,21.80140949,22.29362916,22.78240573,23.26770481,23.74949449,24.22774532,24.70243023,25.17352452,25.64100582,26.10485401,26.56505118,27.02158159,27.47443163,27.92358972,28.36904629,28.81079374,29.24882634,29.68314018,30.11373315,30.54060485,30.96375653,31.38319106,31.79891282,32.21092772,32.61924307,33.02386756,33.42481118,33.82208522,34.21570213,34.60567555,34.9920202,35.37475184,35.75388725,36.12944414,36.50144112,36.86989765,37.23483398,37.59627115,37.95423088,38.30873557,38.65980825,39.00747255,39.35175263,39.69267315,40.03025927,40.36453657,40.69553104,41.02326902,41.34777722,41.66908262,41.9872125,42.30219437,42.61405597,42.92282523,43.22853026,43.53119929,43.83086067,44.12754288,44.42127443,44.71208393,45,45.28505128,45.56726641,45.84667402,46.12330271,46.39718103,46.66833745,46.93680039,47.20259816,47.46575899,47.72631099,47.98428214,48.2397003,48.49259316,48.7429883,48.9909131,49.2363948,49.47946045,49.72013693,49.95845092,50.19442891,50.42809719,50.65948184,50.88860875,51.11550357,51.34019175,51.56269851,51.78304886,52.00126756,52.21737916,52.43140797,52.64337807,52.8533133,53.06123727,53.26717334,53.47114463,53.67317405,53.87328423,54.07149758,54.26783626,54.46232221,54.6549771,54.84582237,55.03487923,55.22216863,55.40771131,55.59152775,55.77363819,55.95406264,56.13282089,56.30993247,56.4854167,56.65929265,56.83157918,57.0022949,57.17145821,57.33908728,57.50520006,57.66981426,57.83294741,57.99461679,58.15483947,58.31363231,58.47101196,58.62699486,58.78159724,58.93483511,59.08672432,59.23728047,59.38651898,59.53445508,59.6811038,59.82647997,59.97059824,60.11347306,60.2551187,60.39554925,60.53477862,60.67282051,60.80968848,60.9453959,61.07995596,61.21338169,61.34568594,61.47688139,61.60698058,61.73599585,61.86393941,61.99082329,62.11665938,62.2414594,62.36523492,62.48799738,62.60975803,62.73052801,62.8503183,62.96913974,63.08700302,63.20391871,63.31989721,63.43494882,63.54908369,63.66231183,63.77464314,63.88608737,63.99665416,64.10635301,64.21519331,64.32318432,64.43033519,64.53665494,64.64215248,64.74683661,64.850716,64.95379922,65.05609474,65.15761089,65.25835594,65.358338,65.45756512,65.55604522,65.65378613,65.75079559,65.84708122,65.94265055,66.03751103,66.13166999,66.22513468,66.31791228,66.41000983,66.50143432,66.59219265,66.68229161,66.77173793,66.86053824,66.94869908,67.03622694,67.1231282,67.20940916,67.29507606,67.38013505,67.46459221,67.54845354,67.63172497,67.71441235,67.79652147,67.87805803,67.95902767,68.03943598,68.11928845,68.19859051,68.27734755,68.35556486,68.43324768,68.5104012,68.58703053,68.66314071,68.73873674,68.81382356,68.88840603,68.96248897,69.03607714,69.10917524,69.1817879,69.25391972,69.32557524,69.39675893,69.46747521,69.53772848,69.60752304,69.67686317,69.7457531,69.81419699,69.88219898,69.94976313,70.01689348,70.08359401,70.14986865,70.2157213,70.28115579,70.34617594,70.4107855,70.47498818,70.53878765,70.60218755,70.66519146,70.72780293,70.79002547,70.85186254,70.91331758,70.97439396,71.03509505,71.09542416,71.15538456,71.21497949,71.27421215,71.33308573,71.39160333,71.44976807,71.50758301,71.56505118,71.62217556,71.67895914,71.73540483,71.79151553,71.84729411,71.90274341,71.95786623,72.01266535,72.0671435,72.1213034,72.17514774,72.22867918,72.28190033,72.3348138,72.38742216,72.43972795,72.49173369,72.54344187,72.59485495,72.64597536,72.69680553,72.74734783,72.79760463,72.84757826,72.89727103,72.94668523,72.99582312,73.04468695,73.09327892,73.14160123,73.18965606,73.23744554,73.28497181,73.33223696,73.37924309,73.42599224,73.47248647,73.51872779,73.56471819,73.61045967,73.65595416,73.70120362,73.74620996,73.79097507,73.83550085,73.87978914,73.9238418,73.96766065,74.01124748,74.0546041,74.09773227,74.14063373,74.18331024,74.2257635,74.26799521,74.31000707,74.35180073,74.39337786,74.43474007,74.475889,74.51682625,74.5575534,74.59807203,74.63838369,74.67848993,74.71839227,74.75809222,74.79759129,74.83689096,74.87599269,74.91489795,74.95360817,74.99212478,75.0304492,75.06858282,75.10652704,75.14428323,75.18185275,75.21923695,75.25643716,75.29345472,75.33029092,75.36694708,75.40342447,75.43972438,75.47584806,75.51179677,75.54757175,75.58317422,75.61860541,75.65386652,75.68895874,75.72388326,75.75864125,75.79323388,75.8276623,75.86192765,75.89603106,75.92997365,75.96375653,75.99738081,76.03084758,76.06415791,76.09731289,76.13031356,76.16316099,76.19585622,76.22840028,76.2607942,76.293039,76.32513567,76.35708522,76.38888865,76.42054692,76.45206101,76.48343189,76.51466051,76.54574782,76.57669475,76.60750225,76.63817122,76.66870259,76.69909725,76.72935612,76.75948008,76.78947002,76.81932681,76.84905132,76.87864441,76.90810694,76.93743974,76.96664367,76.99571956,77.02466822,77.05349048,77.08218716,77.11075904,77.13920694,77.16753164,77.19573393,77.22381459,77.25177439,77.27961409,77.30733445,77.33493623,77.36242018,77.38978702,77.41703751,77.44417235,77.47119229,77.49809803,77.52489029,77.55156976,77.57813716,77.60459317,77.63093847,77.65717376,77.68329971,77.70931699,77.73522627,77.76102821,77.78672345,77.81231266,77.83779648,77.86317554,77.88845049,77.91362195,77.93869054,77.96365689,77.98852161,78.01328532,78.0379486,78.06251208,78.08697633,78.11134196,78.13560955,78.15977968,78.18385293,78.20782987,78.23171107,78.25549709,78.2791885,78.30278585,78.32628969,78.34970056,78.37301902,78.39624559,78.41938082,78.44242523,78.46537935,78.4882437,78.5110188,78.53370516,78.5563033,78.57881373,78.60123693,78.62357342,78.64582369,78.66798823,78.69006753,78.71206207,78.73397233,78.75579879,78.77754193,78.79920221,78.8207801,78.84227607,78.86369057,78.88502406,78.90627699,78.92744981,78.94854297,78.96955692,78.99049208,79.0113489,79.03212782,79.05282925,79.07345364,79.09400139,79.11447295,79.13486871,79.1551891,79.17543453,79.1956054,79.21570213,79.23572512,79.25567476,79.27555146,79.29535561,79.3150876,79.33474782,79.35433666,79.37385449,79.39330171,79.41267868,79.43198579,79.45122341,79.4703919,79.48949164,79.50852299,79.52748631,79.54638196,79.5652103,79.58397169,79.60266648,79.62129501,79.63985764,79.65835471,79.67678656,79.69515353,79.71345597,79.7316942,79.74986857,79.7679794,79.78602701,79.80401175,79.82193393,79.83979388,79.85759191,79.87532834,79.8930035,79.91061768,79.92817122,79.9456644,79.96309755,79.98047096,79.99778494,80.0150398,80.03223582,80.04937331,80.06645257,80.08347388,80.10043754,80.11734384,80.13419306,80.15098549,80.16772142,80.18440112,80.20102488,80.21759297,80.23410567,80.25056326,80.26696601,80.28331418,80.29960806,80.3158479,80.33203397,80.34816653,80.36424586,80.3802722,80.39624582,80.41216698,80.42803592,80.44385291,80.4596182,80.47533203,80.49099466,80.50660633,80.52216729,80.53767779,80.55313807,80.56854836,80.58390891,80.59921996,80.61448174,80.62969449,80.64485844,80.65997383,80.67504088,80.69005983,80.70503089,80.71995431,80.7348303,80.7496591,80.76444091,80.77917596,80.79386447,80.80850667,80.82310275,80.83765295,80.85215748,80.86661654,80.88103036,80.89539913,80.90972308,80.9240024,80.93823731,80.952428,80.96657469,80.98067757,80.99473685,81.00875272,81.02272539,81.03665506,81.05054192,81.06438616,81.07818798,81.09194757,81.10566513,81.11934085,81.13297491,81.14656751,81.16011882,81.17362905,81.18709836,81.20052696,81.21391501,81.2272627,81.24057022,81.25383774,81.26706544,81.28025349,81.29340209,81.30651139,81.31958157,81.33261282,81.3456053,81.35855917,81.37147462,81.38435182,81.39719092,81.4099921,81.42275552,81.43548136,81.44816977,81.46082091,81.47343496,81.48601207,81.4985524,81.51105612,81.52352338,81.53595434,81.54834915,81.56070798,81.57303098,81.5853183,81.5975701,81.60978653,81.62196773,81.63411388,81.6462251,81.65830156,81.67034339,81.68235076,81.69432379,81.70626265,81.71816748,81.73003841,81.7418756,81.75367919,81.76544931,81.77718611,81.78888973,81.80056031,81.81219799,81.8238029,81.83537519,81.84691498,81.85842242,81.86989765,81.88134078,81.89275196,81.90413133,81.915479,81.92679512,81.93807982,81.94933322,81.96055545,81.97174664,81.98290693,81.99403643,82.00513527,82.01620358,82.02724149,82.03824911,82.04922657,82.060174,82.07109152,82.08197924,82.0928373,82.1036658,82.11446487,82.12523464,82.1359752,82.1466867,82.15736923,82.16802293,82.1786479,82.18924425,82.19981212,82.2103516,82.22086281,82.23134587,82.24180088,82.25222796,82.26262723,82.27299878,82.28334273,82.2936592,82.30394828,82.31421009,82.32444473,82.33465231,82.34483294,82.35498673,82.36511377,82.37521418,82.38528806,82.39533551,82.40535663,82.41535153,82.42532032,82.43526308,82.44517993,82.45507096,82.46493628,82.47477598,82.48459016,82.49437892,82.50414236,82.51388058,82.52359367,82.53328173,82.54294486,82.55258315,82.5621967,82.5717856,82.58134994,82.59088982,82.60040534,82.60989658,82.61936364,82.62880661,82.63822558,82.64762064,82.65699188,82.6663394,82.67566327,82.6849636,82.69424047,82.70349396,82.71272417,82.72193118,82.73111508,82.74027595,82.74941389,82.75852898,82.76762129,82.77669093,82.78573796,82.79476248,82.80376457,82.81274431,82.82170179,82.83063708,82.83955027,82.84844145,82.85731068,82.86615805,82.87498365,82.88378755,82.89256983,82.90133057,82.91006986,82.91878776,82.92748435,82.93615972,82.94481394,82.95344709,82.96205924,82.97065047,82.97922085,82.98777047,82.9962994,83.0048077,83.01329546,83.02176275,83.03020965,83.03863621,83.04704253,83.05542867,83.0637947,83.0721407,83.08046674,83.08877288,83.0970592,83.10532578,83.11357267,83.12179995,83.13000769,83.13819596,83.14636483,83.15451436,83.16264463,83.1707557,83.17884764,83.18692051,83.19497439,83.20300934,83.21102543,83.21902271,83.22700127,83.23496116,83.24290246,83.25082521,83.25872949,83.26661537,83.2744829,83.28233216,83.29016319,83.29797608,83.30577087,83.31354763,83.32130643,83.32904733,83.33677038,83.34447565,83.3521632,83.35983309,83.36748538,83.37512014,83.38273742,83.39033727,83.39791977,83.40548497,83.41303292,83.4205637,83.42807735,83.43557393,83.4430535,83.45051612,83.45796185,83.46539075,83.47280286,83.48019825,83.48757697,83.49493908,83.50228464,83.50961369,83.51692631,83.52422253,83.53150242,83.53876603,83.54601341,83.55324462,83.56045971,83.56765874,83.57484176,83.58200882,83.58915998,83.59629528,83.60341479,83.61051855,83.61760661,83.62467903,83.63173585,83.63877714,83.64580293,83.65281329,83.65980825,83.66678788,83.67375223,83.68070133,83.68763525,83.69455402,83.70145771,83.70834636,83.71522002,83.72207873,83.72892255,83.73575153,83.7425657,83.74936513,83.75614985,83.76291992,83.76967539,83.77641629,83.78314268,83.7898546,83.7965521,83.80323522,83.80990402,83.81655854,83.82319882,83.8298249,83.83643685,83.84303469,83.84961847,83.85618824,83.86274405,83.86928593,83.87581394,83.88232811,83.88882849,83.89531512,83.90178805,83.90824732,83.91469297,83.92112505,83.92754359,83.93394865,83.94034026,83.94671846,83.9530833,83.95943482,83.96577306,83.97209806,83.97840986,83.98470851,83.99099404,83.9972665,84.00352593,84.00977236,84.01600584,84.02222641,84.0284341,84.03462896,84.04081103,84.04698035,84.05313695,84.05928087,84.06541216,84.07153085,84.07763698,84.08373059,84.08981172,84.0958804,84.10193668,84.10798059,84.11401217,84.12003145,84.12603848,84.1320333,84.13801593,84.14398641,84.14994479,84.1558911,84.16182538,84.16774766,84.17365797,84.17955636,84.18544286,84.19131751,84.19718034,84.20303138,84.20887068,84.21469827,84.22051417,84.22631844,84.2321111,84.23789219,84.24366174,84.24941978,84.25516636,84.2609015,84.26662524,84.27233761,84.27803865,84.28372839,84.28940686,84.2950741,84.30073014,84.30637501,84.31200875,84.31763138,84.32324295,84.32884348,84.33443301,84.34001156,84.34557918,84.35113589,84.35668172,84.36221671,84.36774089,84.37325428,84.37875693,84.38424886,84.38973011,84.3952007,84.40066066,84.40611003,84.41154884,84.41697712,84.42239489,84.4278022,84.43319906,84.43858551,84.44396158,84.44932729,84.45468269,84.4600278,84.46536264,84.47068725,84.47600165,84.48130588,84.48659997,84.49188394,84.49715782,84.50242165,84.50767544,84.51291924,84.51815306,84.52337693,84.52859089,84.53379496,84.53898917,84.54417355,84.54934812,84.55451292,84.55966797,84.5648133,84.56994893,84.57507489,84.58019122,84.58529793,84.59039506,84.59548262,84.60056066,84.60562919,84.61068824,84.61573784,84.62077801,84.62580879,84.63083019,84.63584224,84.64084497,84.6458384,84.65082257,84.65579749,84.6607632,84.66571971,84.67066705,84.67560525,84.68053434,84.68545433,84.69036526,84.69526714,84.70016001,84.70504389,84.70991879,84.71478476,84.71964181,84.72448996,84.72932925,84.73415969,84.73898131,84.74379413,84.74859818,84.75339348,84.75818005,84.76295793,84.76772712,84.77248766,84.77723957,84.78198288,84.7867176,84.79144375,84.79616137,84.80087048,84.80557109,84.81026324,84.81494694,84.81962221,84.82428909,84.82894758,84.83359772,84.83823953,84.84287303,84.84749823,84.85211518,84.85672387,84.86132435,84.86591662,84.87050072,84.87507665,84.87964446,84.88420415,84.88875574,84.89329927,84.89783475,84.9023622,84.90688164,84.9113931,84.91589659,84.92039214,84.92487977,84.92935949,84.93383134,84.93829532,84.94275147,84.94719979,84.95164032,84.95607307,84.96049807,84.96491532,84.96932486,84.9737267,84.97812087,84.98250737,84.98688624,84.9912575,84.99562116,84.99997724,85.00432576,85.00866674,85.01300021,85.01732617,85.02164466,85.02595569,85.03025927,85.03455544,85.0388442,85.04312557,85.04739958,85.05166625,85.05592559,85.06017762,85.06442236,85.06865983,85.07289005,85.07711304,85.08132881,85.08553738,85.08973878,85.09393301,85.09812011,85.10230008,85.10647294,85.11063872,85.11479743,85.11894909,85.12309371,85.12723132,85.13136193,85.13548556,85.13960223,85.14371195,85.14781475,85.15191063,85.15599962,85.16008174,85.164157,85.16822542,85.17228701,85.1763418,85.1803898,85.18443103,85.1884655,85.19249323,85.19651424,85.20052855,85.20453616,85.20853711,85.2125314,85.21651905,85.22050008,85.22447451,85.22844234,85.2324036,85.23635831,85.24030647,85.24424812,85.24818325,85.25211189,85.25603406,85.25994976,85.26385902,85.26776186,85.27165828,85.2755483,85.27943195,85.28330922,85.28718015,85.29104475,85.29490302,85.298755,85.30260068,85.30644009,85.31027325,85.31410016,85.31792085,85.32173532,85.3255436,85.3293457,85.33314163,85.33693141,85.34071505,85.34449257,85.34826399,85.35202931,85.35578855,85.35954174,85.36328887,85.36702997,85.37076505,85.37449413,85.37821722,85.38193433,85.38564548,85.38935068,85.39304995,85.3967433,85.40043075,85.40411231,85.40778799,85.41145781,85.41512178,85.41877991,85.42243223,85.42607874,85.42971946,85.43335439,85.43698357,85.44060699,85.44422467,85.44783663,85.45144288,85.45504343,85.4586383,85.46222749,85.46581103,85.46938893,85.47296119,85.47652784,85.48008889,85.48364434,85.48719422,85.49073853,85.49427729,85.49781051,85.5013382,85.50486039,85.50837707,85.51188827,85.51539399,85.51889425,85.52238906,85.52587844,85.5293624,85.53284094,85.53631408,85.53978184,85.54324423,85.54670126,85.55015293,85.55359928,85.55704029,85.560476,85.56390641,85.56733153,85.57075138,85.57416596,85.5775753,85.58097939,85.58437827,85.58777192,85.59116038,85.59454364,85.59792173,85.60129465,85.60466241,85.60802503,85.61138252,85.61473489,85.61808216,85.62142432,85.62476141,85.62809342,85.63142037,85.63474226,85.63805913,85.64137096,85.64467778,85.64797959,85.65127641,85.65456825,85.65785512,85.66113703,85.66441399,85.66768602,85.67095312,85.6742153,85.67747258,85.68072497,85.68397248,85.68721512,85.6904529,85.69368582,85.69691392,85.70013718,85.70335563,85.70656927,85.70977812,85.71298218,85.71618147,85.719376,85.72256578,85.72575081,85.72893112,85.7321067,85.73527757,85.73844375,85.74160523,85.74476204,85.74791418,85.75106166,85.75420449,85.75734268,85.76047625,85.7636052,85.76672954,85.76984929,85.77296445,85.77607504,85.77918105,85.78228252,85.78537943,85.78847181,85.79155966,85.794643,85.79772183,85.80079616,85.80386601,85.80693137,85.80999227,85.81304872,85.81610071,85.81914827,85.8221914,85.82523011,85.82826441,85.83129431,85.83431982,85.83734095,85.84035771,85.8433701,85.84637815,85.84938185,85.85238122,85.85537626,85.85836699,85.86135341,85.86433554,85.86731338,85.87028694,85.87325623,85.87622127,85.87918205,85.88213859,85.8850909,85.88803899,85.89098286,85.89392253,85.896858,85.89978929,85.90271639,85.90563933,85.90855811,85.91147274,85.91438322,85.91728957,85.92019179,85.9230899,85.9259839,85.92887381,85.93175962,85.93464135,85.93751901,85.9403926,85.94326214,85.94612763,85.94898908,85.9518465,85.95469991,85.95754929,85.96039468,85.96323606,85.96607346,85.96890688,85.97173633,85.97456182,85.97738335,85.98020094,85.98301459,85.9858243,85.9886301,85.99143199,85.99422996,85.99702405,85.99981424,86.00260055,86.00538299,86.00816157,86.01093629,86.01370716,86.01647419,86.01923738,86.02199676,86.02475231,86.02750406,86.03025201,86.03299616,86.03573653,86.03847312,86.04120594,86.043935,86.0466603,86.04938186,86.05209968,86.05481377,86.05752414,86.06023079,86.06293373,86.06563297,86.06832852,86.07102038,86.07370857,86.07639309,86.07907394,86.08175114,86.08442469,86.0870946,86.08976087,86.09242352,86.09508256,86.09773798,86.1003898,86.10303802,86.10568265,86.10832371,86.11096118,86.11359509,86.11622545,86.11885224,86.1214755,86.12409521,86.1267114,86.12932405,86.1319332,86.13453883,86.13714096,86.13973959,86.14233474,86.1449264,86.14751459,86.15009931,86.15268058,86.15525838,86.15783274,86.16040366,86.16297115,86.16553521,86.16809585,86.17065308,86.1732069,86.17575733,86.17830436,86.180848,86.18338827,86.18592517,86.1884587,86.19098887,86.19351569,86.19603916,86.1985593,86.2010761,86.20358958,86.20609974,86.20860658,86.21111012,86.21361036,86.21610731,86.21860097,86.22109136,86.22357847,86.22606231,86.22854289,86.23102022,86.2334943,86.23596514,86.23843274,86.24089711,86.24335827,86.2458162,86.24827093,86.25072245,86.25317078,86.25561591,86.25805786,86.26049664,86.26293224,86.26536467,86.26779394,86.27022006,86.27264304,86.27506287,86.27747956,86.27989313,86.28230357,86.28471089,86.28711511,86.28951622,86.29191423,86.29430914,86.29670097,86.29908972,86.30147539,86.30385799,86.30623753,86.30861401,86.31098744,86.31335783,86.31572517,86.31808948,86.32045076,86.32280902,86.32516426,86.32751649,86.32986572,86.33221194,86.33455518,86.33689542,86.33923268,86.34156697,86.34389828,86.34622663,86.34855202,86.35087446,86.35319394,86.35551049,86.35782409,86.36013477,86.36244252,86.36474735,86.36704926,86.36934826,86.37164436,86.37393757,86.37622787,86.3785153,86.38079983,86.3830815,86.38536029,86.38763621,86.38990928,86.39217949,86.39444685,86.39671136,86.39897304,86.40123188,86.4034879,86.40574109,86.40799146,86.41023902,86.41248378,86.41472573,86.41696489,86.41920125,86.42143483,86.42366563,86.42589365,86.4281189,86.43034138,86.4325611,86.43477807,86.43699229,86.43920376,86.4414125,86.4436185,86.44582176,86.44802231,86.45022013,86.45241524,86.45460764,86.45679734,86.45898433,86.46116863,86.46335024,86.46552917,86.46770542,86.46987899,86.47204989,86.47421813,86.4763837,86.47854662,86.48070689,86.48286452,86.4850195,86.48717185,86.48932157,86.49146866,86.49361313,86.49575498,86.49789423,86.50003086,86.50216489,86.50429633,86.50642517,86.50855142,86.51067509,86.51279619,86.5149147,86.51703065,86.51914404,86.52125486,86.52336313,86.52546885,86.52757202,86.52967265,86.53177074,86.5338663,86.53595934,86.53804985,86.54013784,86.54222331,86.54430628,86.54638674,86.54846471,86.55054017,86.55261315,86.55468364,86.55675164,86.55881717,86.56088022,86.56294081,86.56499893,86.56705458,86.56910779,86.57115854,86.57320684,86.5752527,86.57729613,86.57933712,86.58137567,86.58341181,86.58544552,86.58747682,86.5895057,86.59153217,86.59355625,86.59557792,86.59759719,86.59961408,86.60162857,86.60364069,86.60565043,86.60765779,86.60966278,86.61166541,86.61366567,86.61566358,86.61765913,86.61965234,86.6216432,86.62363171,86.6256179,86.62760175,86.62958327,86.63156246,86.63353934,86.6355139,86.63748614,86.63945608,86.64142372,86.64338905,86.64535209,86.64731283,86.64927129,86.65122746,86.65318136,86.65513298,86.65708232,86.6590294,86.66097421,86.66291676,86.66485706,86.6667951,86.6687309,86.67066445,86.67259576,86.67452483,86.67645167,86.67837628,86.68029867,86.68221883,86.68413678,86.68605251,86.68796603,86.68987735,86.69178647,86.69369338,86.6955981,86.69750063,86.69940098,86.70129914,86.70319512,86.70508892,86.70698056,86.70887002,86.71075732,86.71264246,86.71452544,86.71640627,86.71828495,86.72016148,86.72203587,86.72390812,86.72577824,86.72764622,86.72951208,86.73137581,86.73323742,86.73509691,86.7369543,86.73880957,86.74066273,86.74251379,86.74436276,86.74620962,86.7480544,86.74989709,86.75173769,86.75357621,86.75541265,86.75724702,86.75907932,86.76090955,86.76273772,86.76456383,86.76638788,86.76820987,86.77002982,86.77184772,86.77366358,86.77547739,86.77728918,86.77909892,86.78090664,86.78271234,86.78451601,86.78631766,86.7881173,86.78991492,86.79171054,86.79350415,86.79529575,86.79708536,86.79887297,86.80065859,86.80244223,86.80422387,86.80600354,86.80778122,86.80955693,86.81133067,86.81310243,86.81487224,86.81664008,86.81840596,86.82016988,86.82193185,86.82369187,86.82544995,86.82720608,86.82896027,86.83071253,86.83246285,86.83421124,86.83595771,86.83770225,86.83944487,86.84118557,86.84292436,86.84466124,86.8463962,86.84812927,86.84986043,86.85158969,86.85331706,86.85504254,86.85676612,86.85848782,86.86020764,86.86192557,86.86364163,86.86535582,86.86706813,86.86877858,86.87048716,86.87219388,86.87389874,86.87560175,86.8773029,86.8790022,86.88069966,86.88239527,86.88408904,86.88578098,86.88747108,86.88915935,86.89084579,86.8925304,86.89421319,86.89589416,86.89757332,86.89925066,86.90092619,86.90259991,86.90427183,86.90594194,86.90761026,86.90927677,86.9109415,86.91260443,86.91426558,86.91592494,86.91758252,86.91923832,86.92089235,86.9225446,86.92419508,86.92584379,86.92749074,86.92913593,86.93077936,86.93242103,86.93406094,86.93569911,86.93733553,86.9389702,86.94060313,86.94223432,86.94386378,86.9454915,86.94711749,86.94874175,86.95036428,86.95198509,86.95360418,86.95522156,86.95683721,86.95845116,86.9600634,86.96167393,86.96328275,86.96488988,86.96649531,86.96809904,86.96970107,86.97130142,86.97290008,86.97449705,86.97609235,86.97768596,86.97927789,86.98086815,86.98245674,86.98404366,86.98562891,86.9872125,86.98879442,86.99037469,86.9919533,86.99353026,86.99510556,86.99667922,86.99825123,86.9998216,87.00139033,87.00295742,87.00452287,87.0060867,87.00764889,87.00920945,87.01076839,87.0123257,87.01388139,87.01543547,87.01698793,87.01853878,87.02008802,87.02163565,87.02318167,87.0247261,87.02626892,87.02781014,87.02934977,87.03088781,87.03242425,87.03395911,87.03549238,87.03702407,87.03855418,87.04008271,87.04160967,87.04313505,87.04465886,87.0461811,87.04770178,87.04922089,87.05073844,87.05225444,87.05376887,87.05528176,87.05679309,87.05830287,87.0598111,87.0613178,87.06282294,87.06432655,87.06582863,87.06732916,87.06882817,87.07032564,87.07182159,87.07331601,87.0748089,87.07630028,87.07779014,87.07927848,87.08076531,87.08225062,87.08373443,87.08521673,87.08669752,87.08817681,87.0896546,87.0911309,87.0926057,87.094079,87.09555081,87.09702114,87.09848998,87.09995733,87.1014232,87.10288759,87.10435051,87.10581194,87.10727191,87.1087304,87.11018743,87.11164299,87.11309708,87.11454972,87.11600089,87.1174506,87.11889886,87.12034567,87.12179102,87.12323493,87.12467739,87.1261184,87.12755798,87.12899611,87.1304328,87.13186806,87.13330188,87.13473428,87.13616524,87.13759477};
-
- 
-static unsigned char ATable[] = {0, 2, 4, 3, 0, 1, 5, 0};
-static unsigned char HNext[] = {0, 3, 6, 2, 5, 1, 4, 0};
-
-typedef struct {
-  unsigned char halt;
-  
-  float dtc_a, dtc_b, dtc_c;
-  
-  float throttle;
-  float sensor_phase;
-  float angle;
-  unsigned char debug_stop;
-} Motor;
-
-#endif
\ No newline at end of file

--- a/isr.cpp	Thu Feb 26 04:49:21 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,125 +0,0 @@
-#include "isr.h"
-#include "mbed.h"
-#include "shared.h"
-#include "constants.h"
-#include "util.h"
-
-void dtc_update() { 
-#ifndef __SVM
-    float ia = motor->angle - motor->sensor_phase;
-    if (ia < 0) ia += 360.0f;
-    if (ia >= 360.0f) ia -= 360.0f;    
-    float ib = ia - 120.0f;
-    if (ib < 0) ib += 360.0f;
-    float ic = ia + 120.0f;
-    if (ic >= 360.0f) ic -= 360.0f;
-      
-    motor->dtc_a = sinetab[(int) (ia)] * motor->throttle;
-    motor->dtc_b = sinetab[(int) (ib)] * motor->throttle;
-    motor->dtc_c = sinetab[(int) (ic)] * motor->throttle;
-#else
-    float ix = 120.0f - motor->angle;
-    if (ix < 0) ix += 360.0f;
-    if (ix >= 360.0f) ix -= 360.0f;
-    float dx = sinetab[(int) ix];
-    float iy = motor->angle;
-    if (iy < 0) iy += 360.0f;
-    if (iy >= 360.0f) iy -= 360.0f;
-    float dy = sinetab[(int) iy];
-    int sector = (int) (iy / 60.0f) + 1;
-    
-    switch(sector) {
-    case 1:
-        motor->dtc_a = 1.0f - dx - dy;
-        motor->dtc_b = 1.0f + dx - dy;
-        motor->dtc_c = 1.0f + dx + dy;
-        break;
-    case 2:
-        motor->dtc_a = 1.0f - dx + dy;
-        motor->dtc_b = 1.0f - dx - dy;
-        motor->dtc_c = 1.0f + dx + dy;
-        break;
-    case 3:
-        motor->dtc_a = 1.0f + dx + dy;
-        motor->dtc_b = 1.0f - dx - dy;
-        motor->dtc_c = 1.0f + dx - dy;
-        break;
-    case 4:
-        motor->dtc_a = 1.0f + dx + dy;
-        motor->dtc_b = 1.0f - dx + dy;
-        motor->dtc_c = 1.0f - dx + dy; //suspect
-        break;
-    case 5:
-        motor->dtc_a = 1.0f + dx - dy;
-        motor->dtc_b = 1.0f + dx + dy;
-        motor->dtc_c = 1.0f - dx - dy;
-        break;
-    case 6:
-        motor->dtc_a = 1.0f - dx - dy;
-        motor->dtc_b = 1.0f + dx + dy;
-        motor->dtc_c = 1.0f - dx + dy;
-        break;
-    default:
-        break;
-    }   
-#endif    
-    if (motor->halt || motor->debug_stop) {
-        setDtcA(0);
-        setDtcB(1.0f);
-        setDtcC(0);
-    } else {
-        setDtcA(DB_COEFF * motor->dtc_a + DB_OFFSET);
-        setDtcB(DB_COEFF * (1.0f - motor->dtc_b) + DB_OFFSET);
-        setDtcC(DB_COEFF * motor->dtc_c + DB_OFFSET);
-    }
-}
-
-void pos_update() {
-    float ascaled = 2*(((float)analoga-0.256f)/(0.484f-0.256f)-0.5f);
-    float bscaled = 2*(((float)analogb-0.254f)/(0.474f-0.254f)-0.5f);
-    
-    float x = bscaled / ascaled;
-    
-    unsigned int index = (abs(x) / ATAN_UPPER_BOUND)* ATAN_TABLE_SIZE;
-    
-    if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
-
-    if(ascaled<0){
-           if(bscaled<0) motor->angle = 90 - arctan[index];
-           if(bscaled>0) motor->angle = 90 + arctan[index];
-   } 
-       
-   if(ascaled>0){
-           if(bscaled>0) motor->angle = 270 - arctan[index];
-           if(bscaled<0) motor->angle = 270 + arctan[index];
-   }
-    
-    if(motor->angle > 360.0f) motor->angle = 360.0f;
-    if(motor->angle < 0) motor->angle = 0;
-    
-#ifdef __DEBUG
-    skipidx++;
-    if (skipidx == SKIP) {
-        skipidx = 0;
-        if (!motor->halt) {
-            fbuffer[bufidx] = motor->angle;
-            bufidx++;
-        }
-    }
-    if (bufidx == DBG_BUF_SZ) {
-        motor->debug_stop = 1;
-    }
-#endif
-}
-
-void throttle_update() {
-    float throttle_raw = throttle_read;
-    throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
-    if (throttle_raw < 0.0f) throttle_raw = 0.0f;
-    motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
-    if (motor->throttle < 0.05f) {
-        motor->halt = 1;
-    } else {
-        motor->halt = 0;
-    }
-}
\ No newline at end of file

--- a/isr.h	Thu Feb 26 04:49:21 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,8 +0,0 @@
-#ifndef __ISR_H
-#define __ISR_H
-
-void dtc_update();
-void pos_update();
-void throttle_update();
-
-#endif
\ No newline at end of file

--- a/main.cpp	Thu Feb 26 04:49:21 2015 +0000
+++ b/main.cpp	Tue Dec 12 12:50:09 2017 +0000
@@ -1,67 +1,152 @@
 #include "mbed.h"
+#include "SDFileSystem.h"
+#include "DS1307.h"
+#include "BNO055.h"
+#include "string.h"
 
+#define M_PI 3.1415927f
+
+#define DI D11
+#define DO D12
+#define SCK D13
+#define CS D10
+
+#define I2C1_SDA PB_9
+#define I2C1_SCL PB_8
 
-#include "constants.h"
-#include "shared.h"
-#include "util.h"
-#include "math.h"
-#include "isr.h"
+#define I2C2_SDA D5
+#define I2C2_SCL D7
+
+#define PAGE_SIZE 512
+#define clip(a) if ((a > 254)) a = 254
 
-Serial pc(SERIAL_TX, SERIAL_RX);
+PwmOut pwm_r(D4);
+PwmOut pwm_g(D6);
+PwmOut pwm_b(D3);
+DigitalOut test(PC_8);
 
-PwmOut pha(_PH_A);
-PwmOut phb(_PH_B);
-PwmOut phc(_PH_C);
+char fname[255];
+char current_time[64];
 
-DigitalOut en(_EN);
+unsigned char buf[PAGE_SIZE];
+int buf_idx = 0;
 
-DigitalIn dummy(D5);
-
-AnalogIn throttle(_THROTTLE);
-AnalogIn analoga(_ANALOGA);
-AnalogIn analogb(_ANALOGB);
-
-Motor* motor;
+void printDT(char *pre, DateTime &dt)
+{
+    printf("%s %u/%u/%02u %2u:%02u:%02u\r\n"
+        ,pre
+        ,dt.month(),dt.day(),dt.year()
+        ,dt.hour(),dt.minute(),dt.second()
+        );
+}
 
-#ifdef __DEBUG
-float *fbuffer;
-int bufidx = 0;
-int skipidx = 0;
-#endif
-#ifdef __USE_THROTTLE
-    Ticker dtc_upd_ticker;
-    Ticker throttle_upd_ticker;
-#endif
-
-float throttle_read;
+bool rtcUpdate(RtcDs1307 &rtc, int32_t bias) // this must be signed
+{   bool bUpdated = false;
+ 
+    // Use the compiled date/time as a basis for setting the clock.
+    // We assign it to a signed integer so that negative biases work correctly
+    int64_t compiledTime = DateTime(__DATE__,__TIME__).unixtime();
+ 
+    // This assumes that the program is run VERY soon after the initial compile.
+    time_t localt = DateTime(compiledTime + bias).unixtime(); // offset by bias
+    
+    // If the stored static time stamp does not equal the compiled time stamp,
+    // then we need to update the RTC clock and the stored time stamp
+    if(*((time_t *)&rtc[0]) != localt)
+    {
+        // Update the RTC time as local time, not GMT/UTC
+        rtc.adjust(localt);
+        // Store the new compiled time statically in the object ram image
+        *((time_t *)&rtc[0]) = localt;
+        // Push the object ram image to the RTC ram image
+        bUpdated = rtc.commit();
+    }
+    
+    return bUpdated;
+}
 
 int main() {
-#ifdef __DEBUG
-    pc.printf("%s\n", "Debug mode ON");
-#endif
-    en = 1;
-#ifdef __DEBUG
-    fbuffer = (float*)malloc(DBG_BUF_SZ*sizeof(float));
-#endif
-    initTimers();
-    initPins();
-    initData();
+    printf("%s\n", "Manworm's High-Speed Logger");
+    
+    /*init RGB lights*/
+    pwm_r.period(0.01);
+    pwm_g.period(0.01);
+    pwm_b.period(0.01);
+    
+    pwm_r = 0.0f;
+    pwm_g = 0.0f;
+    pwm_b = 0.0f;
+    
+    float theta = 0.0f;
+    int tics = 0;
+    
+    /*wait for SD card to be inserted*/
+    SDFileSystem sd(DI, DO, SCK, CS, "sd");
+    while (sd.disk_status()) {
+        sd.disk_initialize();
+        wait(0.5);
+    }
+    
+    /*init RTC, get time and date for filename*/
+    I2C i2c1(I2C1_SDA, I2C1_SCL);
+    i2c1.frequency(100000);
+    RtcDs1307 rtc(i2c1);
+    
+    rtcUpdate(rtc, -(5*60*60));
+    DateTime dt = rtc.now();
+    printDT("It is now", dt);
+    
+    strcpy(fname, "/sd/log_");
+    sprintf(current_time, "%u.%u.%02u_%02u.%02u.%02u"
+        ,dt.month(),dt.day(),dt.year()
+        ,dt.hour(),dt.minute(),dt.second()
+    );    
+    strcat(fname, current_time);
+    strcat(fname, ".csv");
+    printf("Logging to %s\n", fname);
+    
+    FILE *fp = fopen(fname, "w");
     
-    while(1) {
-#ifdef __USE_THROTTLE
-        throttle_read = throttle;
-#endif
-        pos_update();
-#ifndef __USE_THROTTLE
-        dtc_update();
-#endif
-#ifdef __DEBUG
-        if (motor->debug_stop) break;
-#endif
+    /*init IMU*/
+    BNO055 imu(I2C2_SDA, I2C2_SCL);
+    imu.reset();
+    if (!imu.check()) {
+        printf("%s\n", "IMU not ready");  
+    } else {
+        printf("BNO055 found\r\n\r\n");
+        printf("Chip          ID: %03d\r\n",imu.ID.id);
+        printf("Accelerometer ID: %03d\r\n",imu.ID.accel);
+        printf("Gyroscope     ID: %03d\r\n",imu.ID.gyro);
+        printf("Magnetometer  ID: %03d\r\n\r\n",imu.ID.mag);
+        printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
+        printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);
     }
-#ifdef __DEBUG
-    for (int i = 0; i < DBG_BUF_SZ; i++) {
-        pc.printf("%f,", fbuffer[i]);
+    imu.setmode(OPERATION_MODE_NDOF);
+    imu.set_accel_units(MPERSPERS);
+    
+    for (;;) {
+        float r, g, b;
+        r = sinf(theta);
+        g = sinf(theta + 2 * M_PI / 3);
+        b = sinf(theta - 2 * M_PI / 3);
+        
+        pwm_r = 0.5f + 0.5f * r;
+        pwm_g = 0.5f + 0.5f * g;
+        pwm_b = 0.5f + 0.5f * b;
+        
+        theta += 0.001f;
+        if (theta > 2 * M_PI) theta -= 2 * M_PI;
+        
+        tics++;
+        if (tics % 100 == 0) {
+            test = 1;
+            tics = 0;
+            
+            imu.get_accel();
+            fprintf(fp, "%d,%d,%d\n",(int) (100*imu.accel.x),(int)(100*imu.accel.y),(int)(100*imu.accel.z));
+            fflush(fp);
+            test = 0;
+        }
+        wait(0.001);
     }
-#endif
-}
+}
\ No newline at end of file

--- a/shared.h	Thu Feb 26 04:49:21 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,19 +0,0 @@
-#ifndef __SHARED_H
-#define __SHARED_H
-
-#include "constants.h"
-#include "mbed.h"
-
-extern PwmOut pha, phb, phc;
-extern DigitalOut en;
-extern AnalogIn throttle, analoga, analogb;
-extern Motor* motor;
-extern Ticker dtc_upd_ticker, throttle_upd_ticker;
-extern InterruptIn haI, hbI, hcI;
-extern float throttle_read;
-#ifdef __DEBUG
-extern float *fbuffer;
-extern int bufidx, skipidx;
-#endif
-
-#endif
\ No newline at end of file

--- a/util.cpp	Thu Feb 26 04:49:21 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,103 +0,0 @@
-#include "constants.h"
-#include "shared.h"
-#include "isr.h"
-#include <stdlib.h>
-
-void initTimers() {
-#ifdef __USE_THROTTLE
-    dtc_upd_ticker.attach_us(&dtc_update, 200);
-    throttle_upd_ticker.attach_us(&throttle_update, 100000);
-#endif
-
-#ifndef __NATIVE
-    pha.period_us(100);
-    phb.period_us(100);
-    phc.period_us(100);
-    pha = 0;
-    phb = 1.0f;
-    phc = 0;
-#endif
-
-    TIM2->CR1 &= ~(TIM_CR1_CEN);
-    TIM2->CR1 |= TIM_CR1_CMS;
-    
-#ifdef __NATIVE
-    TIM2->CCMR1 |= TIM_CCMR1_OC1M_2;
-    TIM2->CCMR1 |= TIM_CCMR1_OC1M_1;
-    TIM2->CCMR1 |= TIM_CCMR1_OC2M_2;
-    TIM2->CCMR1 |= TIM_CCMR1_OC2M_1;
-    TIM2->CCMR2 |= TIM_CCMR2_OC3M_2;
-    TIM2->CCMR2 |= TIM_CCMR2_OC3M_1;
-    
-    TIM2->CCER &= ~TIM_CCER_CC1P;
-    TIM2->CCER &= ~TIM_CCER_CC2P;
-    TIM2->CCER &= ~TIM_CCER_CC3P;
-    
-    TIM2->CCER |= TIM_CCER_CC1E;
-    TIM2->CCER |= TIM_CCER_CC2E;
-    TIM2->CCER |= TIM_CCER_CC3E;
-    
-    TIM2->PSC = 0;
-    
-    TIM2->ARR = 10000;
-    TIM2->CCR1 = 0;
-    TIM2->CCR2 = 0;
-    TIM2->CCR3 = 0;
-#endif
-
-    TIM2->CR1 |= TIM_CR1_CEN;
-}
-
-void initPins() {        
-    en = 0;
-}
-
-void initData() {
-    motor = (Motor*) malloc(sizeof(Motor));
-    motor->sensor_phase = SENSOR_PHASE;
-    motor->halt = 0;
-    motor->angle = 0;
-    motor->debug_stop = 0;
-
-#ifdef __USE_THROTTLE
-    float throttle_raw;
-    do {
-        throttle_raw = throttle;
-        throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
-        if (throttle_raw < 0.0f) throttle_raw = 0.0f;
-    } while (throttle_raw > 0.05f);
-    
-    motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
-    if (motor->throttle < 0.05f) {
-        motor->halt = 1;
-    } else {
-        motor->halt = 0;
-    }
-#else
-    motor->throttle = 1.0f;
-#endif
-}
-
-void setDtcA(float f) {
-#ifdef __NATIVE
-    TIM2->CCR3 = (uint16_t) (f * 10000.0f);
-#else
-    pha = f;
-#endif
-}
-
-void setDtcB(float f) {
-#ifdef __NATIVE
-    TIM2->CCR1 = (uint16_t) (f * 10000.0f);
-#else
-    phb = f;
-#endif
-}
-
-void setDtcC(float f) {
-#ifdef __NATIVE
-    TIM2->CCR2 = (uint16_t) (f * 10000.0f);
-#else
-    phc = f;
-#endif
-}
\ No newline at end of file

--- a/util.h	Thu Feb 26 04:49:21 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,12 +0,0 @@
-#ifndef __UTIL_H
-#define __UTIL_H
-
-void initTimers();
-void initPins();
-void initData();
-
-void setDtcA(float f);
-void setDtcB(float f);
-void setDtcC(float f);
-
-#endif
\ No newline at end of file