Diff: TVDCTRL.h

Revision:

2:9d69f27a3d3b

Parent:

1:4d86ec2fe4b1

Child:

3:821e2f07a260

--- a/TVDCTRL.h	Sat Jul 09 12:04:47 2016 +0000
+++ b/TVDCTRL.h	Sun Jul 24 02:48:39 2016 +0000
@@ -5,64 +5,99 @@
 //受け入れるパルス時間，これより長い時間は停止とみなす
 #define MAX_PULSETIME   100
 //モーターパルスを車速[m/s]に変換する．indexが時間[ms]に対応．0[ms]は無効(0)とする
-const float convPToV_533[MAX_PULSETIME+1]={
-0.000,167.447,83.723,55.816,41.862,33.489,27.908,23.921,20.931,18.605,16.745,15.222,13.954,12.881,11.960,11.163,10.465,9.850,9.303,8.813,
-8.372,7.974,7.611,7.280,6.977,6.698,6.440,6.202,5.980,5.774,5.582,5.402,5.233,5.074,4.925,4.784,4.651,4.526,4.406,4.294,
-4.186,4.084,3.987,3.894,3.806,3.721,3.640,3.563,3.488,3.417,3.349,3.283,3.220,3.159,3.101,3.044,2.990,2.938,2.887,2.838,
-2.791,2.745,2.701,2.658,2.616,2.576,2.537,2.499,2.462,2.427,2.392,2.358,2.326,2.294,2.263,2.233,2.203,2.175,2.147,2.120,
-2.093,2.067,2.042,2.017,1.993,1.970,1.947,1.925,1.903,1.881,1.861,1.840,1.820,1.801,1.781,1.763,1.744,1.726,1.709,1.691,
-1.674
+const float convPToV_533[MAX_PULSETIME+1]= {
+    0.000,167.447,83.723,55.816,41.862,33.489,27.908,23.921,20.931,18.605,16.745,15.222,13.954,12.881,11.960,11.163,10.465,9.850,9.303,8.813,
+    8.372,7.974,7.611,7.280,6.977,6.698,6.440,6.202,5.980,5.774,5.582,5.402,5.233,5.074,4.925,4.784,4.651,4.526,4.406,4.294,
+    4.186,4.084,3.987,3.894,3.806,3.721,3.640,3.563,3.488,3.417,3.349,3.283,3.220,3.159,3.101,3.044,2.990,2.938,2.887,2.838,
+    2.791,2.745,2.701,2.658,2.616,2.576,2.537,2.499,2.462,2.427,2.392,2.358,2.326,2.294,2.263,2.233,2.203,2.175,2.147,2.120,
+    2.093,2.067,2.042,2.017,1.993,1.970,1.947,1.925,1.903,1.881,1.861,1.840,1.820,1.801,1.781,1.763,1.744,1.726,1.709,1.691,
+    1.674
+};
+
+//10rpm刻みでその回転数での最大出力トルクを返す(3000rpm~)
+//[0]   :3000
+//[+1]  :+10rpm
+const float calcMaxTorque[801] = {
+    45.00000,44.96063,44.92125,44.88188,44.84250,44.80313,44.76375,44.72438,44.68500,44.64563,44.60625,44.56688,44.52750,44.48813,44.44875,44.40938,44.37000,44.33063,44.29125,44.25188,44.21250,44.17313,44.13375,44.09438,44.05500,44.01563,43.97625,43.93688,43.89750,43.85813,43.81875,43.77938,43.74000,43.70063,43.66125,43.62188,43.58250,43.54313,43.50375,43.46438,43.42500,43.38563,43.34625,43.30688,43.26750,43.22813,43.18875,43.14938,43.11000,43.07063,43.03125,42.99188,42.95250,42.91313,42.87375,42.83438,42.79500,42.75563,42.71625,42.67688,42.63750,42.59813,42.55875,42.51938,42.48000,42.44063,42.40125,42.36188,42.32250,42.28313,42.24375,42.20438,42.16500,42.12563,42.08625,42.04688,42.00750,41.96813,41.92875,41.88938,41.85000,41.81063,41.77125,41.73188,41.69250,41.65313,41.61375,41.57438,41.53500,41.49563,41.45625,41.41688,41.37750,41.33813,41.29875,41.25938,41.22000,41.18063,41.14125,41.10188
+    ,41.06250,41.02313,40.98375,40.94438,40.90500,40.86563,40.82625,40.78688,40.74750,40.70813,40.66875,40.62938,40.59000,40.55063,40.51125,40.47188,40.43250,40.39313,40.35375,40.31438,40.27500,40.23563,40.19625,40.15688,40.11750,40.07813,40.03875,39.99938,39.96000,39.92063,39.88125,39.84188,39.80250,39.76313,39.72375,39.68438,39.64500,39.60563,39.56625,39.52688,39.48750,39.44813,39.40875,39.36938,39.33000,39.29063,39.25125,39.21188,39.17250,39.13313,39.09375,39.05438,39.01500,38.97563,38.93625,38.89688,38.85750,38.81813,38.77875,38.73938,38.70000,38.66063,38.62125,38.58188,38.54250,38.50313,38.46375,38.42438,38.38500,38.34563,38.30625,38.26688,38.22750,38.18813,38.14875,38.10938,38.07000,38.03063,37.99125,37.95188,37.91250,37.87313,37.83375,37.79438,37.75500,37.71563,37.67625,37.63688,37.59750,37.55813,37.51875,37.47938,37.44000,37.40063,37.36125,37.32188,37.28250,37.24313,37.20375,37.16438
+    ,37.12500,37.08563,37.04625,37.00688,36.96750,36.92813,36.88875,36.84938,36.81000,36.77063,36.73125,36.69188,36.65250,36.61313,36.57375,36.53438,36.49500,36.45563,36.41625,36.37688,36.33750,36.29813,36.25875,36.21938,36.18000,36.14063,36.10125,36.06188,36.02250,35.98313,35.94375,35.90438,35.86500,35.82563,35.78625,35.74688,35.70750,35.66813,35.62875,35.58938,35.55000,35.51063,35.47125,35.43188,35.39250,35.35313,35.31375,35.27438,35.23500,35.19563,35.15625,35.11688,35.07750,35.03813,34.99875,34.95938,34.92000,34.88063,34.84125,34.80188,34.76250,34.72313,34.68375,34.64438,34.60500,34.56563,34.52625,34.48688,34.44750,34.40813,34.36875,34.32938,34.29000,34.25063,34.21125,34.17188,34.13250,34.09313,34.05375,34.01438,33.97500,33.93563,33.89625,33.85688,33.81750,33.77813,33.73875,33.69938,33.66000,33.62063,33.58125,33.54188,33.50250,33.46313,33.42375,33.38438,33.34500,33.30563,33.26625,33.22688
+    ,33.18750,33.14813,33.10875,33.06938,33.03000,32.99063,32.95125,32.91188,32.87250,32.83313,32.79375,32.75438,32.71500,32.67563,32.63625,32.59688,32.55750,32.51813,32.47875,32.43938,32.40000,32.36063,32.32125,32.28188,32.24250,32.20313,32.16375,32.12438,32.08500,32.04563,32.00625,31.96688,31.92750,31.88813,31.84875,31.80938,31.77000,31.73063,31.69125,31.65188,31.61250,31.57313,31.53375,31.49438,31.45500,31.41563,31.37625,31.33688,31.29750,31.25813,31.21875,31.17938,31.14000,31.10063,31.06125,31.02188,30.98250,30.94313,30.90375,30.86438,30.82500,30.78563,30.74625,30.70688,30.66750,30.62813,30.58875,30.54938,30.51000,30.47063,30.43125,30.39188,30.35250,30.31313,30.27375,30.23438,30.19500,30.15563,30.11625,30.07688,30.03750,29.99813,29.95875,29.91938,29.88000,29.84063,29.80125,29.76188,29.72250,29.68313,29.64375,29.60438,29.56500,29.52563,29.48625,29.44688,29.40750,29.36813,29.32875,29.28938
+    ,29.25000,29.21063,29.17125,29.13188,29.09250,29.05313,29.01375,28.97438,28.93500,28.89563,28.85625,28.81688,28.77750,28.73813,28.69875,28.65938,28.62000,28.58063,28.54125,28.50188,28.46250,28.42313,28.38375,28.34438,28.30500,28.26563,28.22625,28.18688,28.14750,28.10813,28.06875,28.02938,27.99000,27.95063,27.91125,27.87188,27.83250,27.79313,27.75375,27.71438,27.67500,27.63563,27.59625,27.55688,27.51750,27.47813,27.43875,27.39938,27.36000,27.32063,27.28125,27.24188,27.20250,27.16313,27.12375,27.08438,27.04500,27.00563,26.96625,26.92688,26.88750,26.84813,26.80875,26.76938,26.73000,26.69063,26.65125,26.61188,26.57250,26.53313,26.49375,26.45438,26.41500,26.37563,26.33625,26.29688,26.25750,26.21813,26.17875,26.13938,26.10000,26.06063,26.02125,25.98188,25.94250,25.90313,25.86375,25.82438,25.78500,25.74563,25.70625,25.66688,25.62750,25.58813,25.54875,25.50938,25.47000,25.43063,25.39125,25.35188
+    ,25.31250,25.27313,25.23375,25.19438,25.15500,25.11563,25.07625,25.03688,24.99750,24.95813,24.91875,24.87938,24.84000,24.80063,24.76125,24.72188,24.68250,24.64313,24.60375,24.56438,24.52500,24.48563,24.44625,24.40688,24.36750,24.32813,24.28875,24.24938,24.21000,24.17063,24.13125,24.09188,24.05250,24.01313,23.97375,23.93438,23.89500,23.85563,23.81625,23.77688,23.73750,23.69813,23.65875,23.61938,23.58000,23.54063,23.50125,23.46188,23.42250,23.38313,23.34375,23.30438,23.26500,23.22563,23.18625,23.14688,23.10750,23.06813,23.02875,22.98938,22.95000,22.91063,22.87125,22.83188,22.79250,22.75313,22.71375,22.67438,22.63500,22.59563,22.55625,22.51688,22.47750,22.43813,22.39875,22.35938,22.32000,22.28063,22.24125,22.20188,22.16250,22.12313,22.08375,22.04438,22.00500,21.96563,21.92625,21.88688,21.84750,21.80813,21.76875,21.72938,21.69000,21.65063,21.61125,21.57188,21.53250,21.49313,21.45375,21.41438
+    ,21.37500,21.33563,21.29625,21.25688,21.21750,21.17813,21.13875,21.09938,21.06000,21.02063,20.98125,20.94188,20.90250,20.86313,20.82375,20.78438,20.74500,20.70563,20.66625,20.62688,20.58750,20.54813,20.50875,20.46938,20.43000,20.39063,20.35125,20.31188,20.27250,20.23313,20.19375,20.15438,20.11500,20.07563,20.03625,19.99688,19.95750,19.91813,19.87875,19.83938,19.80000,19.76063,19.72125,19.68188,19.64250,19.60313,19.56375,19.52438,19.48500,19.44563,19.40625,19.36688,19.32750,19.28813,19.24875,19.20938,19.17000,19.13063,19.09125,19.05188,19.01250,18.97313,18.93375,18.89438,18.85500,18.81563,18.77625,18.73688,18.69750,18.65813,18.61875,18.57938,18.54000,18.50063,18.46125,18.42188,18.38250,18.34313,18.30375,18.26438,18.22500,18.18563,18.14625,18.10688,18.06750,18.02813,17.98875,17.94938,17.91000,17.87063,17.83125,17.79188,17.75250,17.71313,17.67375,17.63438,17.59500,17.55563,17.51625,17.47688
+    ,17.43750,17.39813,17.35875,17.31938,17.28000,17.24063,17.20125,17.16188,17.12250,17.08313,17.04375,17.00438,16.96500,16.92563,16.88625,16.84688,16.80750,16.76813,16.72875,16.68938,16.65000,16.61063,16.57125,16.53188,16.49250,16.45313,16.41375,16.37438,16.33500,16.29563,16.25625,16.21688,16.17750,16.13813,16.09875,16.05938,16.02000,15.98063,15.94125,15.90188,15.86250,15.82313,15.78375,15.74438,15.70500,15.66563,15.62625,15.58688,15.54750,15.50813,15.46875,15.42938,15.39000,15.35063,15.31125,15.27188,15.23250,15.19313,15.15375,15.11438,15.07500,15.03563,14.99625,14.95688,14.91750,14.87813,14.83875,14.79938,14.76000,14.72063,14.68125,14.64188,14.60250,14.56313,14.52375,14.48438,14.44500,14.40563,14.36625,14.32688,14.28750,14.24813,14.20875,14.16938,14.13000,14.09063,14.05125,14.01188,13.97250,13.93313,13.89375,13.85438,13.81500,13.77563,13.73625,13.69688,13.65750,13.61813,13.57875,13.53938
+    ,13.50000
+};
+
+struct errCounter_t {
+    int apsUnderVolt;       //aps電圧不足
+    int apsExceedVolt;      //aps電圧超過
+    int apsErrorTolerance;  //aps偏差超過
+    int apsStick;           //aps固着
+    int brakeUnderVolt;     //brake電圧不足
+    int brakeExceedVolt;    //brake電圧超過
+    int brakeFuzzyVolt;     //brake曖昧な電圧(ONでもOFFでもない)
+    int brakeOverRide;      //accel-brake同時踏み
 };
 
-//トルク信号[V]をトルク[Nm]に変換
-//[0] -> 0.49V
-//[+1] -> +0.01V
-const float convVToT_500[]={
-0.000,0.048,0.096,0.144,0.192,0.240,0.288,0.337,0.385,0.433,0.481,0.529,0.577,0.625,0.673,0.721,0.769,0.817,0.865,0.913,0.962,1.010,1.058,1.106,1.154,1.202,1.250,1.298,1.346,1.394,1.442,1.490,1.538,1.587,1.635,1.683,1.731,1.779,1.827,1.875,1.923,1.971,2.019,2.067,2.115,2.163,2.212,2.260,2.308,2.356,2.404,2.452,2.500,2.715,2.931,3.146,3.362,3.577,3.792,4.008,4.223,4.438,4.654,4.869,5.085,5.300,5.515,5.731,5.946,6.162,6.377,6.592,6.808,7.023,7.238,7.454,7.669,7.885,8.100,8.377,8.654,8.931,9.208,9.485,9.762,10.038,10.315,10.592,10.869,11.146,11.423,11.700,11.977,12.254,12.531,12.808,13.085,13.362,13.638,13.915,14.192,14.469,14.746,15.023,15.300,15.604,15.908,16.212,16.515,16.819,17.123,17.427,17.731,18.035,18.338,18.642,18.946,19.250,19.554,19.858,20.162,20.465,20.769,21.073,21.377,21.681,21.985,22.288,22.592,22.896,23.200,23.508,23.815,24.123,24.431,24.738,25.046,25.354,25.662,25.969,26.277,26.585,26.892,27.200,27.508,27.815,28.123,28.431,28.738,29.046,29.354,29.662,29.969,30.277,30.585,30.892,31.200,31.488,31.777,32.065,32.354,32.642,32.931,33.219,33.508,33.796,34.085,34.373,34.662,34.950,35.238,35.527,35.815,36.104,36.392,36.681,36.969,37.258,37.546,37.835,38.123,38.412,38.700,39.000,39.300,39.600,39.900,40.200,40.500,40.800,41.100,41.400,41.700,42.000,42.300,42.600,42.900,43.200,43.500,43.800,44.100
-};
-const float convVToT_1000[]={
-0.000,0.048,0.096,0.144,0.192,0.240,0.288,0.337,0.385,0.433,0.481,0.529,0.577,0.625,0.673,0.721,0.769,0.817,0.865,0.913,0.962,1.010,1.058,1.106,1.154,1.202,1.250,1.298,1.346,1.394,1.442,1.490,1.538,1.587,1.635,1.683,1.731,1.779,1.827,1.875,1.923,1.971,2.019,2.067,2.115,2.163,2.212,2.260,2.308,2.356,2.404,2.452,2.500,2.696,2.892,3.088,3.285,3.481,3.677,3.873,4.069,4.265,4.462,4.658,4.854,5.050,5.246,5.442,5.638,5.835,6.031,6.227,6.423,6.619,6.815,7.012,7.208,7.404,7.600,7.869,8.138,8.408,8.677,8.946,9.215,9.485,9.754,10.023,10.292,10.562,10.831,11.100,11.369,11.638,11.908,12.177,12.446,12.715,12.985,13.254,13.523,13.792,14.062,14.331,14.600,14.904,15.208,15.512,15.815,16.119,16.423,16.727,17.031,17.335,17.638,17.942,18.246,18.550,18.854,19.158,19.462,19.765,20.069,20.373,20.677,20.981,21.285,21.588,21.892,22.196,22.500,22.815,23.131,23.446,23.762,24.077,24.392,24.708,25.023,25.338,25.654,25.969,26.285,26.600,26.915,27.231,27.546,27.862,28.177,28.492,28.808,29.123,29.438,29.754,30.069,30.385,30.700,30.996,31.292,31.588,31.885,32.181,32.477,32.773,33.069,33.365,33.662,33.958,34.254,34.550,34.846,35.142,35.438,35.735,36.031,36.327,36.623,36.919,37.215,37.512,37.808,38.104,38.400,38.706,39.011,39.317,39.622,39.928,40.233,40.539,40.844,41.150,41.456,41.761,42.067,42.372,42.678,42.983,43.289,43.594,43.900
-};
-const float convVToT_2000[]={
-0.000,0.058,0.115,0.173,0.231,0.288,0.346,0.404,0.462,0.519,0.577,0.635,0.692,0.750,0.808,0.865,0.923,0.981,1.038,1.096,1.154,1.212,1.269,1.327,1.385,1.442,1.500,1.558,1.615,1.673,1.731,1.788,1.846,1.904,1.962,2.019,2.077,2.135,2.192,2.250,2.308,2.365,2.423,2.481,2.538,2.596,2.654,2.712,2.769,2.827,2.885,2.942,3.000,3.208,3.415,3.623,3.831,4.038,4.246,4.454,4.662,4.869,5.077,5.285,5.492,5.700,5.908,6.115,6.323,6.531,6.738,6.946,7.154,7.362,7.569,7.777,7.985,8.192,8.400,8.665,8.931,9.196,9.462,9.727,9.992,10.258,10.523,10.788,11.054,11.319,11.585,11.850,12.115,12.381,12.646,12.912,13.177,13.442,13.708,13.973,14.238,14.504,14.769,15.035,15.300,15.612,15.923,16.235,16.546,16.858,17.169,17.481,17.792,18.104,18.415,18.727,19.038,19.350,19.662,19.973,20.285,20.596,20.908,21.219,21.531,21.842,22.154,22.465,22.777,23.088,23.400,23.715,24.031,24.346,24.662,24.977,25.292,25.608,25.923,26.238,26.554,26.869,27.185,27.500,27.815,28.131,28.446,28.762,29.077,29.392,29.708,30.023,30.338,30.654,30.969,31.285,31.600,31.900,32.200,32.500,32.800,33.100,33.400,33.700,34.000,34.300,34.600,34.900,35.200,35.500,35.800,36.100,36.400,36.700,37.000,37.300,37.600,37.900,38.200,38.500,38.800,39.100,39.400,39.700,40.000,40.300,40.600,40.900,41.200,41.500,41.800,42.100,42.400,42.700,43.000,43.300,43.600,43.900,44.200,44.500,44.800
-};
-const float convVToT_3000[]={
-0.000,0.058,0.115,0.173,0.231,0.288,0.346,0.404,0.462,0.519,0.577,0.635,0.692,0.750,0.808,0.865,0.923,0.981,1.038,1.096,1.154,1.212,1.269,1.327,1.385,1.442,1.500,1.558,1.615,1.673,1.731,1.788,1.846,1.904,1.962,2.019,2.077,2.135,2.192,2.250,2.308,2.365,2.423,2.481,2.538,2.596,2.654,2.712,2.769,2.827,2.885,2.942,3.000,3.192,3.385,3.577,3.769,3.962,4.154,4.346,4.538,4.731,4.923,5.115,5.308,5.500,5.692,5.885,6.077,6.269,6.462,6.654,6.846,7.038,7.231,7.423,7.615,7.808,8.000,8.250,8.500,8.750,9.000,9.250,9.500,9.750,10.000,10.250,10.500,10.750,11.000,11.250,11.500,11.750,12.000,12.250,12.500,12.750,13.000,13.250,13.500,13.750,14.000,14.250,14.500,14.800,15.100,15.400,15.700,16.000,16.300,16.600,16.900,17.200,17.500,17.800,18.100,18.400,18.700,19.000,19.300,19.600,19.900,20.200,20.500,20.800,21.100,21.400,21.700,22.000,22.300,22.615,22.931,23.246,23.562,23.877,24.192,24.508,24.823,25.138,25.454,25.769,26.085,26.400,26.715,27.031,27.346,27.662,27.977,28.292,28.608,28.923,29.238,29.554,29.869,30.185,30.500,30.785,31.069,31.354,31.638,31.923,32.208,32.492,32.777,33.062,33.346,33.631,33.915,34.200,34.485,34.769,35.054,35.338,35.623,35.908,36.192,36.477,36.762,37.046,37.331,37.615,37.900,38.200,38.500,38.800,39.100,39.400,39.700,40.000,40.300,40.600,40.900,41.200,41.500,41.800,42.100,42.400,42.700,43.000,43.300
-};
-const float convVToT_4000[]={
-0.000,0.048,0.096,0.144,0.192,0.240,0.288,0.337,0.385,0.433,0.481,0.529,0.577,0.625,0.673,0.721,0.769,0.817,0.865,0.913,0.962,1.010,1.058,1.106,1.154,1.202,1.250,1.298,1.346,1.394,1.442,1.490,1.538,1.587,1.635,1.683,1.731,1.779,1.827,1.875,1.923,1.971,2.019,2.067,2.115,2.163,2.212,2.260,2.308,2.356,2.404,2.452,2.500,2.654,2.808,2.962,3.115,3.269,3.423,3.577,3.731,3.885,4.038,4.192,4.346,4.500,4.654,4.808,4.962,5.115,5.269,5.423,5.577,5.731,5.885,6.038,6.192,6.346,6.500,6.715,6.931,7.146,7.362,7.577,7.792,8.008,8.223,8.438,8.654,8.869,9.085,9.300,9.515,9.731,9.946,10.162,10.377,10.592,10.808,11.023,11.238,11.454,11.669,11.885,12.100,12.358,12.615,12.873,13.131,13.388,13.646,13.904,14.162,14.419,14.677,14.935,15.192,15.450,15.708,15.965,16.223,16.481,16.738,16.996,17.254,17.512,17.769,18.027,18.285,18.542,18.800,19.073,19.346,19.619,19.892,20.165,20.438,20.712,20.985,21.258,21.531,21.804,22.077,22.350,22.623,22.896,23.169,23.442,23.715,23.988,24.262,24.535,24.808,25.081,25.354,25.627,25.900,26.154,26.408,26.662,26.915,27.169,27.423,27.677,27.931,28.185,28.438,28.692,28.946,29.200,29.454,29.708,29.962,30.215,30.469,30.723,30.977,31.231,31.485,31.738,31.992,32.246,32.500,32.767,33.033,33.300,33.567,33.833,34.100,34.367,34.633,34.900,35.167,35.433,35.700,35.967,36.233,36.500,36.767,37.033,37.300
+#define ratioLPF        0.6726f //CutOff:20Hz
+#define M_PI            3.1415f //[-]
+#define TVD_GEAR_RATIO  10.0f   //[-]
+#define TIRE_DIAMETER   0.533f  //[m]
+#define WHEEL_BASE      1.760f  //[m]
+#define STABIRITY_FACTOR    0.01f   //[-]
+#define TREAD               1.3f    //[m]
+
+enum {
+    APS_PRIMARY=0,
+    APS_SECONDARY,
+    BRAKE
 };
-const float convVToT_5000[]={
-0.000,0.042,0.085,0.127,0.169,0.212,0.254,0.296,0.338,0.381,0.423,0.465,0.508,0.550,0.592,0.635,0.677,0.719,0.762,0.804,0.846,0.888,0.931,0.973,1.015,1.058,1.100,1.142,1.185,1.227,1.269,1.312,1.354,1.396,1.438,1.481,1.523,1.565,1.608,1.650,1.692,1.735,1.777,1.819,1.862,1.904,1.946,1.988,2.031,2.073,2.115,2.158,2.200,2.323,2.446,2.569,2.692,2.815,2.938,3.062,3.185,3.308,3.431,3.554,3.677,3.800,3.923,4.046,4.169,4.292,4.415,4.538,4.662,4.785,4.908,5.031,5.154,5.277,5.400,5.577,5.754,5.931,6.108,6.285,6.462,6.638,6.815,6.992,7.169,7.346,7.523,7.700,7.877,8.054,8.231,8.408,8.585,8.762,8.938,9.115,9.292,9.469,9.646,9.823,10.000,10.204,10.408,10.612,10.815,11.019,11.223,11.427,11.631,11.835,12.038,12.242,12.446,12.650,12.854,13.058,13.262,13.465,13.669,13.873,14.077,14.281,14.485,14.688,14.892,15.096,15.300,15.523,15.746,15.969,16.192,16.415,16.638,16.862,17.085,17.308,17.531,17.754,17.977,18.200,18.423,18.646,18.869,19.092,19.315,19.538,19.762,19.985,20.208,20.431,20.654,20.877,21.100,21.304,21.508,21.712,21.915,22.119,22.323,22.527,22.731,22.935,23.138,23.342,23.546,23.750,23.954,24.158,24.362,24.565,24.769,24.973,25.177,25.381,25.585,25.788,25.992,26.196,26.400,26.606,26.811,27.017,27.222,27.428,27.633,27.839,28.044,28.250,28.456,28.661,28.867,29.072,29.278,29.483,29.689,29.894,30.100
-};
-const float convVToT_6000[]={
-0.000,0.038,0.077,0.115,0.154,0.192,0.231,0.269,0.308,0.346,0.385,0.423,0.462,0.500,0.538,0.577,0.615,0.654,0.692,0.731,0.769,0.808,0.846,0.885,0.923,0.962,1.000,1.038,1.077,1.115,1.154,1.192,1.231,1.269,1.308,1.346,1.385,1.423,1.462,1.500,1.538,1.577,1.615,1.654,1.692,1.731,1.769,1.808,1.846,1.885,1.923,1.962,2.000,2.112,2.223,2.335,2.446,2.558,2.669,2.781,2.892,3.004,3.115,3.227,3.338,3.450,3.562,3.673,3.785,3.896,4.008,4.119,4.231,4.342,4.454,4.565,4.677,4.788,4.900,5.038,5.177,5.315,5.454,5.592,5.731,5.869,6.008,6.146,6.285,6.423,6.562,6.700,6.838,6.977,7.115,7.254,7.392,7.531,7.669,7.808,7.946,8.085,8.223,8.362,8.500,8.681,8.862,9.042,9.223,9.404,9.585,9.765,9.946,10.127,10.308,10.488,10.669,10.850,11.031,11.212,11.392,11.573,11.754,11.935,12.115,12.296,12.477,12.658,12.838,13.019,13.200,13.381,13.562,13.742,13.923,14.104,14.285,14.465,14.646,14.827,15.008,15.188,15.369,15.550,15.731,15.912,16.092,16.273,16.454,16.635,16.815,16.996,17.177,17.358,17.538,17.719,17.900,18.019,18.138,18.258,18.377,18.496,18.615,18.735,18.854,18.973,19.092,19.212,19.331,19.450,19.569,19.688,19.808,19.927,20.046,20.165,20.285,20.404,20.523,20.642,20.762,20.881,21.000,21.150,21.300,21.450,21.600,21.750,21.900,22.050,22.200,22.350,22.500,22.650,22.800,22.950,23.100,23.250,23.400,23.550,23.700
-};
-const float convVToT_7000[]={
-0.000,0.038,0.077,0.115,0.154,0.192,0.231,0.269,0.308,0.346,0.385,0.423,0.462,0.500,0.538,0.577,0.615,0.654,0.692,0.731,0.769,0.808,0.846,0.885,0.923,0.962,1.000,1.038,1.077,1.115,1.154,1.192,1.231,1.269,1.308,1.346,1.385,1.423,1.462,1.500,1.538,1.577,1.615,1.654,1.692,1.731,1.769,1.808,1.846,1.885,1.923,1.962,2.000,2.092,2.185,2.277,2.369,2.462,2.554,2.646,2.738,2.831,2.923,3.015,3.108,3.200,3.292,3.385,3.477,3.569,3.662,3.754,3.846,3.938,4.031,4.123,4.215,4.308,4.400,4.523,4.646,4.769,4.892,5.015,5.138,5.262,5.385,5.508,5.631,5.754,5.877,6.000,6.123,6.246,6.369,6.492,6.615,6.738,6.862,6.985,7.108,7.231,7.354,7.477,7.600,7.750,7.900,8.050,8.200,8.350,8.500,8.650,8.800,8.950,9.100,9.250,9.400,9.550,9.700,9.850,10.000,10.150,10.300,10.450,10.600,10.750,10.900,11.050,11.200,11.350,11.500,11.650,11.800,11.950,12.100,12.250,12.400,12.550,12.700,12.850,13.000,13.150,13.300,13.450,13.600,13.750,13.900,14.050,14.200,14.350,14.500,14.650,14.800,14.950,15.100,15.250,15.400,15.558,15.715,15.873,16.031,16.188,16.346,16.504,16.662,16.819,16.977,17.135,17.292,17.450,17.608,17.765,17.923,18.081,18.238,18.396,18.554,18.712,18.869,19.027,19.185,19.342,19.500,19.650,19.800,19.950,20.100,20.250,20.400,20.550,20.700,20.850,21.000,21.150,21.300,21.450,21.600,21.750,21.900,22.050,22.200
-};
-const float convVToT_8000[]={
-0.000,0.035,0.069,0.104,0.138,0.173,0.208,0.242,0.277,0.312,0.346,0.381,0.415,0.450,0.485,0.519,0.554,0.588,0.623,0.658,0.692,0.727,0.762,0.796,0.831,0.865,0.900,0.935,0.969,1.004,1.038,1.073,1.108,1.142,1.177,1.212,1.246,1.281,1.315,1.350,1.385,1.419,1.454,1.488,1.523,1.558,1.592,1.627,1.662,1.696,1.731,1.765,1.800,1.881,1.962,2.042,2.123,2.204,2.285,2.365,2.446,2.527,2.608,2.688,2.769,2.850,2.931,3.012,3.092,3.173,3.254,3.335,3.415,3.496,3.577,3.658,3.738,3.819,3.900,4.000,4.100,4.200,4.300,4.400,4.500,4.600,4.700,4.800,4.900,5.000,5.100,5.200,5.300,5.400,5.500,5.600,5.700,5.800,5.900,6.000,6.100,6.200,6.300,6.400,6.500,6.627,6.754,6.881,7.008,7.135,7.262,7.388,7.515,7.642,7.769,7.896,8.023,8.150,8.277,8.404,8.531,8.658,8.785,8.912,9.038,9.165,9.292,9.419,9.546,9.673,9.800,9.931,10.062,10.192,10.323,10.454,10.585,10.715,10.846,10.977,11.108,11.238,11.369,11.500,11.631,11.762,11.892,12.023,12.154,12.285,12.415,12.546,12.677,12.808,12.938,13.069,13.200,13.331,13.462,13.592,13.723,13.854,13.985,14.115,14.246,14.377,14.508,14.638,14.769,14.900,15.031,15.162,15.292,15.423,15.554,15.685,15.815,15.946,16.077,16.208,16.338,16.469,16.600,16.739,16.878,17.017,17.156,17.294,17.433,17.572,17.711,17.850,17.989,18.128,18.267,18.406,18.544,18.683,18.822,18.961,19.100
-};
-const float convVToT_9000[]={
-0.000,0.031,0.062,0.092,0.123,0.154,0.185,0.215,0.246,0.277,0.308,0.338,0.369,0.400,0.431,0.462,0.492,0.523,0.554,0.585,0.615,0.646,0.677,0.708,0.738,0.769,0.800,0.831,0.862,0.892,0.923,0.954,0.985,1.015,1.046,1.077,1.108,1.138,1.169,1.200,1.231,1.262,1.292,1.323,1.354,1.385,1.415,1.446,1.477,1.508,1.538,1.569,1.600,1.669,1.738,1.808,1.877,1.946,2.015,2.085,2.154,2.223,2.292,2.362,2.431,2.500,2.569,2.638,2.708,2.777,2.846,2.915,2.985,3.054,3.123,3.192,3.262,3.331,3.400,3.488,3.577,3.665,3.754,3.842,3.931,4.019,4.108,4.196,4.285,4.373,4.462,4.550,4.638,4.727,4.815,4.904,4.992,5.081,5.169,5.258,5.346,5.435,5.523,5.612,5.700,5.804,5.908,6.012,6.115,6.219,6.323,6.427,6.531,6.635,6.738,6.842,6.946,7.050,7.154,7.258,7.362,7.465,7.569,7.673,7.777,7.881,7.985,8.088,8.192,8.296,8.400,8.515,8.631,8.746,8.862,8.977,9.092,9.208,9.323,9.438,9.554,9.669,9.785,9.900,10.015,10.131,10.246,10.362,10.477,10.592,10.708,10.823,10.938,11.054,11.169,11.285,11.400,11.519,11.638,11.758,11.877,11.996,12.115,12.235,12.354,12.473,12.592,12.712,12.831,12.950,13.069,13.188,13.308,13.427,13.546,13.665,13.785,13.904,14.023,14.142,14.262,14.381,14.500,14.628,14.756,14.883,15.011,15.139,15.267,15.394,15.522,15.650,15.778,15.906,16.033,16.161,16.289,16.417,16.544,16.672,16.800
-};
-const float convVToT_10000[]={
-0.000,0.027,0.054,0.081,0.108,0.135,0.162,0.188,0.215,0.242,0.269,0.296,0.323,0.350,0.377,0.404,0.431,0.458,0.485,0.512,0.538,0.565,0.592,0.619,0.646,0.673,0.700,0.727,0.754,0.781,0.808,0.835,0.862,0.888,0.915,0.942,0.969,0.996,1.023,1.050,1.077,1.104,1.131,1.158,1.185,1.212,1.238,1.265,1.292,1.319,1.346,1.373,1.400,1.465,1.531,1.596,1.662,1.727,1.792,1.858,1.923,1.988,2.054,2.119,2.185,2.250,2.315,2.381,2.446,2.512,2.577,2.642,2.708,2.773,2.838,2.904,2.969,3.035,3.100,3.177,3.254,3.331,3.408,3.485,3.562,3.638,3.715,3.792,3.869,3.946,4.023,4.100,4.177,4.254,4.331,4.408,4.485,4.562,4.638,4.715,4.792,4.869,4.946,5.023,5.100,5.188,5.277,5.365,5.454,5.542,5.631,5.719,5.808,5.896,5.985,6.073,6.162,6.250,6.338,6.427,6.515,6.604,6.692,6.781,6.869,6.958,7.046,7.135,7.223,7.312,7.400,7.500,7.600,7.700,7.800,7.900,8.000,8.100,8.200,8.300,8.400,8.500,8.600,8.700,8.800,8.900,9.000,9.100,9.200,9.300,9.400,9.500,9.600,9.700,9.800,9.900,10.000,10.108,10.215,10.323,10.431,10.538,10.646,10.754,10.862,10.969,11.077,11.185,11.292,11.400,11.508,11.615,11.723,11.831,11.938,12.046,12.154,12.262,12.369,12.477,12.585,12.692,12.800,12.928,13.056,13.183,13.311,13.439,13.567,13.694,13.822,13.950,14.078,14.206,14.333,14.461,14.589,14.717,14.844,14.972,15.100
-};
-const float convVToT_11000[]={
-0.000,0.025,0.050,0.075,0.100,0.125,0.150,0.175,0.200,0.225,0.250,0.275,0.300,0.325,0.350,0.375,0.400,0.425,0.450,0.475,0.500,0.525,0.550,0.575,0.600,0.625,0.650,0.675,0.700,0.725,0.750,0.775,0.800,0.825,0.850,0.875,0.900,0.925,0.950,0.975,1.000,1.025,1.050,1.075,1.100,1.125,1.150,1.175,1.200,1.225,1.250,1.275,1.300,1.362,1.423,1.485,1.546,1.608,1.669,1.731,1.792,1.854,1.915,1.977,2.038,2.100,2.162,2.223,2.285,2.346,2.408,2.469,2.531,2.592,2.654,2.715,2.777,2.838,2.900,2.965,3.031,3.096,3.162,3.227,3.292,3.358,3.423,3.488,3.554,3.619,3.685,3.750,3.815,3.881,3.946,4.012,4.077,4.142,4.208,4.273,4.338,4.404,4.469,4.535,4.600,4.677,4.754,4.831,4.908,4.985,5.062,5.138,5.215,5.292,5.369,5.446,5.523,5.600,5.677,5.754,5.831,5.908,5.985,6.062,6.138,6.215,6.292,6.369,6.446,6.523,6.600,6.692,6.785,6.877,6.969,7.062,7.154,7.246,7.338,7.431,7.523,7.615,7.708,7.800,7.892,7.985,8.077,8.169,8.262,8.354,8.446,8.538,8.631,8.723,8.815,8.908,9.000,9.096,9.192,9.288,9.385,9.481,9.577,9.673,9.769,9.865,9.962,10.058,10.154,10.250,10.346,10.442,10.538,10.635,10.731,10.827,10.923,11.019,11.115,11.212,11.308,11.404,11.500,11.611,11.722,11.833,11.944,12.056,12.167,12.278,12.389,12.500,12.611,12.722,12.833,12.944,13.056,13.167,13.278,13.389,13.500
+
+enum SelectMotor {
+    RIGHT_MOTOR=0,
+    LEFT_MOTOR
 };
 
-#define ratioLPF 0.11795f    //CutOff:20Hz
+const int MAX_MOTOR_TORQUE      = 0xFFFF;   //LSB : 45/65535 = 0.686655[mNm]
+const int MAX_OUTPUT_TORQUE      = (int)(MAX_MOTOR_TORQUE * 0.7);  //出力最大値
+const float MAX_DISTRIBUTION_TORQUE = (MAX_OUTPUT_TORQUE*0.8);
+const float MAX_STEER_ANGLE     = (float)(M_PI/180.0 * 25.0); //[-]
+
+const int APS_MIN_POSITION          =(int)(0xFFFF/3.3 * 1.0);     //"正常時"最小入力電圧
+const int APS_MAX_POSITION          =(int)(0xFFFF/3.3 * 2.0);    //"正常時"最大入力電圧
+const int APS_DEADBAND              =(int)((APS_MAX_POSITION - APS_MIN_POSITION) * 0.2);   //APS信号不感帯
+const int APS_VALID_RANGE           =(APS_MAX_POSITION - APS_MIN_POSITION) - APS_DEADBAND;  //APS信号有効範囲
+const int APS_DEVIATION_TOLERANCE   =(int)((APS_MAX_POSITION - APS_MIN_POSITION) * 0.1);   //10%偏差許容値
+const int APS_OVERRIDE              =(int)(APS_VALID_RANGE * 0.25);    //ブレーキオーバーライドの閾値
 
-const unsigned int MAX_MOTOR_TORQUE     = 45;
+const int BRK_ON_VOLTAGE            =(int)(0xFFFF/3.3 * 1.65);    //ブレーキON時電圧
+const int BRK_OFF_VOLTAGE           =(int)(0xFFFF/3.3 * 2.20);     //ブレーキOFF時電圧
+
+const int ERROR_TOLERANCE           =(int)(0xFFFF * 0.05);    //センサ値誤差許容値
+
+//DACは12bit仕様だが一旦16bitに統一してから12bitに変換する
+const int DACOUTPUT_MIN             =(int)(0xFFFF/3.3 * 0.49);    //MCの最低入力電圧
+const int DACOUTPUT_MAX             =(int)(0xFFFF/3.3 * 2.50);    //MCの最大入力電圧
+const int DACOUTPUT_VALID_RANGE     =DACOUTPUT_MAX - DACOUTPUT_MIN; //実質有効電圧範囲
+
+const int LINEAR_REGION_TORQUE      =(int)(MAX_MOTOR_TORQUE/45.0 * 2.5);      //トルクに回転数が影響しない領域の境界値(トルク値)
+const int LINEAR_REGION_VOLTAGE     =(int)(0xFFFF/3.3 * 1.0) - DACOUTPUT_MIN; //トルクに回転数が影響しない領域の境界値(出力電圧値)
+const int MAX_REVOLUTION_TORQUE     =(int)(MAX_MOTOR_TORQUE/45.0 * 13.5);     //最高回転数における最大出力トルク
+
+const float FIX_DACOUTFORM          =(float)((0xFFF/3.3 * 2.5)/0xFFFF);
 
 void loadSensorsLPF(void);
 int checkSensorPlausibility(void);
 
 void initTVD(void);
+void driveTVD(void);
 
-int getVelocity(void);
+float getVelocity(void);
+int loadCurrentSensor(int sensor);
+int loadRawSensor(int sensor);
+void getCurrentErrCount(struct errCounter_t *ptr);
+int calcRequestTorque(void);
+
 
 #endif
\ No newline at end of file