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.cpp@45:8e5d35beb957, 2017-12-06 (annotated)

Committer:
sift
Date:
Wed Dec 06 08:02:28 2017 +0000
Revision:
45:8e5d35beb957
Parent:
44:d433bb5f77c0
Child:
46:16f1a7a01f5f
?????????????????or?????????; ???????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sift 0:276c1dab2d62 1 #include "TVDCTRL.h"
sift 1:4d86ec2fe4b1 2 #include "MCP4922.h"
sift 1:4d86ec2fe4b1 3 #include "Steering.h"
sift 39:c05074379713 4 #include "Global.h"
sift 1:4d86ec2fe4b1 5
sift 1:4d86ec2fe4b1 6 extern AnalogIn apsP;
sift 1:4d86ec2fe4b1 7 extern AnalogIn apsS;
sift 2:9d69f27a3d3b 8 extern AnalogIn brake;
sift 1:4d86ec2fe4b1 9 extern DigitalOut LED[];
sift 27:37a8b4f6d28d 10 extern DigitalOut brakeSignal;
sift 27:37a8b4f6d28d 11 extern DigitalOut indicatorLed;
sift 27:37a8b4f6d28d 12 extern DigitalOut shutDown;
sift 27:37a8b4f6d28d 13 extern DigitalIn sdState;
sift 1:4d86ec2fe4b1 14 extern InterruptIn rightMotorPulse;
sift 1:4d86ec2fe4b1 15 extern InterruptIn leftMotorPulse;
sift 39:c05074379713 16 extern InterruptIn rightWheelPulse1;
sift 39:c05074379713 17 extern InterruptIn rightWheelPulse2;
sift 39:c05074379713 18 extern InterruptIn leftWheelPulse1;
sift 39:c05074379713 19 extern InterruptIn leftWheelPulse2;
sift 1:4d86ec2fe4b1 20 extern MCP4922 mcp;
sift 8:a22aec357a64 21 extern Serial pc;
sift 19:571a4d00b89c 22 extern AnalogOut STR2AN;
sift 27:37a8b4f6d28d 23 extern CAN can;
sift 1:4d86ec2fe4b1 24
sift 26:331e77bb479b 25 #define indicateSystem(x) (indicatorLed.write(x))
sift 26:331e77bb479b 26
sift 43:5da6b1574227 27 Timer wheelPulseTimer[4];
sift 2:9d69f27a3d3b 28 Ticker ticker1;
sift 2:9d69f27a3d3b 29 Ticker ticker2;
sift 1:4d86ec2fe4b1 30
sift 2:9d69f27a3d3b 31 #define apsPVol() (apsP.read() * 3.3)
sift 2:9d69f27a3d3b 32 #define apsSVol() (apsS.read() * 3.3)
sift 0:276c1dab2d62 33
sift 0:276c1dab2d62 34 struct {
sift 0:276c1dab2d62 35 unsigned int valA:12;
sift 0:276c1dab2d62 36 unsigned int valB:12;
sift 2:9d69f27a3d3b 37 } McpData;
sift 1:4d86ec2fe4b1 38
sift 2:9d69f27a3d3b 39 //各変数が一定値を超えた時点でエラー検出とする
sift 2:9d69f27a3d3b 40 //2つのAPSの区別はつけないことにする
sift 12:ae291fa7239c 41 struct errCounter_t errCounter= {0,0,0,0,0,0,0};
sift 1:4d86ec2fe4b1 42
sift 12:ae291fa7239c 43 int readyToDriveFlag = 1;
sift 12:ae291fa7239c 44
sift 12:ae291fa7239c 45 int gApsP=0, gApsS=0, gBrake=0; //現在のセンサ値
sift 12:ae291fa7239c 46 int rawApsP=0, rawApsS=0, rawBrake=0; //現在の補正無しのセンサ値
sift 2:9d69f27a3d3b 47
sift 28:47e9531a3a9d 48 int gRightMotorTorque=0, gLeftMotorTorque=0;
sift 28:47e9531a3a9d 49
sift 43:5da6b1574227 50 int getMotorTorque(select_t rl)
sift 28:47e9531a3a9d 51 {
sift 43:5da6b1574227 52 return ((rl==RL_MOTOR) ? gLeftMotorTorque : gRightMotorTorque);
sift 28:47e9531a3a9d 53 }
sift 28:47e9531a3a9d 54
sift 18:b7c362c8f0fd 55 //エラーカウンタ外部参照用関数
sift 18:b7c362c8f0fd 56 //errCounter_t型変数のポインタを引数に取る
sift 2:9d69f27a3d3b 57 void getCurrentErrCount(struct errCounter_t *ptr)
sift 1:4d86ec2fe4b1 58 {
sift 12:ae291fa7239c 59 ptr->apsUnderVolt = errCounter.apsUnderVolt;
sift 12:ae291fa7239c 60 ptr->apsExceedVolt = errCounter.apsExceedVolt;
sift 12:ae291fa7239c 61 ptr->apsErrorTolerance = errCounter.apsErrorTolerance;
sift 12:ae291fa7239c 62 ptr->apsStick = errCounter.apsStick;
sift 12:ae291fa7239c 63 ptr->brakeUnderVolt = errCounter.brakeUnderVolt;
sift 12:ae291fa7239c 64 ptr->brakeExceedVolt = errCounter.brakeExceedVolt;
sift 12:ae291fa7239c 65 ptr->brakeFuzzyVolt = errCounter.brakeFuzzyVolt;
sift 12:ae291fa7239c 66 ptr->brakeOverRide = errCounter.brakeOverRide;
sift 12:ae291fa7239c 67 }
sift 12:ae291fa7239c 68
sift 18:b7c362c8f0fd 69 //ブレーキONOFF判定関数
sift 12:ae291fa7239c 70 //Brake-ON:1 Brake-OFF:0
sift 12:ae291fa7239c 71 int isBrakeOn(void)
sift 12:ae291fa7239c 72 {
sift 12:ae291fa7239c 73 int brake = gBrake;
sift 12:ae291fa7239c 74 int brakeOnOff = 0;
sift 12:ae291fa7239c 75
sift 30:c596a0f5d685 76 if(brake < (BRK_ON_VOLTAGE + ERROR_TOLERANCE))
sift 12:ae291fa7239c 77 brakeOnOff = 1;
sift 30:c596a0f5d685 78 if(brake > (BRK_OFF_VOLTAGE - ERROR_TOLERANCE))
sift 12:ae291fa7239c 79 brakeOnOff = 0;
sift 12:ae291fa7239c 80
sift 12:ae291fa7239c 81 return brakeOnOff;
sift 2:9d69f27a3d3b 82 }
sift 1:4d86ec2fe4b1 83
sift 18:b7c362c8f0fd 84 //センサ現在値外部参照関数
sift 7:ad013d88a539 85 int getCurrentSensor(int sensor)
sift 2:9d69f27a3d3b 86 {
sift 2:9d69f27a3d3b 87 switch (sensor) {
sift 2:9d69f27a3d3b 88 case APS_PRIMARY:
sift 2:9d69f27a3d3b 89 return gApsP;
sift 2:9d69f27a3d3b 90 case APS_SECONDARY:
sift 2:9d69f27a3d3b 91 return gApsS;
sift 2:9d69f27a3d3b 92 case BRAKE:
sift 2:9d69f27a3d3b 93 return gBrake;
sift 2:9d69f27a3d3b 94 default:
sift 2:9d69f27a3d3b 95 return -1;
sift 1:4d86ec2fe4b1 96 }
sift 2:9d69f27a3d3b 97 }
sift 2:9d69f27a3d3b 98
sift 18:b7c362c8f0fd 99 //補正前センサ現在値外部参照関数
sift 7:ad013d88a539 100 int getRawSensor(int sensor)
sift 2:9d69f27a3d3b 101 {
sift 2:9d69f27a3d3b 102 switch (sensor) {
sift 2:9d69f27a3d3b 103 case APS_PRIMARY:
sift 2:9d69f27a3d3b 104 return rawApsP;
sift 2:9d69f27a3d3b 105 case APS_SECONDARY:
sift 2:9d69f27a3d3b 106 return rawApsS;
sift 2:9d69f27a3d3b 107 case BRAKE:
sift 2:9d69f27a3d3b 108 return rawBrake;
sift 2:9d69f27a3d3b 109 default:
sift 2:9d69f27a3d3b 110 return -1;
sift 1:4d86ec2fe4b1 111 }
sift 2:9d69f27a3d3b 112 }
sift 2:9d69f27a3d3b 113
sift 44:d433bb5f77c0 114 volatile bool loadSensorFlag = false;
sift 2:9d69f27a3d3b 115
sift 2:9d69f27a3d3b 116 //タイマー割り込みでコールされる
sift 2:9d69f27a3d3b 117 void loadSensorsISR(void)
sift 2:9d69f27a3d3b 118 {
sift 2:9d69f27a3d3b 119 loadSensorFlag = true;
sift 1:4d86ec2fe4b1 120 }
sift 1:4d86ec2fe4b1 121
sift 25:c21d35c7f0de 122 //センサ読み込み関数
sift 2:9d69f27a3d3b 123 void loadSensors(void)
sift 1:4d86ec2fe4b1 124 {
sift 2:9d69f27a3d3b 125 if(true == loadSensorFlag) {
sift 2:9d69f27a3d3b 126 loadSensorFlag = false;
sift 2:9d69f27a3d3b 127 static int preApsP=0, preApsS=0; //過去のセンサ値
sift 2:9d69f27a3d3b 128 static int preBrake=0;
sift 2:9d69f27a3d3b 129 int tmpApsP=0, tmpApsS=0, tmpBrake=0; //補正後のセンサ値
sift 2:9d69f27a3d3b 130 int tmpApsErrCountU=0, tmpApsErrCountE=0; //APSの一時的なエラーカウンタ
sift 44:d433bb5f77c0 131 int tmpRawApsP, tmpRawApsS, tmpRawBrake;
sift 2:9d69f27a3d3b 132
sift 44:d433bb5f77c0 133 tmpRawApsP = (int)apsP.read_u16();
sift 44:d433bb5f77c0 134 tmpRawApsS = (int)apsS.read_u16();
sift 44:d433bb5f77c0 135 tmpRawBrake = (int)brake.read_u16();
sift 44:d433bb5f77c0 136
sift 2:9d69f27a3d3b 137 //Low Pass Filter
sift 45:8e5d35beb957 138 tmpApsP = (int)(tmpRawApsP * ratioLPF_ACC_BRK + preApsP * (1.0-ratioLPF_ACC_BRK));
sift 45:8e5d35beb957 139 tmpApsS = (int)(tmpRawApsS * ratioLPF_ACC_BRK + preApsS * (1.0-ratioLPF_ACC_BRK));
sift 45:8e5d35beb957 140 tmpBrake = (int)(tmpRawBrake * ratioLPF_ACC_BRK + preBrake * (1.0-ratioLPF_ACC_BRK));
sift 2:9d69f27a3d3b 141
sift 2:9d69f27a3d3b 142 //生のセンサ値取得
sift 2:9d69f27a3d3b 143 rawApsP = tmpApsP;
sift 2:9d69f27a3d3b 144 rawApsS = tmpApsS;
sift 2:9d69f27a3d3b 145 rawBrake = tmpBrake;
sift 2:9d69f27a3d3b 146
sift 2:9d69f27a3d3b 147 //センサーチェック
sift 2:9d69f27a3d3b 148 //APS上限値チェック
sift 2:9d69f27a3d3b 149 if(tmpApsP > APS_MAX_POSITION + ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 150 tmpApsP = APS_MAX_POSITION; //異常時，上限値にクリップ
sift 2:9d69f27a3d3b 151 tmpApsErrCountE++;
sift 2:9d69f27a3d3b 152 }
sift 2:9d69f27a3d3b 153 if(tmpApsS > APS_MAX_POSITION + ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 154 tmpApsS = APS_MAX_POSITION; //異常時，上限値にクリップ
sift 2:9d69f27a3d3b 155 tmpApsErrCountE++;
sift 2:9d69f27a3d3b 156 }
sift 2:9d69f27a3d3b 157 if(0 == tmpApsErrCountE)
sift 2:9d69f27a3d3b 158 errCounter.apsExceedVolt = 0; //どちらも正常時エラーカウンタクリア
sift 2:9d69f27a3d3b 159 else
sift 2:9d69f27a3d3b 160 errCounter.apsExceedVolt += tmpApsErrCountE;
sift 2:9d69f27a3d3b 161
sift 2:9d69f27a3d3b 162 //APS下限値チェック
sift 2:9d69f27a3d3b 163 if(tmpApsP < APS_MIN_POSITION - ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 164 tmpApsP = APS_MIN_POSITION; //下限値にクリップ
sift 2:9d69f27a3d3b 165 tmpApsErrCountU++;
sift 2:9d69f27a3d3b 166 }
sift 2:9d69f27a3d3b 167 if(tmpApsS < APS_MIN_POSITION - ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 168 tmpApsS = APS_MIN_POSITION; //下限値にクリップ
sift 2:9d69f27a3d3b 169 tmpApsErrCountU++;
sift 2:9d69f27a3d3b 170 }
sift 2:9d69f27a3d3b 171 if(0 == tmpApsErrCountU)
sift 2:9d69f27a3d3b 172 errCounter.apsUnderVolt = 0; //どちらも正常時エラーカウンタクリア
sift 2:9d69f27a3d3b 173 else
sift 2:9d69f27a3d3b 174 errCounter.apsUnderVolt += tmpApsErrCountU;
sift 2:9d69f27a3d3b 175
sift 2:9d69f27a3d3b 176 //センサー偏差チェック
sift 2:9d69f27a3d3b 177 if(myAbs(tmpApsP - tmpApsS) > APS_DEVIATION_TOLERANCE) { //偏差チェックには補正後の値(tmp)を使用
sift 2:9d69f27a3d3b 178 errCounter.apsErrorTolerance++;
sift 2:9d69f27a3d3b 179 } else {
sift 2:9d69f27a3d3b 180 errCounter.apsErrorTolerance = 0;
sift 2:9d69f27a3d3b 181 }
sift 1:4d86ec2fe4b1 182
sift 2:9d69f27a3d3b 183 //小さい方にクリップ
sift 2:9d69f27a3d3b 184 //APS値は好きな方を使いな
sift 2:9d69f27a3d3b 185 if(tmpApsP > tmpApsS) {
sift 2:9d69f27a3d3b 186 tmpApsP = tmpApsS;
sift 2:9d69f27a3d3b 187 } else {
sift 2:9d69f27a3d3b 188 tmpApsS = tmpApsP;
sift 2:9d69f27a3d3b 189 }
sift 2:9d69f27a3d3b 190
sift 2:9d69f27a3d3b 191 //Brake上限値チェック
sift 30:c596a0f5d685 192 if(tmpBrake > BRK_OFF_VOLTAGE + ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 193 errCounter.brakeExceedVolt++;
sift 30:c596a0f5d685 194 tmpBrake = BRK_OFF_VOLTAGE;
sift 2:9d69f27a3d3b 195 } else {
sift 2:9d69f27a3d3b 196 errCounter.brakeExceedVolt = 0;
sift 2:9d69f27a3d3b 197 }
sift 2:9d69f27a3d3b 198
sift 2:9d69f27a3d3b 199 //Brake下限値チェック
sift 30:c596a0f5d685 200 if(tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 201 errCounter.brakeUnderVolt++;
sift 30:c596a0f5d685 202 tmpBrake = BRK_ON_VOLTAGE;
sift 2:9d69f27a3d3b 203 } else {
sift 2:9d69f27a3d3b 204 errCounter.brakeUnderVolt = 0;
sift 2:9d69f27a3d3b 205 }
sift 1:4d86ec2fe4b1 206
sift 2:9d69f27a3d3b 207 //brake範囲外電圧チェック
sift 30:c596a0f5d685 208 if((tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE)) {
sift 2:9d69f27a3d3b 209 errCounter.brakeFuzzyVolt++;
sift 2:9d69f27a3d3b 210 tmpBrake = BRK_OFF_VOLTAGE;
sift 2:9d69f27a3d3b 211 } else {
sift 2:9d69f27a3d3b 212 errCounter.brakeFuzzyVolt=0;
sift 2:9d69f27a3d3b 213 }
sift 2:9d69f27a3d3b 214
sift 2:9d69f27a3d3b 215 //APS固着チェック
sift 2:9d69f27a3d3b 216 if((preApsP == tmpApsP) && (tmpApsP == APS_MAX_POSITION))
sift 2:9d69f27a3d3b 217 errCounter.apsStick++;
sift 2:9d69f27a3d3b 218 else
sift 2:9d69f27a3d3b 219 errCounter.apsStick=0;
sift 2:9d69f27a3d3b 220
sift 2:9d69f27a3d3b 221 //ブレーキオーバーライドチェック
sift 12:ae291fa7239c 222 if((isBrakeOn() == 1) && (tmpApsP >= APS_OVERRIDE25)) //Brake-ON and APS > 25%
sift 2:9d69f27a3d3b 223 errCounter.brakeOverRide++;
sift 41:0c53acd31247 224 if(tmpApsP < APS_OVERRIDE05) //APS < 5%
sift 2:9d69f27a3d3b 225 errCounter.brakeOverRide=0;
sift 2:9d69f27a3d3b 226
sift 2:9d69f27a3d3b 227 //センサ値取得
sift 2:9d69f27a3d3b 228 gApsP = tmpApsP;
sift 2:9d69f27a3d3b 229 gApsS = tmpApsS;
sift 2:9d69f27a3d3b 230 gBrake = tmpBrake;
sift 2:9d69f27a3d3b 231
sift 2:9d69f27a3d3b 232 //未来の自分に期待
sift 2:9d69f27a3d3b 233 preApsP = rawApsP;
sift 2:9d69f27a3d3b 234 preApsS = rawApsS;
sift 2:9d69f27a3d3b 235 preBrake = rawBrake;
sift 2:9d69f27a3d3b 236 }
sift 1:4d86ec2fe4b1 237 }
sift 1:4d86ec2fe4b1 238
sift 43:5da6b1574227 239 //*******************************************************
sift 39:c05074379713 240 //車輪速計測は”【空転再粘着制御】山下道寛”を参照のこと(一部改修)
sift 43:5da6b1574227 241 //*******************************************************
sift 43:5da6b1574227 242 //wheelNumbler:ホイール識別番号
sift 43:5da6b1574227 243 //FR:0 FL:1 RR:2 RL:3
sift 43:5da6b1574227 244 typedef struct {
sift 43:5da6b1574227 245 int counter;
sift 43:5da6b1574227 246 int dT1;
sift 43:5da6b1574227 247 int dT2;
sift 43:5da6b1574227 248 bool preInputState;
sift 43:5da6b1574227 249 int stopCounter;
sift 43:5da6b1574227 250 float preRps;
sift 43:5da6b1574227 251 } rps_t;
sift 43:5da6b1574227 252
sift 39:c05074379713 253 //パルス数カウンタ
sift 43:5da6b1574227 254 volatile int gWheelPulseCounter[4] = {0};
sift 39:c05074379713 255 //パルス入力までの時間
sift 43:5da6b1574227 256 volatile int gWheelPulse_dT2[4] = {0};
sift 43:5da6b1574227 257 //現在の回転数[rps]
sift 43:5da6b1574227 258 float gRps[4] = {0};
sift 43:5da6b1574227 259 //車輪速計測周期フラグ
sift 43:5da6b1574227 260 volatile bool gloadWheelRpsFlag = false;
sift 39:c05074379713 261
sift 43:5da6b1574227 262 //***********************************
sift 39:c05074379713 263 //モータパルスカウント
sift 25:c21d35c7f0de 264 void countRightMotorPulseISR(void)
sift 1:4d86ec2fe4b1 265 {
sift 43:5da6b1574227 266 gWheelPulseCounter[RR_MOTOR]++;
sift 43:5da6b1574227 267 gWheelPulse_dT2[RR_MOTOR] = wheelPulseTimer[RR_MOTOR].read_us(); //現在の時間いただきます
sift 1:4d86ec2fe4b1 268 }
sift 25:c21d35c7f0de 269 void countLeftMotorPulseISR(void)
sift 1:4d86ec2fe4b1 270 {
sift 43:5da6b1574227 271 gWheelPulseCounter[RL_MOTOR]++;
sift 43:5da6b1574227 272 gWheelPulse_dT2[RL_MOTOR] = wheelPulseTimer[RL_MOTOR].read_us(); //現在の時間いただきます
sift 2:9d69f27a3d3b 273 }
sift 43:5da6b1574227 274 //***********************************
sift 39:c05074379713 275 //ホイールパルスカウント
sift 39:c05074379713 276 void countRightWheelPulseISR(void)
sift 39:c05074379713 277 {
sift 43:5da6b1574227 278 gWheelPulseCounter[FR_WHEEL]++;
sift 43:5da6b1574227 279 gWheelPulse_dT2[FR_WHEEL] = wheelPulseTimer[FR_WHEEL].read_us(); //現在の時間いただきます
sift 39:c05074379713 280 }
sift 39:c05074379713 281 void countLeftWheelPulseISR(void)
sift 39:c05074379713 282 {
sift 43:5da6b1574227 283 gWheelPulseCounter[FL_WHEEL]++;
sift 43:5da6b1574227 284 gWheelPulse_dT2[FL_WHEEL] = wheelPulseTimer[FL_WHEEL].read_us(); //現在の時間いただきます
sift 39:c05074379713 285 }
sift 43:5da6b1574227 286 //***********************************
sift 41:0c53acd31247 287
sift 45:8e5d35beb957 288 ////回転数構造体初期化関数
sift 45:8e5d35beb957 289 //rps_t initRps(void)
sift 45:8e5d35beb957 290 //{
sift 45:8e5d35beb957 291 // rps_t initResult = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0f};
sift 45:8e5d35beb957 292 // return initResult;
sift 45:8e5d35beb957 293 //}
sift 45:8e5d35beb957 294 //
sift 43:5da6b1574227 295 //RPS読み込み許可設定関数
sift 43:5da6b1574227 296 void loadRpsISR(void)
sift 43:5da6b1574227 297 {
sift 43:5da6b1574227 298 gloadWheelRpsFlag = true;
sift 39:c05074379713 299 }
sift 2:9d69f27a3d3b 300
sift 43:5da6b1574227 301 //RPS読み込み関数
sift 43:5da6b1574227 302 void loadRps(void)
sift 2:9d69f27a3d3b 303 {
sift 45:8e5d35beb957 304 const rps_t INITRPS = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0f}; //構造体初期化用定数
sift 45:8e5d35beb957 305 // static rps_t rps[4] = {initRps(), initRps(), initRps(), initRps()};
sift 45:8e5d35beb957 306 static rps_t rps[4] = {INITRPS, INITRPS, INITRPS, INITRPS};
sift 43:5da6b1574227 307 static int currentTime[4] = {0};
sift 43:5da6b1574227 308 float pulseNumPerRev;
sift 43:5da6b1574227 309
sift 43:5da6b1574227 310 if(false == gloadWheelRpsFlag)
sift 43:5da6b1574227 311 return;
sift 43:5da6b1574227 312 else
sift 43:5da6b1574227 313 gloadWheelRpsFlag = false;
sift 43:5da6b1574227 314
sift 43:5da6b1574227 315 for(int i=0; i<4; i++) {
sift 43:5da6b1574227 316 rps[i].counter = gWheelPulseCounter[i];
sift 43:5da6b1574227 317 rps[i].dT2 = gWheelPulse_dT2[i];
sift 2:9d69f27a3d3b 318
sift 43:5da6b1574227 319 //前回パルス入力がない場合
sift 43:5da6b1574227 320 if(rps[i].preInputState == false) {
sift 43:5da6b1574227 321 //以前のdT1に前回の計測周期の時間を積算
sift 43:5da6b1574227 322 rps[i].dT1 = rps[i].dT1 + currentTime[i];
sift 43:5da6b1574227 323 //overflow防止処理
sift 43:5da6b1574227 324 if(rps[i].dT1 > MAX_WHEEL_PULSE_TIME_US)
sift 43:5da6b1574227 325 rps[i].dT1 = MAX_WHEEL_PULSE_TIME_US;
sift 43:5da6b1574227 326 }
sift 43:5da6b1574227 327
sift 43:5da6b1574227 328 //現在の時間取得
sift 43:5da6b1574227 329 currentTime[i] = wheelPulseTimer[i].read_us();
sift 43:5da6b1574227 330
sift 43:5da6b1574227 331 //次回計測周期までのパルス時間計測開始
sift 43:5da6b1574227 332 wheelPulseTimer[i].reset();
sift 43:5da6b1574227 333 //パルス数クリア
sift 43:5da6b1574227 334 gWheelPulseCounter[i] = 0;
sift 43:5da6b1574227 335 //dT2の初期値はパルス入力ない状態 => 計測時間=0
sift 43:5da6b1574227 336 gWheelPulse_dT2[i] = 0;
sift 43:5da6b1574227 337
sift 43:5da6b1574227 338 //一回転当たりのパルス数設定
sift 45:8e5d35beb957 339 if(i <= FL_WHEEL)
sift 43:5da6b1574227 340 pulseNumPerRev = WHEEL_PULSE_NUM; //Front車輪パルス数*割込み回数
sift 43:5da6b1574227 341 else
sift 43:5da6b1574227 342 pulseNumPerRev = MOTOR_PULSE_NUM; //モータパルス数*割込み回数
sift 43:5da6b1574227 343
sift 43:5da6b1574227 344 //パルス入力あれば直前のパルス入力からの経過時間取得
sift 43:5da6b1574227 345 if(rps[i].counter != 0) {
sift 43:5da6b1574227 346 rps[i].dT2 = currentTime[i] - rps[i].dT2;
sift 25:c21d35c7f0de 347 }
sift 25:c21d35c7f0de 348
sift 43:5da6b1574227 349 //パルス入力ない場合---(設定回数未満)前回値保持/(設定回数以上)疑似パルス入力判定 (ピーク値を保存したい)
sift 43:5da6b1574227 350 if(rps[i].counter == 0) {
sift 43:5da6b1574227 351 if(rps[i].stopCounter < 50) //低回転数時、急に0rpsと演算しないように前回値保持(設定値はだいたい)
sift 43:5da6b1574227 352 rps[i].stopCounter++;
sift 43:5da6b1574227 353 else
sift 43:5da6b1574227 354 gRps[i] = 0.0f;
sift 43:5da6b1574227 355 } else {
sift 43:5da6b1574227 356 //RPS計算[rps](1sec当たりパルス数/タイヤパルス数)
sift 43:5da6b1574227 357 gRps[i] = ((float)rps[i].counter / ((currentTime[i] + rps[i].dT1 - rps[i].dT2) / 1000000.0f)) / pulseNumPerRev;
sift 43:5da6b1574227 358 gRps[i] = gRps[i] * ratioLPF_RPS + (1.0f-ratioLPF_RPS)*rps[i].preRps;
sift 43:5da6b1574227 359 rps[i].stopCounter = 0;
sift 43:5da6b1574227 360 }
sift 10:87ad65eef0e9 361
sift 43:5da6b1574227 362 rps[i].preRps = gRps[i];
sift 25:c21d35c7f0de 363
sift 43:5da6b1574227 364 //パルス入力あれば次回のdT1はdT2を採用(パルス入力なければ現在値保持)
sift 43:5da6b1574227 365 if(rps[i].counter != 0)
sift 43:5da6b1574227 366 rps[i].dT1 = rps[i].dT2;
sift 43:5da6b1574227 367
sift 43:5da6b1574227 368 if(rps[i].counter == 0)
sift 43:5da6b1574227 369 rps[i].preInputState = false;
sift 43:5da6b1574227 370 else
sift 43:5da6b1574227 371 rps[i].preInputState = true;
sift 2:9d69f27a3d3b 372 }
sift 2:9d69f27a3d3b 373 }
sift 2:9d69f27a3d3b 374
sift 43:5da6b1574227 375 //車輪RPS取得関数
sift 43:5da6b1574227 376 float getWheelRps(select_t position)
sift 43:5da6b1574227 377 {
sift 43:5da6b1574227 378 float deltaN; //左右モータ回転数差
sift 43:5da6b1574227 379 float aveN; //左右モータ回転数平均値
sift 43:5da6b1574227 380
sift 45:8e5d35beb957 381 if(position <= FL_WHEEL) //前輪の場合
sift 45:8e5d35beb957 382 return gRps[position]; //演算結果をそのまま適用
sift 45:8e5d35beb957 383 else { //後輪の場合,モータ回転数から速度線図に従い演算
sift 43:5da6b1574227 384 //右車輪回転数が大きいと仮定
sift 45:8e5d35beb957 385 aveN = ((gRps[RR_MOTOR] + gRps[RL_MOTOR]) / GEAR_RATIO) / 2.0f; //平均回転数計算
sift 45:8e5d35beb957 386 deltaN = ((gRps[RR_MOTOR] - gRps[RL_MOTOR]) / GEAR_RATIO) / ALPHA; //速度線図上の車輪回転数差計算
sift 43:5da6b1574227 387
sift 43:5da6b1574227 388 if(position == RR_MOTOR)
sift 43:5da6b1574227 389 return aveN + deltaN / 2.0f; //右車輪回転数
sift 43:5da6b1574227 390 else
sift 43:5da6b1574227 391 return aveN - deltaN / 2.0f; //左車輪回転数
sift 43:5da6b1574227 392 }
sift 43:5da6b1574227 393 }
sift 43:5da6b1574227 394
sift 43:5da6b1574227 395 float getRps(select_t position)
sift 43:5da6b1574227 396 {
sift 43:5da6b1574227 397 return gRps[position];
sift 43:5da6b1574227 398 }
sift 43:5da6b1574227 399
sift 44:d433bb5f77c0 400 //***********************************
sift 44:d433bb5f77c0 401 //スリップ率取得関数
sift 44:d433bb5f77c0 402 //select :0---RIGHT WHEEL
sift 44:d433bb5f77c0 403 // 1---LEFT WHEEL
sift 44:d433bb5f77c0 404 float getSlipRatio(bool rl_select)
sift 44:d433bb5f77c0 405 {
sift 44:d433bb5f77c0 406 float slipRatio;
sift 44:d433bb5f77c0 407
sift 44:d433bb5f77c0 408 if(rl_select == 0) {
sift 44:d433bb5f77c0 409 slipRatio = 1.0f - getWheelRps(FR_WHEEL) / getWheelRps(RR_MOTOR);
sift 44:d433bb5f77c0 410 } else {
sift 44:d433bb5f77c0 411 slipRatio = 1.0f - getWheelRps(FL_WHEEL) / getWheelRps(RL_MOTOR);
sift 44:d433bb5f77c0 412 }
sift 44:d433bb5f77c0 413
sift 44:d433bb5f77c0 414 if(slipRatio < 0.0f)
sift 44:d433bb5f77c0 415 slipRatio = 0.0f; //減速時は考慮しない
sift 44:d433bb5f77c0 416
sift 44:d433bb5f77c0 417 return slipRatio;
sift 44:d433bb5f77c0 418 }
sift 44:d433bb5f77c0 419
sift 43:5da6b1574227 420 //車速取得関数[m/s]
sift 43:5da6b1574227 421 //左右従動輪回転数の平均値から車速を演算
sift 2:9d69f27a3d3b 422 float getVelocity(void)
sift 2:9d69f27a3d3b 423 {
sift 43:5da6b1574227 424 return (0.5f*TIRE_DIAMETER*2.0f*M_PI*(getWheelRps(FR_WHEEL) + getWheelRps(FL_WHEEL))*0.5f);
sift 1:4d86ec2fe4b1 425 }
sift 1:4d86ec2fe4b1 426
sift 34:594ddb4008b2 427 int distributeTorque_omega(float steeringWheelAngle)
sift 21:bbf2ad7e6602 428 {
sift 21:bbf2ad7e6602 429 static float lastSteering=0.0f;
sift 22:95c1f753ecad 430 float omega=0;
sift 22:95c1f753ecad 431 int disTrq=0;
sift 30:c596a0f5d685 432
sift 34:594ddb4008b2 433 steeringWheelAngle = ratioLPF * steeringWheelAngle + (1.0f - ratioLPF) * lastSteering;
sift 21:bbf2ad7e6602 434
sift 34:594ddb4008b2 435 omega = steeringWheelAngle - lastSteering; //舵角の差分算出
sift 21:bbf2ad7e6602 436 omega /= 0.01f; //制御周期で角速度演算
sift 36:dc33a3a194c9 437
sift 22:95c1f753ecad 438 if(myAbs(omega) < 0.349f) { //20deg/s
sift 21:bbf2ad7e6602 439 disTrq = 0;
sift 22:95c1f753ecad 440 } else if(myAbs(omega) <= 8.727f) { //500deg/s
sift 34:594ddb4008b2 441 disTrq = (int)(MAX_DISTRIBUTION_TORQUE_OMEGA / (8.727f-0.349f) * (myAbs(omega) - 0.349f));
sift 21:bbf2ad7e6602 442 } else
sift 34:594ddb4008b2 443 disTrq = (int)MAX_DISTRIBUTION_TORQUE_OMEGA;
sift 22:95c1f753ecad 444
sift 34:594ddb4008b2 445 lastSteering = steeringWheelAngle;
sift 21:bbf2ad7e6602 446
sift 34:594ddb4008b2 447 if(omega < 0)
sift 22:95c1f753ecad 448 disTrq = -disTrq;
sift 22:95c1f753ecad 449
sift 21:bbf2ad7e6602 450 return disTrq;
sift 21:bbf2ad7e6602 451 }
sift 21:bbf2ad7e6602 452
sift 44:d433bb5f77c0 453 //int distributeTorque(float steeringWheelAngle, float velocity)
sift 44:d433bb5f77c0 454 //{
sift 44:d433bb5f77c0 455 // double V2 = velocity * velocity;
sift 44:d433bb5f77c0 456 // double R = 0.0; //旋回半径
sift 44:d433bb5f77c0 457 // double Gy = 0.0; //横G
sift 44:d433bb5f77c0 458 // double deadband = 0.0;
sift 44:d433bb5f77c0 459 // double steeringAngle = (double)steeringWheelAngle * STEER_RATIO;
sift 44:d433bb5f77c0 460 // double steeringSign = 1.0;
sift 44:d433bb5f77c0 461 // int disTrq = 0;
sift 44:d433bb5f77c0 462 //
sift 44:d433bb5f77c0 463 // if(steeringAngle > 0)
sift 44:d433bb5f77c0 464 // steeringSign = 1.0;
sift 44:d433bb5f77c0 465 // else
sift 44:d433bb5f77c0 466 // steeringSign = -1.0;
sift 44:d433bb5f77c0 467 //
sift 44:d433bb5f77c0 468 // steeringAngle = myAbs(steeringAngle);
sift 44:d433bb5f77c0 469 //
sift 44:d433bb5f77c0 470 // if(steeringAngle <= 0.0001)
sift 44:d433bb5f77c0 471 // steeringAngle = 0.0001;
sift 44:d433bb5f77c0 472 //
sift 44:d433bb5f77c0 473 // R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle; //理論旋回半径 計算
sift 44:d433bb5f77c0 474 // Gy = V2 / R / 9.81; //理論横G
sift 44:d433bb5f77c0 475 //
sift 44:d433bb5f77c0 476 // if(Gy <= deadband)
sift 44:d433bb5f77c0 477 // disTrq = 0;
sift 44:d433bb5f77c0 478 // else if(Gy <= 1.5) {
sift 44:d433bb5f77c0 479 // Gy -= deadband;
sift 44:d433bb5f77c0 480 // disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (1.5 - deadband) * Gy);
sift 44:d433bb5f77c0 481 // } else {
sift 44:d433bb5f77c0 482 // disTrq = MAX_DISTRIBUTION_TORQUE;
sift 44:d433bb5f77c0 483 // }
sift 44:d433bb5f77c0 484 //
sift 44:d433bb5f77c0 485 // return (int)(disTrq * steeringSign);
sift 44:d433bb5f77c0 486 //}
sift 44:d433bb5f77c0 487
sift 44:d433bb5f77c0 488 int distributeTorque(float steeringWheelAngle)
sift 2:9d69f27a3d3b 489 {
sift 44:d433bb5f77c0 490 float deadband = 0.0;
sift 31:042c08a7434f 491 double steeringSign = 1.0;
sift 9:220e4e77e056 492 int disTrq = 0;
sift 25:c21d35c7f0de 493
sift 44:d433bb5f77c0 494 if(steeringWheelAngle > 0)
sift 34:594ddb4008b2 495 steeringSign = 1.0;
sift 25:c21d35c7f0de 496 else
sift 34:594ddb4008b2 497 steeringSign = -1.0;
sift 25:c21d35c7f0de 498
sift 44:d433bb5f77c0 499 if(myAbs(steeringWheelAngle) <= deadband)
sift 9:220e4e77e056 500 disTrq = 0;
sift 44:d433bb5f77c0 501 else if(myAbs(steeringWheelAngle) <= (0.5f*M_PI)) {
sift 44:d433bb5f77c0 502 disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (0.5f*M_PI - deadband) * (myAbs(steeringWheelAngle) - deadband));
sift 25:c21d35c7f0de 503 } else {
sift 19:571a4d00b89c 504 disTrq = MAX_DISTRIBUTION_TORQUE;
sift 25:c21d35c7f0de 505 }
sift 2:9d69f27a3d3b 506
sift 36:dc33a3a194c9 507 return (int)(disTrq * steeringSign);
sift 2:9d69f27a3d3b 508 }
sift 2:9d69f27a3d3b 509
sift 44:d433bb5f77c0 510
sift 25:c21d35c7f0de 511 //rpm +++++ モータ回転数
sift 25:c21d35c7f0de 512 //regSwitch +++++ 回生=1 力行=0
sift 25:c21d35c7f0de 513 inline int calcMaxTorque(int rpm, bool regSwitch)
sift 2:9d69f27a3d3b 514 {
sift 25:c21d35c7f0de 515 int maxTrq=0;
sift 25:c21d35c7f0de 516
sift 25:c21d35c7f0de 517 //後で削除
sift 25:c21d35c7f0de 518 rpm = 2000;
sift 26:331e77bb479b 519 //++++++++++++++++++++
sift 25:c21d35c7f0de 520
sift 25:c21d35c7f0de 521 if(regSwitch == 0) {
sift 25:c21d35c7f0de 522 if(rpm <3000)
sift 25:c21d35c7f0de 523 maxTrq = MAX_MOTOR_TORQUE_POWER;
sift 25:c21d35c7f0de 524 else if(rpm <= 11000)
sift 25:c21d35c7f0de 525 maxTrq = maxTorqueMap[(int)(rpm / 10.0)];
sift 25:c21d35c7f0de 526 else
sift 25:c21d35c7f0de 527 maxTrq = MAX_REVOLUTION_TORQUE_POWER;
sift 25:c21d35c7f0de 528 } else {
sift 28:47e9531a3a9d 529 if(rpm < 600) {
sift 25:c21d35c7f0de 530 maxTrq = 0;
sift 28:47e9531a3a9d 531 } else if(rpm < 1250) {
sift 28:47e9531a3a9d 532 //+++++++++++++++++++++++++++++++++++++
sift 28:47e9531a3a9d 533 //暫定処理 今後回生トルクマップも要作成
sift 28:47e9531a3a9d 534 maxTrq = 0;
sift 28:47e9531a3a9d 535 //+++++++++++++++++++++++++++++++++++++
sift 25:c21d35c7f0de 536 } else if(rpm <= 6000) {
sift 26:331e77bb479b 537 maxTrq = MAX_MOTOR_TORQUE_REGENERATIVE;
sift 26:331e77bb479b 538 } else if(rpm <= 11000) {
sift 25:c21d35c7f0de 539 //+++++++++++++++++++++++++++++++++++++
sift 25:c21d35c7f0de 540 //暫定処理 今後回生トルクマップも要作成
sift 26:331e77bb479b 541 maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE;
sift 25:c21d35c7f0de 542 //+++++++++++++++++++++++++++++++++++++
sift 25:c21d35c7f0de 543 } else {
sift 25:c21d35c7f0de 544 maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE;
sift 25:c21d35c7f0de 545 }
sift 25:c21d35c7f0de 546 }
sift 25:c21d35c7f0de 547 return maxTrq;
sift 2:9d69f27a3d3b 548 }
sift 2:9d69f27a3d3b 549
sift 25:c21d35c7f0de 550 //演算方法
sift 25:c21d35c7f0de 551 //y = a(x - b) + c
sift 25:c21d35c7f0de 552 //x = 1/a * (y + ab - c)
sift 25:c21d35c7f0de 553 unsigned int calcTorqueToVoltage(int reqTorque, int rpm)
sift 2:9d69f27a3d3b 554 {
sift 25:c21d35c7f0de 555 float slope = 0; //a
sift 25:c21d35c7f0de 556 int startPoint = 0; //b
sift 25:c21d35c7f0de 557 int intercept = 0; //c
sift 12:ae291fa7239c 558
sift 25:c21d35c7f0de 559 int outputVoltage=0;
sift 16:7afd623ef48a 560
sift 25:c21d35c7f0de 561 if(reqTorque > LINEAR_REGION_TORQUE_POWER) { //力行トルクがrpmに対して非線形となる領域
sift 25:c21d35c7f0de 562 slope = (float)(calcMaxTorque(rpm, 0) - LINEAR_REGION_TORQUE_POWER)/(DACOUTPUT_MAX - LINEAR_REGION_VOLTAGE_POWER);
sift 25:c21d35c7f0de 563 startPoint = LINEAR_REGION_VOLTAGE_POWER;
sift 25:c21d35c7f0de 564 intercept = LINEAR_REGION_TORQUE_POWER;
sift 25:c21d35c7f0de 565
sift 25:c21d35c7f0de 566 outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
sift 6:26fa8c78500e 567
sift 25:c21d35c7f0de 568 } else if(reqTorque > 0) { //力行トルクがrpmに対して線形となる領域
sift 25:c21d35c7f0de 569 slope = (float)LINEAR_REGION_TORQUE_POWER/(LINEAR_REGION_VOLTAGE_POWER - ZERO_TORQUE_VOLTAGE_P);
sift 25:c21d35c7f0de 570 startPoint = ZERO_TORQUE_VOLTAGE_P;
sift 25:c21d35c7f0de 571 intercept = 0;
sift 25:c21d35c7f0de 572
sift 25:c21d35c7f0de 573 outputVoltage = (int)(reqTorque/slope + startPoint);
sift 25:c21d35c7f0de 574
sift 25:c21d35c7f0de 575 } else if(0 == reqTorque) {
sift 25:c21d35c7f0de 576
sift 25:c21d35c7f0de 577 outputVoltage = ZERO_TORQUE_VOLTAGE_NEUTRAL; //ニュートラル信号
sift 6:26fa8c78500e 578
sift 25:c21d35c7f0de 579 } else if(reqTorque > LINEAR_REGION_TORQUE_REGENERATIVE) { //回生トルクがrpmに対して線形となる領域
sift 25:c21d35c7f0de 580 slope = (float)(0 - LINEAR_REGION_TORQUE_REGENERATIVE)/(ZERO_TORQUE_VOLTAGE_REG - LINEAR_REGION_VOLTAGE_REGENERATIVE);
sift 25:c21d35c7f0de 581 startPoint = LINEAR_REGION_VOLTAGE_REGENERATIVE;
sift 25:c21d35c7f0de 582 intercept = LINEAR_REGION_TORQUE_REGENERATIVE;
sift 25:c21d35c7f0de 583
sift 25:c21d35c7f0de 584 outputVoltage = (int)(reqTorque/slope + startPoint);
sift 25:c21d35c7f0de 585
sift 25:c21d35c7f0de 586 } else { //回生トルクがrpmに対して非線形となる領域
sift 25:c21d35c7f0de 587 slope = (float)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/(LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN);
sift 25:c21d35c7f0de 588 startPoint = DACOUTPUT_MIN;
sift 25:c21d35c7f0de 589 intercept = calcMaxTorque(rpm, 1);
sift 25:c21d35c7f0de 590
sift 25:c21d35c7f0de 591 outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
sift 2:9d69f27a3d3b 592 }
sift 2:9d69f27a3d3b 593
sift 25:c21d35c7f0de 594 if(outputVoltage > DACOUTPUT_MAX)
sift 25:c21d35c7f0de 595 outputVoltage = DACOUTPUT_MAX;
sift 25:c21d35c7f0de 596 if(outputVoltage < DACOUTPUT_MIN)
sift 25:c21d35c7f0de 597 outputVoltage = DACOUTPUT_MIN;
sift 16:7afd623ef48a 598
sift 20:3c5061281a7a 599 return (unsigned int)(0xFFF*((double)outputVoltage/0xFFFF)); //DACの分解能に適応(16bit->12bit)
sift 2:9d69f27a3d3b 600 }
sift 2:9d69f27a3d3b 601
sift 2:9d69f27a3d3b 602 int calcRequestTorque(void)
sift 2:9d69f27a3d3b 603 {
sift 2:9d69f27a3d3b 604 int currentAPS;
sift 2:9d69f27a3d3b 605 int requestTorque;
sift 2:9d69f27a3d3b 606
sift 2:9d69f27a3d3b 607 currentAPS = ((gApsP>gApsS) ? gApsS : gApsP); //センサ値は小さい方を採用
sift 12:ae291fa7239c 608
sift 2:9d69f27a3d3b 609 if(currentAPS < APS_MIN_POSITION)
sift 2:9d69f27a3d3b 610 currentAPS = 0;
sift 2:9d69f27a3d3b 611 else
sift 2:9d69f27a3d3b 612 currentAPS -= APS_MIN_POSITION; //オフセット修正
sift 2:9d69f27a3d3b 613
sift 25:c21d35c7f0de 614 if(currentAPS <= APS_REG_RANGE) //デッドバンド内であれば要求トルク->0
sift 25:c21d35c7f0de 615 requestTorque = (int)((float)(-MAX_OUTPUT_TORQUE_REGENERATIVE) / APS_REG_RANGE * currentAPS + MAX_OUTPUT_TORQUE_REGENERATIVE);
sift 2:9d69f27a3d3b 616 else
sift 25:c21d35c7f0de 617 requestTorque = (int)((float)MAX_OUTPUT_TORQUE_POWER / APS_PWR_RANGE * (currentAPS - APS_REG_RANGE));
sift 2:9d69f27a3d3b 618
sift 25:c21d35c7f0de 619 if(requestTorque > MAX_OUTPUT_TORQUE_POWER)
sift 25:c21d35c7f0de 620 requestTorque = MAX_OUTPUT_TORQUE_POWER;
sift 25:c21d35c7f0de 621 else if(requestTorque < MAX_OUTPUT_TORQUE_REGENERATIVE)
sift 25:c21d35c7f0de 622 requestTorque = MAX_OUTPUT_TORQUE_REGENERATIVE;
sift 2:9d69f27a3d3b 623
sift 2:9d69f27a3d3b 624 return requestTorque;
sift 2:9d69f27a3d3b 625 }
sift 2:9d69f27a3d3b 626
sift 17:a2246ce3333f 627 //トルク配分車速制限関数
sift 17:a2246ce3333f 628 //車速が低速域の場合，トルク配分0
sift 17:a2246ce3333f 629 float limitTorqueDistribution(void)
sift 17:a2246ce3333f 630 {
sift 17:a2246ce3333f 631 float limitRate;
sift 17:a2246ce3333f 632 float currentVelocity = getVelocity() * 3.6f; //km/hで車速取得
sift 20:3c5061281a7a 633
sift 37:ba10cf09c151 634 if(currentVelocity < 15.0f)
sift 17:a2246ce3333f 635 limitRate = 0.0f;
sift 36:dc33a3a194c9 636 else if(currentVelocity < 30.0f)
sift 38:11753ee9734f 637 limitRate = (currentVelocity - 15.0f) / (30.0f - 15.0f);
sift 17:a2246ce3333f 638 else
sift 17:a2246ce3333f 639 limitRate = 1.0f;
sift 36:dc33a3a194c9 640
sift 17:a2246ce3333f 641 return limitRate;
sift 17:a2246ce3333f 642 }
sift 17:a2246ce3333f 643
sift 26:331e77bb479b 644 void driveTVD(int TVDmode, bool isRedyToDrive)
sift 1:4d86ec2fe4b1 645 {
sift 6:26fa8c78500e 646 int requestTorque=0; //ドライバー要求トルク
sift 6:26fa8c78500e 647 int distributionTrq=0; //分配トルク
sift 21:bbf2ad7e6602 648 int disTrq_omega=0;
sift 21:bbf2ad7e6602 649 int torqueRight, torqueLeft; //トルクの右左
sift 20:3c5061281a7a 650 static unsigned int preMcpA=0, preMcpB=0;
sift 20:3c5061281a7a 651
sift 2:9d69f27a3d3b 652 loadSensors(); //APS,BRAKE更新
sift 2:9d69f27a3d3b 653 loadSteerAngle(); //舵角更新
sift 44:d433bb5f77c0 654 loadRps(); //従動輪・モータ回転数更新
sift 41:0c53acd31247 655
sift 26:331e77bb479b 656 if(isRedyToDrive && isBrakeOn())
sift 12:ae291fa7239c 657 readyToDriveFlag = 0;
sift 2:9d69f27a3d3b 658
sift 12:ae291fa7239c 659 if((errCounter.apsUnderVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 660 || (errCounter.apsExceedVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 661 || (errCounter.apsErrorTolerance > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 662 // || (errCounter.apsStick > ERRCOUNTER_DECISION)
sift 30:c596a0f5d685 663 || (errCounter.brakeUnderVolt > ERRCOUNTER_DECISION)
sift 30:c596a0f5d685 664 || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION)
sift 30:c596a0f5d685 665 || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 666 ) {
sift 16:7afd623ef48a 667 readyToDriveFlag = 1;
sift 12:ae291fa7239c 668 }
sift 16:7afd623ef48a 669
sift 12:ae291fa7239c 670 indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
sift 20:3c5061281a7a 671 LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);
sift 16:7afd623ef48a 672
sift 6:26fa8c78500e 673 requestTorque=calcRequestTorque(); //ドライバー要求トルク取得
sift 4:d7778cde0aff 674
sift 45:8e5d35beb957 675 if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1))
sift 45:8e5d35beb957 676 requestTorque = 0;
sift 45:8e5d35beb957 677
sift 44:d433bb5f77c0 678 distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) / 2.0f); //片モーターのトルク分配量計算
sift 38:11753ee9734f 679 disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f)*limitTorqueDistribution()) / 2.0f); //微分制御
sift 12:ae291fa7239c 680
sift 45:8e5d35beb957 681 distributionTrq = 0;
sift 35:b75595b1da36 682 disTrq_omega = 0;
sift 31:042c08a7434f 683
sift 25:c21d35c7f0de 684 torqueRight = requestTorque + distributionTrq;
sift 25:c21d35c7f0de 685 torqueLeft = requestTorque - distributionTrq;
sift 21:bbf2ad7e6602 686
sift 22:95c1f753ecad 687 torqueRight += disTrq_omega;
sift 22:95c1f753ecad 688 torqueLeft -= disTrq_omega;
sift 20:3c5061281a7a 689
sift 44:d433bb5f77c0 690 //現在バグあり
sift 44:d433bb5f77c0 691 //アクセル全開で旋回後、舵を中立に戻していくと加速する。旋回を優先するモード
sift 44:d433bb5f77c0 692 if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
sift 44:d433bb5f77c0 693 torqueRight = torqueLeft = requestTorque; //内輪側モーター最低トルクより小さい要求トルクなら等配分
sift 44:d433bb5f77c0 694 } else {
sift 44:d433bb5f77c0 695 if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
sift 44:d433bb5f77c0 696 torqueLeft = MAX_OUTPUT_TORQUE_POWER;
sift 44:d433bb5f77c0 697
sift 44:d433bb5f77c0 698 if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
sift 44:d433bb5f77c0 699 torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
sift 44:d433bb5f77c0 700 }
sift 38:11753ee9734f 701 }
sift 44:d433bb5f77c0 702 if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
sift 44:d433bb5f77c0 703 torqueRight = MAX_OUTPUT_TORQUE_POWER;
sift 44:d433bb5f77c0 704 if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
sift 44:d433bb5f77c0 705 torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
sift 44:d433bb5f77c0 706 }
sift 38:11753ee9734f 707 }
sift 44:d433bb5f77c0 708 if(torqueLeft < MIN_INNERWHEEL_MOTOR_TORQUE) { //内輪最低トルク時
sift 44:d433bb5f77c0 709 torqueLeft = MIN_INNERWHEEL_MOTOR_TORQUE; //内輪最低トルクにクリップ
sift 44:d433bb5f77c0 710 torqueRight = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE; //片モーター下限値時，トルク高側のモーターも出力クリップ
sift 44:d433bb5f77c0 711 }
sift 44:d433bb5f77c0 712 if(torqueRight < MIN_INNERWHEEL_MOTOR_TORQUE) { //内輪最低トルク時
sift 44:d433bb5f77c0 713 torqueRight = MIN_INNERWHEEL_MOTOR_TORQUE; //内輪最低トルクにクリップ
sift 44:d433bb5f77c0 714 torqueLeft = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE; //片モーター下限値時，トルク高側のモーターも出力クリップ
sift 44:d433bb5f77c0 715 }
sift 26:331e77bb479b 716 }
sift 34:594ddb4008b2 717
sift 34:594ddb4008b2 718 gRightMotorTorque = torqueRight;
sift 34:594ddb4008b2 719 gLeftMotorTorque = torqueLeft;
sift 34:594ddb4008b2 720
sift 44:d433bb5f77c0 721 McpData.valA = calcTorqueToVoltage(torqueRight, getRps(RR_MOTOR) * 60.0f);
sift 44:d433bb5f77c0 722 McpData.valB = calcTorqueToVoltage(torqueLeft, getRps(RL_MOTOR) * 60.0f);
sift 16:7afd623ef48a 723
sift 38:11753ee9734f 724 preMcpA = McpData.valA;
sift 38:11753ee9734f 725 preMcpB = McpData.valB;
sift 20:3c5061281a7a 726
sift 20:3c5061281a7a 727 mcp.writeA(preMcpA); //右モーター
sift 20:3c5061281a7a 728 mcp.writeB(preMcpB); //左モーター
sift 1:4d86ec2fe4b1 729 }
sift 1:4d86ec2fe4b1 730
sift 1:4d86ec2fe4b1 731 void initTVD(void)
sift 1:4d86ec2fe4b1 732 {
sift 43:5da6b1574227 733 mcp.writeA(0); //右モーター初期値
sift 43:5da6b1574227 734 mcp.writeB(0); //左モーター初期値
sift 39:c05074379713 735
sift 43:5da6b1574227 736 wheelPulseTimer[FR_WHEEL].reset();
sift 43:5da6b1574227 737 wheelPulseTimer[FL_WHEEL].reset();
sift 43:5da6b1574227 738 wheelPulseTimer[RR_MOTOR].reset();
sift 43:5da6b1574227 739 wheelPulseTimer[RL_MOTOR].reset();
sift 1:4d86ec2fe4b1 740
sift 43:5da6b1574227 741 wheelPulseTimer[FR_WHEEL].start();
sift 43:5da6b1574227 742 wheelPulseTimer[FL_WHEEL].start();
sift 43:5da6b1574227 743 wheelPulseTimer[RR_MOTOR].start();
sift 43:5da6b1574227 744 wheelPulseTimer[RL_MOTOR].start();
sift 43:5da6b1574227 745
sift 43:5da6b1574227 746 rightMotorPulse.rise(&countRightMotorPulseISR);
sift 43:5da6b1574227 747 leftMotorPulse.rise(&countLeftMotorPulseISR);
sift 43:5da6b1574227 748
sift 43:5da6b1574227 749 // rightWheelPulse1.fall(&countRightWheelPulseISR); //パルス測定は立ち上がりor立下りのどちらかを計測するのが吉
sift 43:5da6b1574227 750 // rightWheelPulse2.fall(&countRightWheelPulseISR); //立下り特性悪すぎなので測定誤差が増える
sift 42:3ab09d0e3071 751 rightWheelPulse1.rise(&countRightWheelPulseISR);
sift 43:5da6b1574227 752 // rightWheelPulse2.rise(&countRightWheelPulseISR);
sift 25:c21d35c7f0de 753
sift 43:5da6b1574227 754 // leftWheelPulse1.fall(&countLeftWheelPulseISR);
sift 43:5da6b1574227 755 // leftWheelPulse2.fall(&countLeftWheelPulseISR);
sift 43:5da6b1574227 756 leftWheelPulse1.rise(&countLeftWheelPulseISR);
sift 43:5da6b1574227 757 // leftWheelPulse2.rise(&countLeftWheelPulseISR); //AB相の位相差が90度から離れすぎなので測定誤差が増える(スリットが一定間隔でないことになる)
sift 10:87ad65eef0e9 758
sift 43:5da6b1574227 759 ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S); //制御周期毎にデータ読み込み(LPF演算のため)
sift 43:5da6b1574227 760 ticker2.attach(&loadRpsISR, RPS_MEAS_CYCLE_S); //RPS計測周期設定
sift 28:47e9531a3a9d 761
sift 30:c596a0f5d685 762 printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_POWER);
sift 30:c596a0f5d685 763 printf("MAX OUTPUT REG-TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_REGENERATIVE);
sift 30:c596a0f5d685 764 printf("MAX DISTRIBUTION TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_DISTRIBUTION_TORQUE);
sift 30:c596a0f5d685 765 printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MIN_INNERWHEEL_MOTOR_TORQUE);
sift 43:5da6b1574227 766 }