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@38:11753ee9734f, 2017-10-24 (annotated)

Committer:
sift
Date:
Tue Oct 24 10:19:51 2017 +0000
Revision:
38:11753ee9734f
Parent:
37:ba10cf09c151
Child:
39:c05074379713
?????????????; ?????0.0001???????????Overflow?????????

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 1:4d86ec2fe4b1 4
sift 1:4d86ec2fe4b1 5 extern AnalogIn apsP;
sift 1:4d86ec2fe4b1 6 extern AnalogIn apsS;
sift 2:9d69f27a3d3b 7 extern AnalogIn brake;
sift 1:4d86ec2fe4b1 8 extern DigitalOut LED[];
sift 27:37a8b4f6d28d 9 extern DigitalOut brakeSignal;
sift 27:37a8b4f6d28d 10 extern DigitalOut indicatorLed;
sift 27:37a8b4f6d28d 11 extern DigitalOut shutDown;
sift 27:37a8b4f6d28d 12 extern DigitalIn sdState;
sift 1:4d86ec2fe4b1 13 extern InterruptIn rightMotorPulse;
sift 1:4d86ec2fe4b1 14 extern InterruptIn leftMotorPulse;
sift 24:1de0291bc5eb 15 extern InterruptIn rightTirePulse1;
sift 24:1de0291bc5eb 16 extern InterruptIn rightTirePulse2;
sift 24:1de0291bc5eb 17 extern InterruptIn leftTirePulse1;
sift 24:1de0291bc5eb 18 extern InterruptIn leftTirePulse2;
sift 1:4d86ec2fe4b1 19 extern MCP4922 mcp;
sift 8:a22aec357a64 20 extern Serial pc;
sift 19:571a4d00b89c 21 extern AnalogOut STR2AN;
sift 27:37a8b4f6d28d 22 extern CAN can;
sift 1:4d86ec2fe4b1 23
sift 26:331e77bb479b 24 #define indicateSystem(x) (indicatorLed.write(x))
sift 26:331e77bb479b 25
sift 2:9d69f27a3d3b 26 Timer RightPulseTimer;
sift 2:9d69f27a3d3b 27 Timer LeftPulseTimer;
sift 2:9d69f27a3d3b 28 Ticker ticker1;
sift 2:9d69f27a3d3b 29 Ticker ticker2;
sift 1:4d86ec2fe4b1 30
sift 18:b7c362c8f0fd 31 #define myAbs(x) ((x>0)?(x):(-(x)))
sift 18:b7c362c8f0fd 32
sift 2:9d69f27a3d3b 33 #define apsPVol() (apsP.read() * 3.3)
sift 2:9d69f27a3d3b 34 #define apsSVol() (apsS.read() * 3.3)
sift 0:276c1dab2d62 35
sift 0:276c1dab2d62 36 struct {
sift 0:276c1dab2d62 37 unsigned int valA:12;
sift 0:276c1dab2d62 38 unsigned int valB:12;
sift 2:9d69f27a3d3b 39 } McpData;
sift 1:4d86ec2fe4b1 40
sift 2:9d69f27a3d3b 41 //各変数が一定値を超えた時点でエラー検出とする
sift 2:9d69f27a3d3b 42 //2つのAPSの区別はつけないことにする
sift 12:ae291fa7239c 43 struct errCounter_t errCounter= {0,0,0,0,0,0,0};
sift 1:4d86ec2fe4b1 44
sift 12:ae291fa7239c 45 int readyToDriveFlag = 1;
sift 12:ae291fa7239c 46
sift 12:ae291fa7239c 47 int gApsP=0, gApsS=0, gBrake=0; //現在のセンサ値
sift 12:ae291fa7239c 48 int rawApsP=0, rawApsS=0, rawBrake=0; //現在の補正無しのセンサ値
sift 2:9d69f27a3d3b 49
sift 28:47e9531a3a9d 50 int gRightMotorTorque=0, gLeftMotorTorque=0;
sift 28:47e9531a3a9d 51
sift 28:47e9531a3a9d 52 int getMotorTorque(Select rl)
sift 28:47e9531a3a9d 53 {
sift 28:47e9531a3a9d 54 return ((rl==LEFT) ? gLeftMotorTorque : gRightMotorTorque);
sift 28:47e9531a3a9d 55 }
sift 28:47e9531a3a9d 56
sift 18:b7c362c8f0fd 57 //エラーカウンタ外部参照用関数
sift 18:b7c362c8f0fd 58 //errCounter_t型変数のポインタを引数に取る
sift 2:9d69f27a3d3b 59 void getCurrentErrCount(struct errCounter_t *ptr)
sift 1:4d86ec2fe4b1 60 {
sift 12:ae291fa7239c 61 ptr->apsUnderVolt = errCounter.apsUnderVolt;
sift 12:ae291fa7239c 62 ptr->apsExceedVolt = errCounter.apsExceedVolt;
sift 12:ae291fa7239c 63 ptr->apsErrorTolerance = errCounter.apsErrorTolerance;
sift 12:ae291fa7239c 64 ptr->apsStick = errCounter.apsStick;
sift 12:ae291fa7239c 65 ptr->brakeUnderVolt = errCounter.brakeUnderVolt;
sift 12:ae291fa7239c 66 ptr->brakeExceedVolt = errCounter.brakeExceedVolt;
sift 12:ae291fa7239c 67 ptr->brakeFuzzyVolt = errCounter.brakeFuzzyVolt;
sift 12:ae291fa7239c 68 ptr->brakeOverRide = errCounter.brakeOverRide;
sift 12:ae291fa7239c 69 }
sift 12:ae291fa7239c 70
sift 18:b7c362c8f0fd 71 //ブレーキONOFF判定関数
sift 12:ae291fa7239c 72 //Brake-ON:1 Brake-OFF:0
sift 12:ae291fa7239c 73 int isBrakeOn(void)
sift 12:ae291fa7239c 74 {
sift 12:ae291fa7239c 75 int brake = gBrake;
sift 12:ae291fa7239c 76 int brakeOnOff = 0;
sift 12:ae291fa7239c 77
sift 30:c596a0f5d685 78 if(brake < (BRK_ON_VOLTAGE + ERROR_TOLERANCE))
sift 12:ae291fa7239c 79 brakeOnOff = 1;
sift 30:c596a0f5d685 80 if(brake > (BRK_OFF_VOLTAGE - ERROR_TOLERANCE))
sift 12:ae291fa7239c 81 brakeOnOff = 0;
sift 12:ae291fa7239c 82
sift 12:ae291fa7239c 83 return brakeOnOff;
sift 2:9d69f27a3d3b 84 }
sift 1:4d86ec2fe4b1 85
sift 18:b7c362c8f0fd 86 //センサ現在値外部参照関数
sift 7:ad013d88a539 87 int getCurrentSensor(int sensor)
sift 2:9d69f27a3d3b 88 {
sift 2:9d69f27a3d3b 89 switch (sensor) {
sift 2:9d69f27a3d3b 90 case APS_PRIMARY:
sift 2:9d69f27a3d3b 91 return gApsP;
sift 2:9d69f27a3d3b 92 case APS_SECONDARY:
sift 2:9d69f27a3d3b 93 return gApsS;
sift 2:9d69f27a3d3b 94 case BRAKE:
sift 2:9d69f27a3d3b 95 return gBrake;
sift 2:9d69f27a3d3b 96 default:
sift 2:9d69f27a3d3b 97 return -1;
sift 1:4d86ec2fe4b1 98 }
sift 2:9d69f27a3d3b 99 }
sift 2:9d69f27a3d3b 100
sift 18:b7c362c8f0fd 101 //補正前センサ現在値外部参照関数
sift 7:ad013d88a539 102 int getRawSensor(int sensor)
sift 2:9d69f27a3d3b 103 {
sift 2:9d69f27a3d3b 104 switch (sensor) {
sift 2:9d69f27a3d3b 105 case APS_PRIMARY:
sift 2:9d69f27a3d3b 106 return rawApsP;
sift 2:9d69f27a3d3b 107 case APS_SECONDARY:
sift 2:9d69f27a3d3b 108 return rawApsS;
sift 2:9d69f27a3d3b 109 case BRAKE:
sift 2:9d69f27a3d3b 110 return rawBrake;
sift 2:9d69f27a3d3b 111 default:
sift 2:9d69f27a3d3b 112 return -1;
sift 1:4d86ec2fe4b1 113 }
sift 2:9d69f27a3d3b 114 }
sift 2:9d69f27a3d3b 115
sift 2:9d69f27a3d3b 116 bool loadSensorFlag = false;
sift 2:9d69f27a3d3b 117
sift 2:9d69f27a3d3b 118 //タイマー割り込みでコールされる
sift 2:9d69f27a3d3b 119 void loadSensorsISR(void)
sift 2:9d69f27a3d3b 120 {
sift 2:9d69f27a3d3b 121 loadSensorFlag = true;
sift 1:4d86ec2fe4b1 122 }
sift 1:4d86ec2fe4b1 123
sift 25:c21d35c7f0de 124 //センサ読み込み関数
sift 2:9d69f27a3d3b 125 void loadSensors(void)
sift 1:4d86ec2fe4b1 126 {
sift 2:9d69f27a3d3b 127 if(true == loadSensorFlag) {
sift 2:9d69f27a3d3b 128 loadSensorFlag = false;
sift 2:9d69f27a3d3b 129 static int preApsP=0, preApsS=0; //過去のセンサ値
sift 2:9d69f27a3d3b 130 static int preBrake=0;
sift 2:9d69f27a3d3b 131 int tmpApsP=0, tmpApsS=0, tmpBrake=0; //補正後のセンサ値
sift 2:9d69f27a3d3b 132 int tmpApsErrCountU=0, tmpApsErrCountE=0; //APSの一時的なエラーカウンタ
sift 2:9d69f27a3d3b 133
sift 2:9d69f27a3d3b 134 //Low Pass Filter
sift 2:9d69f27a3d3b 135 tmpApsP = (int)(apsP.read_u16()*ratioLPF + preApsP*(1.0f-ratioLPF));
sift 2:9d69f27a3d3b 136 tmpApsS = (int)(apsS.read_u16()*ratioLPF + preApsS*(1.0f-ratioLPF));
sift 2:9d69f27a3d3b 137 tmpBrake = (int)(brake.read_u16()*ratioLPF + preBrake*(1.0f-ratioLPF));
sift 2:9d69f27a3d3b 138
sift 2:9d69f27a3d3b 139 //生のセンサ値取得
sift 2:9d69f27a3d3b 140 rawApsP = tmpApsP;
sift 2:9d69f27a3d3b 141 rawApsS = tmpApsS;
sift 2:9d69f27a3d3b 142 rawBrake = tmpBrake;
sift 2:9d69f27a3d3b 143
sift 2:9d69f27a3d3b 144 //センサーチェック
sift 2:9d69f27a3d3b 145 //APS上限値チェック
sift 2:9d69f27a3d3b 146 if(tmpApsP > APS_MAX_POSITION + ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 147 tmpApsP = APS_MAX_POSITION; //異常時，上限値にクリップ
sift 2:9d69f27a3d3b 148 tmpApsErrCountE++;
sift 2:9d69f27a3d3b 149 }
sift 2:9d69f27a3d3b 150 if(tmpApsS > APS_MAX_POSITION + ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 151 tmpApsS = APS_MAX_POSITION; //異常時，上限値にクリップ
sift 2:9d69f27a3d3b 152 tmpApsErrCountE++;
sift 2:9d69f27a3d3b 153 }
sift 2:9d69f27a3d3b 154 if(0 == tmpApsErrCountE)
sift 2:9d69f27a3d3b 155 errCounter.apsExceedVolt = 0; //どちらも正常時エラーカウンタクリア
sift 2:9d69f27a3d3b 156 else
sift 2:9d69f27a3d3b 157 errCounter.apsExceedVolt += tmpApsErrCountE;
sift 2:9d69f27a3d3b 158
sift 2:9d69f27a3d3b 159 //APS下限値チェック
sift 2:9d69f27a3d3b 160 if(tmpApsP < APS_MIN_POSITION - ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 161 tmpApsP = APS_MIN_POSITION; //下限値にクリップ
sift 2:9d69f27a3d3b 162 tmpApsErrCountU++;
sift 2:9d69f27a3d3b 163 }
sift 2:9d69f27a3d3b 164 if(tmpApsS < APS_MIN_POSITION - ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 165 tmpApsS = APS_MIN_POSITION; //下限値にクリップ
sift 2:9d69f27a3d3b 166 tmpApsErrCountU++;
sift 2:9d69f27a3d3b 167 }
sift 2:9d69f27a3d3b 168 if(0 == tmpApsErrCountU)
sift 2:9d69f27a3d3b 169 errCounter.apsUnderVolt = 0; //どちらも正常時エラーカウンタクリア
sift 2:9d69f27a3d3b 170 else
sift 2:9d69f27a3d3b 171 errCounter.apsUnderVolt += tmpApsErrCountU;
sift 2:9d69f27a3d3b 172
sift 2:9d69f27a3d3b 173 //センサー偏差チェック
sift 2:9d69f27a3d3b 174 if(myAbs(tmpApsP - tmpApsS) > APS_DEVIATION_TOLERANCE) { //偏差チェックには補正後の値(tmp)を使用
sift 2:9d69f27a3d3b 175 errCounter.apsErrorTolerance++;
sift 2:9d69f27a3d3b 176 } else {
sift 2:9d69f27a3d3b 177 errCounter.apsErrorTolerance = 0;
sift 2:9d69f27a3d3b 178 }
sift 1:4d86ec2fe4b1 179
sift 2:9d69f27a3d3b 180 //小さい方にクリップ
sift 2:9d69f27a3d3b 181 //APS値は好きな方を使いな
sift 2:9d69f27a3d3b 182 if(tmpApsP > tmpApsS) {
sift 2:9d69f27a3d3b 183 tmpApsP = tmpApsS;
sift 2:9d69f27a3d3b 184 } else {
sift 2:9d69f27a3d3b 185 tmpApsS = tmpApsP;
sift 2:9d69f27a3d3b 186 }
sift 2:9d69f27a3d3b 187
sift 2:9d69f27a3d3b 188 //Brake上限値チェック
sift 30:c596a0f5d685 189 if(tmpBrake > BRK_OFF_VOLTAGE + ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 190 errCounter.brakeExceedVolt++;
sift 30:c596a0f5d685 191 tmpBrake = BRK_OFF_VOLTAGE;
sift 2:9d69f27a3d3b 192 } else {
sift 2:9d69f27a3d3b 193 errCounter.brakeExceedVolt = 0;
sift 2:9d69f27a3d3b 194 }
sift 2:9d69f27a3d3b 195
sift 2:9d69f27a3d3b 196 //Brake下限値チェック
sift 30:c596a0f5d685 197 if(tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 198 errCounter.brakeUnderVolt++;
sift 30:c596a0f5d685 199 tmpBrake = BRK_ON_VOLTAGE;
sift 2:9d69f27a3d3b 200 } else {
sift 2:9d69f27a3d3b 201 errCounter.brakeUnderVolt = 0;
sift 2:9d69f27a3d3b 202 }
sift 1:4d86ec2fe4b1 203
sift 2:9d69f27a3d3b 204 //brake範囲外電圧チェック
sift 30:c596a0f5d685 205 if((tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE)) {
sift 2:9d69f27a3d3b 206 errCounter.brakeFuzzyVolt++;
sift 2:9d69f27a3d3b 207 tmpBrake = BRK_OFF_VOLTAGE;
sift 2:9d69f27a3d3b 208 } else {
sift 2:9d69f27a3d3b 209 errCounter.brakeFuzzyVolt=0;
sift 2:9d69f27a3d3b 210 }
sift 2:9d69f27a3d3b 211
sift 2:9d69f27a3d3b 212 //APS固着チェック
sift 2:9d69f27a3d3b 213 if((preApsP == tmpApsP) && (tmpApsP == APS_MAX_POSITION))
sift 2:9d69f27a3d3b 214 errCounter.apsStick++;
sift 2:9d69f27a3d3b 215 else
sift 2:9d69f27a3d3b 216 errCounter.apsStick=0;
sift 2:9d69f27a3d3b 217
sift 2:9d69f27a3d3b 218 //ブレーキオーバーライドチェック
sift 12:ae291fa7239c 219 if((isBrakeOn() == 1) && (tmpApsP >= APS_OVERRIDE25)) //Brake-ON and APS > 25%
sift 2:9d69f27a3d3b 220 errCounter.brakeOverRide++;
sift 12:ae291fa7239c 221 if(tmpApsP < APS_OVERRIDE05) //Brake-ON and APS < 5%
sift 2:9d69f27a3d3b 222 errCounter.brakeOverRide=0;
sift 2:9d69f27a3d3b 223
sift 2:9d69f27a3d3b 224 //センサ値取得
sift 2:9d69f27a3d3b 225 gApsP = tmpApsP;
sift 2:9d69f27a3d3b 226 gApsS = tmpApsS;
sift 2:9d69f27a3d3b 227 gBrake = tmpBrake;
sift 2:9d69f27a3d3b 228
sift 2:9d69f27a3d3b 229 //未来の自分に期待
sift 2:9d69f27a3d3b 230 preApsP = rawApsP;
sift 2:9d69f27a3d3b 231 preApsS = rawApsS;
sift 2:9d69f27a3d3b 232 preBrake = rawBrake;
sift 2:9d69f27a3d3b 233 }
sift 1:4d86ec2fe4b1 234 }
sift 1:4d86ec2fe4b1 235
sift 25:c21d35c7f0de 236 volatile int gRightMotorPulseCounter = 0, gLeftMotorPulseCounter = 0;
sift 7:ad013d88a539 237 volatile bool pulseTimeISRFlag = false;
sift 1:4d86ec2fe4b1 238
sift 25:c21d35c7f0de 239 void countRightMotorPulseISR(void)
sift 1:4d86ec2fe4b1 240 {
sift 25:c21d35c7f0de 241 gRightMotorPulseCounter++;
sift 1:4d86ec2fe4b1 242 }
sift 1:4d86ec2fe4b1 243
sift 25:c21d35c7f0de 244 void countLeftMotorPulseISR(void)
sift 1:4d86ec2fe4b1 245 {
sift 25:c21d35c7f0de 246 gLeftMotorPulseCounter++;
sift 2:9d69f27a3d3b 247 }
sift 2:9d69f27a3d3b 248
sift 25:c21d35c7f0de 249 void getPulseCounterISR(void)
sift 2:9d69f27a3d3b 250 {
sift 7:ad013d88a539 251 pulseTimeISRFlag = true;
sift 2:9d69f27a3d3b 252 }
sift 2:9d69f27a3d3b 253
sift 28:47e9531a3a9d 254 //default argument : switchWheel=false
sift 28:47e9531a3a9d 255 int getRPS(Select rl, bool switchWheel)
sift 2:9d69f27a3d3b 256 {
sift 37:ba10cf09c151 257 static int rightMotorPulse[100]= {0}, leftMotorPulse[100]= {0}; //過去1.0秒間のパルス数格納
sift 25:c21d35c7f0de 258 static int sumRightMotorPulse, sumLeftMotorPulse;
sift 28:47e9531a3a9d 259 float rps;
sift 2:9d69f27a3d3b 260
sift 7:ad013d88a539 261 if(pulseTimeISRFlag == true) {
sift 37:ba10cf09c151 262 for(int i=99; i>0; i--) {
sift 25:c21d35c7f0de 263 rightMotorPulse[i] = rightMotorPulse[i-1];
sift 25:c21d35c7f0de 264 leftMotorPulse[i] = leftMotorPulse[i-1];
sift 25:c21d35c7f0de 265 }
sift 25:c21d35c7f0de 266
sift 25:c21d35c7f0de 267 rightMotorPulse[0] = gRightMotorPulseCounter;
sift 25:c21d35c7f0de 268 leftMotorPulse[0] = gLeftMotorPulseCounter;
sift 2:9d69f27a3d3b 269
sift 25:c21d35c7f0de 270 gRightMotorPulseCounter = 0;
sift 25:c21d35c7f0de 271 gLeftMotorPulseCounter = 0;
sift 10:87ad65eef0e9 272
sift 25:c21d35c7f0de 273 sumRightMotorPulse = 0;
sift 25:c21d35c7f0de 274 sumLeftMotorPulse = 0;
sift 25:c21d35c7f0de 275
sift 37:ba10cf09c151 276 for(int i=0; i<100; i++) {
sift 25:c21d35c7f0de 277 sumRightMotorPulse += rightMotorPulse[i];
sift 25:c21d35c7f0de 278 sumLeftMotorPulse += leftMotorPulse[i];
sift 25:c21d35c7f0de 279 }
sift 28:47e9531a3a9d 280 pulseTimeISRFlag = false;
sift 2:9d69f27a3d3b 281 }
sift 2:9d69f27a3d3b 282
sift 31:042c08a7434f 283 if(switchWheel == false) {
sift 28:47e9531a3a9d 284 if(rl == RIGHT)
sift 37:ba10cf09c151 285 rps = (float)1.0f*sumRightMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO; //過去0.5秒間のモータパルス数を使ってRPS算出
sift 28:47e9531a3a9d 286 else
sift 37:ba10cf09c151 287 rps = (float)1.0f*sumLeftMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO; //過去0.5秒間のモータパルス数を使ってRPS算出
sift 28:47e9531a3a9d 288 } else {
sift 28:47e9531a3a9d 289 //こっちはタイヤ回転数
sift 28:47e9531a3a9d 290 //そのうち対応
sift 28:47e9531a3a9d 291 if(rl == RIGHT)
sift 28:47e9531a3a9d 292 rps = (float)sumRightMotorPulse / MOTOR_PULSE_NUM; //過去1秒間のモータパルス数を使ってRPS算出
sift 28:47e9531a3a9d 293 else
sift 28:47e9531a3a9d 294 rps = (float)sumLeftMotorPulse / MOTOR_PULSE_NUM; //過去1秒間のモータパルス数を使ってRPS算出
sift 28:47e9531a3a9d 295 }
sift 28:47e9531a3a9d 296 return (int)(rps / LSB_MOTORSPEED); //LSB変換
sift 2:9d69f27a3d3b 297 }
sift 2:9d69f27a3d3b 298
sift 2:9d69f27a3d3b 299 float getVelocity(void)
sift 2:9d69f27a3d3b 300 {
sift 38:11753ee9734f 301 // static float debugVelocity = 0.0f;
sift 38:11753ee9734f 302 // debugVelocity += 0.002;
sift 38:11753ee9734f 303 //
sift 38:11753ee9734f 304 // printf("%1.2f\r\n", debugVelocity);
sift 38:11753ee9734f 305 //
sift 38:11753ee9734f 306 // return debugVelocity;
sift 32:9688c30ac38b 307 return (0.5f*TIRE_DIAMETER*2*M_PI*(getRPS(RIGHT) + getRPS(LEFT))*0.5f)*LSB_MOTORSPEED;
sift 38:11753ee9734f 308 // return 15.0f;
sift 1:4d86ec2fe4b1 309 }
sift 1:4d86ec2fe4b1 310
sift 34:594ddb4008b2 311 int distributeTorque_omega(float steeringWheelAngle)
sift 21:bbf2ad7e6602 312 {
sift 21:bbf2ad7e6602 313 static float lastSteering=0.0f;
sift 22:95c1f753ecad 314 float omega=0;
sift 22:95c1f753ecad 315 int disTrq=0;
sift 30:c596a0f5d685 316
sift 34:594ddb4008b2 317 steeringWheelAngle = ratioLPF * steeringWheelAngle + (1.0f - ratioLPF) * lastSteering;
sift 21:bbf2ad7e6602 318
sift 34:594ddb4008b2 319 omega = steeringWheelAngle - lastSteering; //舵角の差分算出
sift 21:bbf2ad7e6602 320 omega /= 0.01f; //制御周期で角速度演算
sift 36:dc33a3a194c9 321
sift 22:95c1f753ecad 322 if(myAbs(omega) < 0.349f) { //20deg/s
sift 21:bbf2ad7e6602 323 disTrq = 0;
sift 22:95c1f753ecad 324 } else if(myAbs(omega) <= 8.727f) { //500deg/s
sift 34:594ddb4008b2 325 disTrq = (int)(MAX_DISTRIBUTION_TORQUE_OMEGA / (8.727f-0.349f) * (myAbs(omega) - 0.349f));
sift 21:bbf2ad7e6602 326 } else
sift 34:594ddb4008b2 327 disTrq = (int)MAX_DISTRIBUTION_TORQUE_OMEGA;
sift 22:95c1f753ecad 328
sift 34:594ddb4008b2 329 lastSteering = steeringWheelAngle;
sift 21:bbf2ad7e6602 330
sift 34:594ddb4008b2 331 if(omega < 0)
sift 22:95c1f753ecad 332 disTrq = -disTrq;
sift 22:95c1f753ecad 333
sift 21:bbf2ad7e6602 334 return disTrq;
sift 21:bbf2ad7e6602 335 }
sift 21:bbf2ad7e6602 336
sift 31:042c08a7434f 337 int distributeTorque(float steeringWheelAngle, float velocity)
sift 2:9d69f27a3d3b 338 {
sift 31:042c08a7434f 339 double V2 = velocity * velocity;
sift 31:042c08a7434f 340 double R = 0.0; //旋回半径
sift 31:042c08a7434f 341 double Gy = 0.0; //横G
sift 31:042c08a7434f 342 double deadband = 0.0;
sift 32:9688c30ac38b 343 double steeringAngle = (double)steeringWheelAngle * STEER_RATIO;
sift 31:042c08a7434f 344 double steeringSign = 1.0;
sift 9:220e4e77e056 345 int disTrq = 0;
sift 25:c21d35c7f0de 346
sift 31:042c08a7434f 347 if(steeringAngle > 0)
sift 34:594ddb4008b2 348 steeringSign = 1.0;
sift 25:c21d35c7f0de 349 else
sift 34:594ddb4008b2 350 steeringSign = -1.0;
sift 25:c21d35c7f0de 351
sift 31:042c08a7434f 352 steeringAngle = myAbs(steeringAngle);
sift 32:9688c30ac38b 353
sift 38:11753ee9734f 354 if(steeringAngle <= 0.0001)
sift 31:042c08a7434f 355 steeringAngle = 0.0001;
sift 2:9d69f27a3d3b 356
sift 31:042c08a7434f 357 R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle; //理論旋回半径 計算
sift 32:9688c30ac38b 358 Gy = V2 / R / 9.81; //理論横G
sift 31:042c08a7434f 359
sift 34:594ddb4008b2 360 if(Gy <= deadband)
sift 9:220e4e77e056 361 disTrq = 0;
sift 34:594ddb4008b2 362 else if(Gy <= 1.5) {
sift 31:042c08a7434f 363 Gy -= deadband;
sift 34:594ddb4008b2 364 disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (1.5 - deadband) * Gy);
sift 25:c21d35c7f0de 365 } else {
sift 19:571a4d00b89c 366 disTrq = MAX_DISTRIBUTION_TORQUE;
sift 25:c21d35c7f0de 367 }
sift 2:9d69f27a3d3b 368
sift 36:dc33a3a194c9 369 return (int)(disTrq * steeringSign);
sift 2:9d69f27a3d3b 370 }
sift 2:9d69f27a3d3b 371
sift 25:c21d35c7f0de 372 //rpm +++++ モータ回転数
sift 25:c21d35c7f0de 373 //regSwitch +++++ 回生=1 力行=0
sift 25:c21d35c7f0de 374 inline int calcMaxTorque(int rpm, bool regSwitch)
sift 2:9d69f27a3d3b 375 {
sift 25:c21d35c7f0de 376 int maxTrq=0;
sift 25:c21d35c7f0de 377
sift 25:c21d35c7f0de 378 //後で削除
sift 25:c21d35c7f0de 379 rpm = 2000;
sift 26:331e77bb479b 380 //++++++++++++++++++++
sift 25:c21d35c7f0de 381
sift 25:c21d35c7f0de 382 if(regSwitch == 0) {
sift 25:c21d35c7f0de 383 if(rpm <3000)
sift 25:c21d35c7f0de 384 maxTrq = MAX_MOTOR_TORQUE_POWER;
sift 25:c21d35c7f0de 385 else if(rpm <= 11000)
sift 25:c21d35c7f0de 386 maxTrq = maxTorqueMap[(int)(rpm / 10.0)];
sift 25:c21d35c7f0de 387 else
sift 25:c21d35c7f0de 388 maxTrq = MAX_REVOLUTION_TORQUE_POWER;
sift 25:c21d35c7f0de 389 } else {
sift 28:47e9531a3a9d 390 if(rpm < 600) {
sift 25:c21d35c7f0de 391 maxTrq = 0;
sift 28:47e9531a3a9d 392 } else if(rpm < 1250) {
sift 28:47e9531a3a9d 393 //+++++++++++++++++++++++++++++++++++++
sift 28:47e9531a3a9d 394 //暫定処理 今後回生トルクマップも要作成
sift 28:47e9531a3a9d 395 maxTrq = 0;
sift 28:47e9531a3a9d 396 //+++++++++++++++++++++++++++++++++++++
sift 25:c21d35c7f0de 397 } else if(rpm <= 6000) {
sift 26:331e77bb479b 398 maxTrq = MAX_MOTOR_TORQUE_REGENERATIVE;
sift 26:331e77bb479b 399 } else if(rpm <= 11000) {
sift 25:c21d35c7f0de 400 //+++++++++++++++++++++++++++++++++++++
sift 25:c21d35c7f0de 401 //暫定処理 今後回生トルクマップも要作成
sift 26:331e77bb479b 402 maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE;
sift 25:c21d35c7f0de 403 //+++++++++++++++++++++++++++++++++++++
sift 25:c21d35c7f0de 404 } else {
sift 25:c21d35c7f0de 405 maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE;
sift 25:c21d35c7f0de 406 }
sift 25:c21d35c7f0de 407 }
sift 25:c21d35c7f0de 408 return maxTrq;
sift 2:9d69f27a3d3b 409 }
sift 2:9d69f27a3d3b 410
sift 25:c21d35c7f0de 411 //演算方法
sift 25:c21d35c7f0de 412 //y = a(x - b) + c
sift 25:c21d35c7f0de 413 //x = 1/a * (y + ab - c)
sift 25:c21d35c7f0de 414 unsigned int calcTorqueToVoltage(int reqTorque, int rpm)
sift 2:9d69f27a3d3b 415 {
sift 25:c21d35c7f0de 416 float slope = 0; //a
sift 25:c21d35c7f0de 417 int startPoint = 0; //b
sift 25:c21d35c7f0de 418 int intercept = 0; //c
sift 12:ae291fa7239c 419
sift 25:c21d35c7f0de 420 int outputVoltage=0;
sift 16:7afd623ef48a 421
sift 25:c21d35c7f0de 422 if(reqTorque > LINEAR_REGION_TORQUE_POWER) { //力行トルクがrpmに対して非線形となる領域
sift 25:c21d35c7f0de 423 slope = (float)(calcMaxTorque(rpm, 0) - LINEAR_REGION_TORQUE_POWER)/(DACOUTPUT_MAX - LINEAR_REGION_VOLTAGE_POWER);
sift 25:c21d35c7f0de 424 startPoint = LINEAR_REGION_VOLTAGE_POWER;
sift 25:c21d35c7f0de 425 intercept = LINEAR_REGION_TORQUE_POWER;
sift 25:c21d35c7f0de 426
sift 25:c21d35c7f0de 427 outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
sift 6:26fa8c78500e 428
sift 25:c21d35c7f0de 429 } else if(reqTorque > 0) { //力行トルクがrpmに対して線形となる領域
sift 25:c21d35c7f0de 430 slope = (float)LINEAR_REGION_TORQUE_POWER/(LINEAR_REGION_VOLTAGE_POWER - ZERO_TORQUE_VOLTAGE_P);
sift 25:c21d35c7f0de 431 startPoint = ZERO_TORQUE_VOLTAGE_P;
sift 25:c21d35c7f0de 432 intercept = 0;
sift 25:c21d35c7f0de 433
sift 25:c21d35c7f0de 434 outputVoltage = (int)(reqTorque/slope + startPoint);
sift 25:c21d35c7f0de 435
sift 25:c21d35c7f0de 436 } else if(0 == reqTorque) {
sift 25:c21d35c7f0de 437
sift 25:c21d35c7f0de 438 outputVoltage = ZERO_TORQUE_VOLTAGE_NEUTRAL; //ニュートラル信号
sift 6:26fa8c78500e 439
sift 25:c21d35c7f0de 440 } else if(reqTorque > LINEAR_REGION_TORQUE_REGENERATIVE) { //回生トルクがrpmに対して線形となる領域
sift 25:c21d35c7f0de 441 slope = (float)(0 - LINEAR_REGION_TORQUE_REGENERATIVE)/(ZERO_TORQUE_VOLTAGE_REG - LINEAR_REGION_VOLTAGE_REGENERATIVE);
sift 25:c21d35c7f0de 442 startPoint = LINEAR_REGION_VOLTAGE_REGENERATIVE;
sift 25:c21d35c7f0de 443 intercept = LINEAR_REGION_TORQUE_REGENERATIVE;
sift 25:c21d35c7f0de 444
sift 25:c21d35c7f0de 445 outputVoltage = (int)(reqTorque/slope + startPoint);
sift 25:c21d35c7f0de 446
sift 25:c21d35c7f0de 447 } else { //回生トルクがrpmに対して非線形となる領域
sift 25:c21d35c7f0de 448 slope = (float)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/(LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN);
sift 25:c21d35c7f0de 449 startPoint = DACOUTPUT_MIN;
sift 25:c21d35c7f0de 450 intercept = calcMaxTorque(rpm, 1);
sift 25:c21d35c7f0de 451
sift 25:c21d35c7f0de 452 outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
sift 2:9d69f27a3d3b 453 }
sift 2:9d69f27a3d3b 454
sift 25:c21d35c7f0de 455 if(outputVoltage > DACOUTPUT_MAX)
sift 25:c21d35c7f0de 456 outputVoltage = DACOUTPUT_MAX;
sift 25:c21d35c7f0de 457 if(outputVoltage < DACOUTPUT_MIN)
sift 25:c21d35c7f0de 458 outputVoltage = DACOUTPUT_MIN;
sift 16:7afd623ef48a 459
sift 20:3c5061281a7a 460 return (unsigned int)(0xFFF*((double)outputVoltage/0xFFFF)); //DACの分解能に適応(16bit->12bit)
sift 2:9d69f27a3d3b 461 }
sift 2:9d69f27a3d3b 462
sift 2:9d69f27a3d3b 463 int calcRequestTorque(void)
sift 2:9d69f27a3d3b 464 {
sift 2:9d69f27a3d3b 465 int currentAPS;
sift 2:9d69f27a3d3b 466 int requestTorque;
sift 2:9d69f27a3d3b 467
sift 2:9d69f27a3d3b 468 currentAPS = ((gApsP>gApsS) ? gApsS : gApsP); //センサ値は小さい方を採用
sift 12:ae291fa7239c 469
sift 2:9d69f27a3d3b 470 if(currentAPS < APS_MIN_POSITION)
sift 2:9d69f27a3d3b 471 currentAPS = 0;
sift 2:9d69f27a3d3b 472 else
sift 2:9d69f27a3d3b 473 currentAPS -= APS_MIN_POSITION; //オフセット修正
sift 2:9d69f27a3d3b 474
sift 25:c21d35c7f0de 475 if(currentAPS <= APS_REG_RANGE) //デッドバンド内であれば要求トルク->0
sift 25:c21d35c7f0de 476 requestTorque = (int)((float)(-MAX_OUTPUT_TORQUE_REGENERATIVE) / APS_REG_RANGE * currentAPS + MAX_OUTPUT_TORQUE_REGENERATIVE);
sift 2:9d69f27a3d3b 477 else
sift 25:c21d35c7f0de 478 requestTorque = (int)((float)MAX_OUTPUT_TORQUE_POWER / APS_PWR_RANGE * (currentAPS - APS_REG_RANGE));
sift 2:9d69f27a3d3b 479
sift 25:c21d35c7f0de 480 if(requestTorque > MAX_OUTPUT_TORQUE_POWER)
sift 25:c21d35c7f0de 481 requestTorque = MAX_OUTPUT_TORQUE_POWER;
sift 25:c21d35c7f0de 482 else if(requestTorque < MAX_OUTPUT_TORQUE_REGENERATIVE)
sift 25:c21d35c7f0de 483 requestTorque = MAX_OUTPUT_TORQUE_REGENERATIVE;
sift 2:9d69f27a3d3b 484
sift 12:ae291fa7239c 485 if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1))
sift 12:ae291fa7239c 486 requestTorque = 0;
sift 12:ae291fa7239c 487
sift 2:9d69f27a3d3b 488 return requestTorque;
sift 2:9d69f27a3d3b 489 }
sift 2:9d69f27a3d3b 490
sift 17:a2246ce3333f 491 //トルク配分車速制限関数
sift 17:a2246ce3333f 492 //車速が低速域の場合，トルク配分0
sift 17:a2246ce3333f 493 float limitTorqueDistribution(void)
sift 17:a2246ce3333f 494 {
sift 17:a2246ce3333f 495 float limitRate;
sift 17:a2246ce3333f 496 float currentVelocity = getVelocity() * 3.6f; //km/hで車速取得
sift 20:3c5061281a7a 497
sift 37:ba10cf09c151 498 if(currentVelocity < 15.0f)
sift 17:a2246ce3333f 499 limitRate = 0.0f;
sift 36:dc33a3a194c9 500 else if(currentVelocity < 30.0f)
sift 38:11753ee9734f 501 limitRate = (currentVelocity - 15.0f) / (30.0f - 15.0f);
sift 17:a2246ce3333f 502 else
sift 17:a2246ce3333f 503 limitRate = 1.0f;
sift 36:dc33a3a194c9 504
sift 35:b75595b1da36 505 //printf("rate:%1.3f\r\n", limitRate);
sift 36:dc33a3a194c9 506
sift 17:a2246ce3333f 507 return limitRate;
sift 17:a2246ce3333f 508 }
sift 17:a2246ce3333f 509
sift 26:331e77bb479b 510 void driveTVD(int TVDmode, bool isRedyToDrive)
sift 1:4d86ec2fe4b1 511 {
sift 6:26fa8c78500e 512 int requestTorque=0; //ドライバー要求トルク
sift 6:26fa8c78500e 513 int distributionTrq=0; //分配トルク
sift 21:bbf2ad7e6602 514 int disTrq_omega=0;
sift 21:bbf2ad7e6602 515 int torqueRight, torqueLeft; //トルクの右左
sift 20:3c5061281a7a 516 static unsigned int preMcpA=0, preMcpB=0;
sift 20:3c5061281a7a 517
sift 2:9d69f27a3d3b 518 loadSensors(); //APS,BRAKE更新
sift 2:9d69f27a3d3b 519 loadSteerAngle(); //舵角更新
sift 12:ae291fa7239c 520
sift 26:331e77bb479b 521 if(isRedyToDrive && isBrakeOn())
sift 12:ae291fa7239c 522 readyToDriveFlag = 0;
sift 2:9d69f27a3d3b 523
sift 12:ae291fa7239c 524 if((errCounter.apsUnderVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 525 || (errCounter.apsExceedVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 526 || (errCounter.apsErrorTolerance > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 527 // || (errCounter.apsStick > ERRCOUNTER_DECISION)
sift 30:c596a0f5d685 528 || (errCounter.brakeUnderVolt > ERRCOUNTER_DECISION)
sift 30:c596a0f5d685 529 || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION)
sift 30:c596a0f5d685 530 || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 531 ) {
sift 16:7afd623ef48a 532 readyToDriveFlag = 1;
sift 12:ae291fa7239c 533 }
sift 16:7afd623ef48a 534
sift 12:ae291fa7239c 535 indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
sift 20:3c5061281a7a 536 LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);
sift 16:7afd623ef48a 537
sift 6:26fa8c78500e 538 requestTorque=calcRequestTorque(); //ドライバー要求トルク取得
sift 4:d7778cde0aff 539
sift 37:ba10cf09c151 540 distributionTrq = (int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity())*limitTorqueDistribution()) / 2.0f); //片モーターのトルク分配量計算
sift 38:11753ee9734f 541 disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f)*limitTorqueDistribution()) / 2.0f); //微分制御
sift 12:ae291fa7239c 542
sift 34:594ddb4008b2 543 //printf("%d\r\n", distributionTrq);
sift 34:594ddb4008b2 544
sift 38:11753ee9734f 545 // distributionTrq = 0;
sift 35:b75595b1da36 546 disTrq_omega = 0;
sift 31:042c08a7434f 547
sift 25:c21d35c7f0de 548 torqueRight = requestTorque + distributionTrq;
sift 25:c21d35c7f0de 549 torqueLeft = requestTorque - distributionTrq;
sift 21:bbf2ad7e6602 550
sift 22:95c1f753ecad 551 torqueRight += disTrq_omega;
sift 22:95c1f753ecad 552 torqueLeft -= disTrq_omega;
sift 20:3c5061281a7a 553
sift 38:11753ee9734f 554 if(torqueRight < 0) {
sift 38:11753ee9734f 555 if((getRPS(RIGHT) * LSB_MOTORSPEED * 60.0f) < 600.0f) {
sift 38:11753ee9734f 556 torqueLeft = requestTorque + torqueRight;
sift 38:11753ee9734f 557 torqueRight = 0;
sift 38:11753ee9734f 558 } else if((getRPS(RIGHT) * LSB_MOTORSPEED * 60.0f) <= 1250.0f) {
sift 38:11753ee9734f 559 torqueLeft = requestTorque + torqueRight*((getRPS(RIGHT) * GEAR_RATIO * LSB_MOTORSPEED * 60.0f - 600.0f)/(1250.0f - 600.0f));
sift 38:11753ee9734f 560 torqueRight = torqueRight*((getRPS(RIGHT)-600.0f)/(1250.0f - 600.0f));
sift 38:11753ee9734f 561 }
sift 38:11753ee9734f 562 }
sift 38:11753ee9734f 563 if(torqueLeft < 0) {
sift 38:11753ee9734f 564 if((getRPS(LEFT) * LSB_MOTORSPEED * 60.0f) < 600.0f) {
sift 38:11753ee9734f 565 torqueRight = requestTorque + torqueLeft;
sift 38:11753ee9734f 566 torqueLeft = 0;
sift 38:11753ee9734f 567 } else if((getRPS(LEFT) * LSB_MOTORSPEED * 60.0f) <= 1250.0f) {
sift 38:11753ee9734f 568 torqueRight = requestTorque + torqueLeft*((getRPS(LEFT) * GEAR_RATIO * LSB_MOTORSPEED * 60.0f - 600.0f)/(1250.0f - 600.0f));
sift 38:11753ee9734f 569 torqueLeft = torqueLeft*((getRPS(LEFT)-600.0f)/(1250.0f - 600.0f));
sift 38:11753ee9734f 570 }
sift 38:11753ee9734f 571 }
sift 38:11753ee9734f 572
sift 26:331e77bb479b 573 //アクセルべた踏みでトルクMAX、旋回より駆動を優先(加速番長モード)
sift 26:331e77bb479b 574 if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
sift 26:331e77bb479b 575 torqueLeft = MAX_OUTPUT_TORQUE_POWER;
sift 26:331e77bb479b 576 torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
sift 16:7afd623ef48a 577 }
sift 26:331e77bb479b 578 if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
sift 26:331e77bb479b 579 torqueRight = MAX_OUTPUT_TORQUE_POWER;
sift 26:331e77bb479b 580 torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
sift 26:331e77bb479b 581 }
sift 34:594ddb4008b2 582
sift 34:594ddb4008b2 583 gRightMotorTorque = torqueRight;
sift 34:594ddb4008b2 584 gLeftMotorTorque = torqueLeft;
sift 34:594ddb4008b2 585
sift 28:47e9531a3a9d 586 McpData.valA = calcTorqueToVoltage(torqueRight, getRPS(RIGHT));
sift 28:47e9531a3a9d 587 McpData.valB = calcTorqueToVoltage(torqueLeft, getRPS(LEFT));
sift 16:7afd623ef48a 588
sift 38:11753ee9734f 589 // preMcpA = (unsigned int)(McpData.valA * 0.456 + preMcpA * 0.544);
sift 38:11753ee9734f 590 // preMcpB = (unsigned int)(McpData.valB * 0.456 + preMcpB * 0.544);
sift 38:11753ee9734f 591 preMcpA = McpData.valA;
sift 38:11753ee9734f 592 preMcpB = McpData.valB;
sift 20:3c5061281a7a 593
sift 20:3c5061281a7a 594 mcp.writeA(preMcpA); //右モーター
sift 20:3c5061281a7a 595 mcp.writeB(preMcpB); //左モーター
sift 1:4d86ec2fe4b1 596 }
sift 1:4d86ec2fe4b1 597
sift 1:4d86ec2fe4b1 598 void initTVD(void)
sift 1:4d86ec2fe4b1 599 {
sift 2:9d69f27a3d3b 600 RightPulseTimer.reset();
sift 2:9d69f27a3d3b 601 LeftPulseTimer.reset();
sift 2:9d69f27a3d3b 602 RightPulseTimer.start();
sift 2:9d69f27a3d3b 603 LeftPulseTimer.start();
sift 1:4d86ec2fe4b1 604
sift 25:c21d35c7f0de 605 rightMotorPulse.fall(&countRightMotorPulseISR);
sift 25:c21d35c7f0de 606 leftMotorPulse.fall(&countLeftMotorPulseISR);
sift 25:c21d35c7f0de 607
sift 25:c21d35c7f0de 608 ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S); //制御周期毎にデータ読み込み(LPF演算のため)
sift 25:c21d35c7f0de 609 ticker2.attach(&getPulseCounterISR, CONTROL_CYCLE_S); //
sift 10:87ad65eef0e9 610
sift 10:87ad65eef0e9 611 mcp.writeA(0); //右モーター
sift 10:87ad65eef0e9 612 mcp.writeB(0); //左モーター
sift 28:47e9531a3a9d 613
sift 30:c596a0f5d685 614 printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_POWER);
sift 30:c596a0f5d685 615 printf("MAX OUTPUT REG-TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_REGENERATIVE);
sift 30:c596a0f5d685 616 printf("MAX DISTRIBUTION TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_DISTRIBUTION_TORQUE);
sift 30:c596a0f5d685 617 printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MIN_INNERWHEEL_MOTOR_TORQUE);
sift 1:4d86ec2fe4b1 618 }