File content as of revision 8:a22aec357a64:

#ifndef TVDCTRL_H
#define TVDCTRL_H
#include "mbed.h"

//10rpm刻みでその回転数での最大出力トルクを返す(3000rpm~)
//[0]   :3000
//[+1]  :+10rpm
const int calcMaxTorque[801] = {
    65535,65478,65420,65363,65306,65248,65191,65134,65076,65019,64962,64904,64847,64790,64732,64675,64618,64560,64503,64445,64388,64331,64273,64216,64159,64101,64044,63987,63929,63872,63815,63757,63700,63643,63585,63528,63471,63413,63356,63299,63241,63184,63127,63069,63012,62955,62897,62840,62783,62725,62668,62611,62553,62496,62438,62381,62324,62266,62209,62152,62094,62037,61980,61922,61865,61808,61750,61693,61636,61578,61521,61464,61406,61349,61292,61234,61177,61120,61062,61005,60948,60890,60833,60776,60718,60661,60603,60546,60489,60431,60374,60317,60259,60202,60145,60087,60030,59973,59915,59858
    ,59801,59743,59686,59629,59571,59514,59457,59399,59342,59285,59227,59170,59113,59055,58998,58941,58883,58826,58769,58711,58654,58596,58539,58482,58424,58367,58310,58252,58195,58138,58080,58023,57966,57908,57851,57794,57736,57679,57622,57564,57507,57450,57392,57335,57278,57220,57163,57106,57048,56991,56934,56876,56819,56762,56704,56647,56589,56532,56475,56417,56360,56303,56245,56188,56131,56073,56016,55959,55901,55844,55787,55729,55672,55615,55557,55500,55443,55385,55328,55271,55213,55156,55099,55041,54984,54927,54869,54812,54754,54697,54640,54582,54525,54468,54410,54353,54296,54238,54181,54124
    ,54066,54009,53952,53894,53837,53780,53722,53665,53608,53550,53493,53436,53378,53321,53264,53206,53149,53092,53034,52977,52920,52862,52805,52747,52690,52633,52575,52518,52461,52403,52346,52289,52231,52174,52117,52059,52002,51945,51887,51830,51773,51715,51658,51601,51543,51486,51429,51371,51314,51257,51199,51142,51085,51027,50970,50913,50855,50798,50740,50683,50626,50568,50511,50454,50396,50339,50282,50224,50167,50110,50052,49995,49938,49880,49823,49766,49708,49651,49594,49536,49479,49422,49364,49307,49250,49192,49135,49078,49020,48963,48905,48848,48791,48733,48676,48619,48561,48504,48447,48389
    ,48332,48275,48217,48160,48103,48045,47988,47931,47873,47816,47759,47701,47644,47587,47529,47472,47415,47357,47300,47243,47185,47128,47071,47013,46956,46898,46841,46784,46726,46669,46612,46554,46497,46440,46382,46325,46268,46210,46153,46096,46038,45981,45924,45866,45809,45752,45694,45637,45580,45522,45465,45408,45350,45293,45236,45178,45121,45064,45006,44949,44891,44834,44777,44719,44662,44605,44547,44490,44433,44375,44318,44261,44203,44146,44089,44031,43974,43917,43859,43802,43745,43687,43630,43573,43515,43458,43401,43343,43286,43229,43171,43114,43056,42999,42942,42884,42827,42770,42712,42655
    ,42598,42540,42483,42426,42368,42311,42254,42196,42139,42082,42024,41967,41910,41852,41795,41738,41680,41623,41566,41508,41451,41394,41336,41279,41222,41164,41107,41049,40992,40935,40877,40820,40763,40705,40648,40591,40533,40476,40419,40361,40304,40247,40189,40132,40075,40017,39960,39903,39845,39788,39731,39673,39616,39559,39501,39444,39387,39329,39272,39215,39157,39100,39042,38985,38928,38870,38813,38756,38698,38641,38584,38526,38469,38412,38354,38297,38240,38182,38125,38068,38010,37953,37896,37838,37781,37724,37666,37609,37552,37494,37437,37380,37322,37265,37207,37150,37093,37035,36978,36921
    ,36863,36806,36749,36691,36634,36577,36519,36462,36405,36347,36290,36233,36175,36118,36061,36003,35946,35889,35831,35774,35717,35659,35602,35545,35487,35430,35373,35315,35258,35200,35143,35086,35028,34971,34914,34856,34799,34742,34684,34627,34570,34512,34455,34398,34340,34283,34226,34168,34111,34054,33996,33939,33882,33824,33767,33710,33652,33595,33538,33480,33423,33366,33308,33251,33193,33136,33079,33021,32964,32907,32849,32792,32735,32677,32620,32563,32505,32448,32391,32333,32276,32219,32161,32104,32047,31989,31932,31875,31817,31760,31703,31645,31588,31531,31473,31416,31358,31301,31244,31186
    ,31129,31072,31014,30957,30900,30842,30785,30728,30670,30613,30556,30498,30441,30384,30326,30269,30212,30154,30097,30040,29982,29925,29868,29810,29753,29696,29638,29581,29524,29466,29409,29351,29294,29237,29179,29122,29065,29007,28950,28893,28835,28778,28721,28663,28606,28549,28491,28434,28377,28319,28262,28205,28147,28090,28033,27975,27918,27861,27803,27746,27689,27631,27574,27517,27459,27402,27344,27287,27230,27172,27115,27058,27000,26943,26886,26828,26771,26714,26656,26599,26542,26484,26427,26370,26312,26255,26198,26140,26083,26026,25968,25911,25854,25796,25739,25682,25624,25567,25509,25452
    ,25395,25337,25280,25223,25165,25108,25051,24993,24936,24879,24821,24764,24707,24649,24592,24535,24477,24420,24363,24305,24248,24191,24133,24076,24019,23961,23904,23847,23789,23732,23675,23617,23560,23502,23445,23388,23330,23273,23216,23158,23101,23044,22986,22929,22872,22814,22757,22700,22642,22585,22528,22470,22413,22356,22298,22241,22184,22126,22069,22012,21954,21897,21840,21782,21725,21668,21610,21553,21495,21438,21381,21323,21266,21209,21151,21094,21037,20979,20922,20865,20807,20750,20693,20635,20578,20521,20463,20406,20349,20291,20234,20177,20119,20062,20005,19947,19890,19833,19775,19718
    ,19661
};

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同時踏み
};

#define ratioLPF        0.456f //CutOff:10Hz
#define ratioLPF_V      0.061f  //CutOff:10Hz(sampling:10ms)
#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
};

enum SelectMotor {
    RIGHT_MOTOR=0,
    LEFT_MOTOR
};

const int MAX_MOTOR_TORQUE          = 0xFFFF;   //LSB : 45/65535 = 0.686655[mNm]
const int MAX_OUTPUT_TORQUE         = (int)(MAX_MOTOR_TORQUE * 1.0);  //出力最大値
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 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); //トルクに回転数が影響しない領域の境界値(出力電圧値)
const int MAX_REVOLUTION_TORQUE     =(int)(MAX_MOTOR_TORQUE/45.0 * 13.5);     //最高回転数における最大出力トルク

const double FIX_DACOUTFORM         =(double)(((0xFFF/3.3) * 2.5)/0xFFFF);

const int MAX_PULSE_TIME            =5000;  //12000rpmが最高回転数

void initTVD(void);
void driveTVD(void);

int getPulseTime(SelectMotor rl);
float getVelocity(void);
int getCurrentSensor(int sensor);
int getRawSensor(int sensor);
void getCurrentErrCount(struct errCounter_t *ptr);
int calcRequestTorque(void);

#endif