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@2:9d69f27a3d3b, 2016-07-24 (annotated)

Committer:
sift
Date:
Sun Jul 24 02:48:39 2016 +0000
Revision:
2:9d69f27a3d3b
Parent:
1:4d86ec2fe4b1
Child:
3:821e2f07a260
??????????????????????????????LPF????; ??????????????????????????????FB?????????

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