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@43:5da6b1574227, 2017-11-02 (annotated)

Committer:
sift
Date:
Thu Nov 02 01:56:46 2017 +0000
Revision:
43:5da6b1574227
Parent:
42:3ab09d0e3071
Child:
44:d433bb5f77c0
???????

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