2014 sift / Mbed 2 deprecated TVDctrller2017_brdRev1_PandA

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of TVDctrller2017_brdRev1_ver6 by 2014 sift

TVDCTRL.h@6:26fa8c78500e, 2016-07-26 (annotated)

Committer:
sift
Date:
Tue Jul 26 05:12:15 2016 +0000
Revision:
6:26fa8c78500e
Parent:
5:a5462959b3ab
Child:
7:ad013d88a539
??????????????????????16bit??????????????????????????????; ?????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sift 0:276c1dab2d62 1 #ifndef TVDCTRL_H
sift 0:276c1dab2d62 2 #define TVDCTRL_H
sift 0:276c1dab2d62 3 #include "mbed.h"
sift 0:276c1dab2d62 4
sift 2:9d69f27a3d3b 5 //10rpm刻みでその回転数での最大出力トルクを返す(3000rpm~)
sift 2:9d69f27a3d3b 6 //[0] :3000
sift 2:9d69f27a3d3b 7 //[+1] :+10rpm
sift 6:26fa8c78500e 8 const int calcMaxTorque[801] = {
sift 6:26fa8c78500e 9 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
sift 6:26fa8c78500e 10 ,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
sift 6:26fa8c78500e 11 ,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
sift 6:26fa8c78500e 12 ,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
sift 6:26fa8c78500e 13 ,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
sift 6:26fa8c78500e 14 ,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
sift 6:26fa8c78500e 15 ,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
sift 6:26fa8c78500e 16 ,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
sift 6:26fa8c78500e 17 ,19661
sift 2:9d69f27a3d3b 18 };
sift 2:9d69f27a3d3b 19
sift 2:9d69f27a3d3b 20 struct errCounter_t {
sift 2:9d69f27a3d3b 21 int apsUnderVolt; //aps電圧不足
sift 2:9d69f27a3d3b 22 int apsExceedVolt; //aps電圧超過
sift 2:9d69f27a3d3b 23 int apsErrorTolerance; //aps偏差超過
sift 2:9d69f27a3d3b 24 int apsStick; //aps固着
sift 2:9d69f27a3d3b 25 int brakeUnderVolt; //brake電圧不足
sift 2:9d69f27a3d3b 26 int brakeExceedVolt; //brake電圧超過
sift 2:9d69f27a3d3b 27 int brakeFuzzyVolt; //brake曖昧な電圧(ONでもOFFでもない)
sift 2:9d69f27a3d3b 28 int brakeOverRide; //accel-brake同時踏み
sift 0:276c1dab2d62 29 };
sift 0:276c1dab2d62 30
sift 3:821e2f07a260 31 #define ratioLPF 0.456f //CutOff:10Hz
sift 5:a5462959b3ab 32 #define ratioLPF_V 0.061f //CutOff:10Hz(sampling:10ms)
sift 2:9d69f27a3d3b 33 #define M_PI 3.1415f //[-]
sift 2:9d69f27a3d3b 34 #define TVD_GEAR_RATIO 10.0f //[-]
sift 2:9d69f27a3d3b 35 #define TIRE_DIAMETER 0.533f //[m]
sift 2:9d69f27a3d3b 36 #define WHEEL_BASE 1.760f //[m]
sift 2:9d69f27a3d3b 37 #define STABIRITY_FACTOR 0.01f //[-]
sift 2:9d69f27a3d3b 38 #define TREAD 1.3f //[m]
sift 2:9d69f27a3d3b 39
sift 2:9d69f27a3d3b 40 enum {
sift 2:9d69f27a3d3b 41 APS_PRIMARY=0,
sift 2:9d69f27a3d3b 42 APS_SECONDARY,
sift 2:9d69f27a3d3b 43 BRAKE
sift 0:276c1dab2d62 44 };
sift 2:9d69f27a3d3b 45
sift 2:9d69f27a3d3b 46 enum SelectMotor {
sift 2:9d69f27a3d3b 47 RIGHT_MOTOR=0,
sift 2:9d69f27a3d3b 48 LEFT_MOTOR
sift 0:276c1dab2d62 49 };
sift 0:276c1dab2d62 50
sift 5:a5462959b3ab 51 const int MAX_MOTOR_TORQUE = 0xFFFF; //LSB : 45/65535 = 0.686655[mNm]
sift 5:a5462959b3ab 52 const int MAX_OUTPUT_TORQUE = (int)(MAX_MOTOR_TORQUE * 0.7); //出力最大値
sift 2:9d69f27a3d3b 53 const float MAX_DISTRIBUTION_TORQUE = (MAX_OUTPUT_TORQUE*0.8);
sift 5:a5462959b3ab 54 const float MAX_STEER_ANGLE = (float)(M_PI/180.0 * 25.0); //[-]
sift 2:9d69f27a3d3b 55
sift 2:9d69f27a3d3b 56 const int APS_MIN_POSITION =(int)(0xFFFF/3.3 * 1.0); //"正常時"最小入力電圧
sift 2:9d69f27a3d3b 57 const int APS_MAX_POSITION =(int)(0xFFFF/3.3 * 2.0); //"正常時"最大入力電圧
sift 2:9d69f27a3d3b 58 const int APS_DEADBAND =(int)((APS_MAX_POSITION - APS_MIN_POSITION) * 0.2); //APS信号不感帯
sift 2:9d69f27a3d3b 59 const int APS_VALID_RANGE =(APS_MAX_POSITION - APS_MIN_POSITION) - APS_DEADBAND; //APS信号有効範囲
sift 2:9d69f27a3d3b 60 const int APS_DEVIATION_TOLERANCE =(int)((APS_MAX_POSITION - APS_MIN_POSITION) * 0.1); //10%偏差許容値
sift 2:9d69f27a3d3b 61 const int APS_OVERRIDE =(int)(APS_VALID_RANGE * 0.25); //ブレーキオーバーライドの閾値
sift 1:4d86ec2fe4b1 62
sift 2:9d69f27a3d3b 63 const int BRK_ON_VOLTAGE =(int)(0xFFFF/3.3 * 1.65); //ブレーキON時電圧
sift 2:9d69f27a3d3b 64 const int BRK_OFF_VOLTAGE =(int)(0xFFFF/3.3 * 2.20); //ブレーキOFF時電圧
sift 2:9d69f27a3d3b 65
sift 2:9d69f27a3d3b 66 const int ERROR_TOLERANCE =(int)(0xFFFF * 0.05); //センサ値誤差許容値
sift 2:9d69f27a3d3b 67
sift 2:9d69f27a3d3b 68 //DACは12bit仕様だが一旦16bitに統一してから12bitに変換する
sift 2:9d69f27a3d3b 69 const int DACOUTPUT_MIN =(int)(0xFFFF/3.3 * 0.49); //MCの最低入力電圧
sift 2:9d69f27a3d3b 70 const int DACOUTPUT_MAX =(int)(0xFFFF/3.3 * 2.50); //MCの最大入力電圧
sift 2:9d69f27a3d3b 71 const int DACOUTPUT_VALID_RANGE =DACOUTPUT_MAX - DACOUTPUT_MIN; //実質有効電圧範囲
sift 2:9d69f27a3d3b 72
sift 2:9d69f27a3d3b 73 const int LINEAR_REGION_TORQUE =(int)(MAX_MOTOR_TORQUE/45.0 * 2.5); //トルクに回転数が影響しない領域の境界値(トルク値)
sift 2:9d69f27a3d3b 74 const int LINEAR_REGION_VOLTAGE =(int)(0xFFFF/3.3 * 1.0) - DACOUTPUT_MIN; //トルクに回転数が影響しない領域の境界値(出力電圧値)
sift 2:9d69f27a3d3b 75 const int MAX_REVOLUTION_TORQUE =(int)(MAX_MOTOR_TORQUE/45.0 * 13.5); //最高回転数における最大出力トルク
sift 2:9d69f27a3d3b 76
sift 2:9d69f27a3d3b 77 const float FIX_DACOUTFORM =(float)((0xFFF/3.3 * 2.5)/0xFFFF);
sift 1:4d86ec2fe4b1 78
sift 5:a5462959b3ab 79 const int MAX_PULSE_TIME =5000; //12000rpmが最高回転数
sift 5:a5462959b3ab 80
sift 1:4d86ec2fe4b1 81 void loadSensorsLPF(void);
sift 1:4d86ec2fe4b1 82 int checkSensorPlausibility(void);
sift 1:4d86ec2fe4b1 83
sift 1:4d86ec2fe4b1 84 void initTVD(void);
sift 2:9d69f27a3d3b 85 void driveTVD(void);
sift 1:4d86ec2fe4b1 86
sift 5:a5462959b3ab 87 int getPulseTime(SelectMotor rl);
sift 2:9d69f27a3d3b 88 float getVelocity(void);
sift 2:9d69f27a3d3b 89 int loadCurrentSensor(int sensor);
sift 2:9d69f27a3d3b 90 int loadRawSensor(int sensor);
sift 2:9d69f27a3d3b 91 void getCurrentErrCount(struct errCounter_t *ptr);
sift 2:9d69f27a3d3b 92 int calcRequestTorque(void);
sift 2:9d69f27a3d3b 93
sift 0:276c1dab2d62 94
sift 0:276c1dab2d62 95 #endif