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@18:b7c362c8f0fd, 2016-08-10 (annotated)

Committer:
sift
Date:
Wed Aug 10 11:21:47 2016 +0000
Revision:
18:b7c362c8f0fd
Parent:
17:a2246ce3333f
Child:
19:571a4d00b89c
????????????????????????; ???????????

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 1:4d86ec2fe4b1 9 extern InterruptIn rightMotorPulse;
sift 1:4d86ec2fe4b1 10 extern InterruptIn leftMotorPulse;
sift 1:4d86ec2fe4b1 11 extern DigitalOut MotorPulse[];
sift 1:4d86ec2fe4b1 12 extern MCP4922 mcp;
sift 8:a22aec357a64 13 extern Serial pc;
sift 1:4d86ec2fe4b1 14
sift 2:9d69f27a3d3b 15 Timer RightPulseTimer;
sift 2:9d69f27a3d3b 16 Timer LeftPulseTimer;
sift 2:9d69f27a3d3b 17 Ticker ticker1;
sift 2:9d69f27a3d3b 18 Ticker ticker2;
sift 2:9d69f27a3d3b 19 Ticker ticker3;
sift 1:4d86ec2fe4b1 20
sift 18:b7c362c8f0fd 21 #define myAbs(x) ((x>0)?(x):(-(x)))
sift 18:b7c362c8f0fd 22
sift 2:9d69f27a3d3b 23 #define apsPVol() (apsP.read() * 3.3)
sift 2:9d69f27a3d3b 24 #define apsSVol() (apsS.read() * 3.3)
sift 0:276c1dab2d62 25
sift 0:276c1dab2d62 26 struct {
sift 0:276c1dab2d62 27 unsigned int valA:12;
sift 0:276c1dab2d62 28 unsigned int valB:12;
sift 2:9d69f27a3d3b 29 } McpData;
sift 1:4d86ec2fe4b1 30
sift 2:9d69f27a3d3b 31 //各変数が一定値を超えた時点でエラー検出とする
sift 2:9d69f27a3d3b 32 //2つのAPSの区別はつけないことにする
sift 12:ae291fa7239c 33 struct errCounter_t errCounter= {0,0,0,0,0,0,0};
sift 1:4d86ec2fe4b1 34
sift 12:ae291fa7239c 35 int readyToDriveFlag = 1;
sift 12:ae291fa7239c 36
sift 12:ae291fa7239c 37 int gApsP=0, gApsS=0, gBrake=0; //現在のセンサ値
sift 12:ae291fa7239c 38 int rawApsP=0, rawApsS=0, rawBrake=0; //現在の補正無しのセンサ値
sift 2:9d69f27a3d3b 39
sift 18:b7c362c8f0fd 40 //エラーカウンタ外部参照用関数
sift 18:b7c362c8f0fd 41 //errCounter_t型変数のポインタを引数に取る
sift 2:9d69f27a3d3b 42 void getCurrentErrCount(struct errCounter_t *ptr)
sift 1:4d86ec2fe4b1 43 {
sift 12:ae291fa7239c 44 ptr->apsUnderVolt = errCounter.apsUnderVolt;
sift 12:ae291fa7239c 45 ptr->apsExceedVolt = errCounter.apsExceedVolt;
sift 12:ae291fa7239c 46 ptr->apsErrorTolerance = errCounter.apsErrorTolerance;
sift 12:ae291fa7239c 47 ptr->apsStick = errCounter.apsStick;
sift 12:ae291fa7239c 48 ptr->brakeUnderVolt = errCounter.brakeUnderVolt;
sift 12:ae291fa7239c 49 ptr->brakeExceedVolt = errCounter.brakeExceedVolt;
sift 12:ae291fa7239c 50 ptr->brakeFuzzyVolt = errCounter.brakeFuzzyVolt;
sift 12:ae291fa7239c 51 ptr->brakeOverRide = errCounter.brakeOverRide;
sift 12:ae291fa7239c 52 }
sift 12:ae291fa7239c 53
sift 18:b7c362c8f0fd 54 //ブレーキONOFF判定関数
sift 12:ae291fa7239c 55 //Brake-ON:1 Brake-OFF:0
sift 12:ae291fa7239c 56 int isBrakeOn(void)
sift 12:ae291fa7239c 57 {
sift 12:ae291fa7239c 58 int brake = gBrake;
sift 12:ae291fa7239c 59 int brakeOnOff = 0;
sift 12:ae291fa7239c 60
sift 12:ae291fa7239c 61 if(brake > (BRK_ON_VOLTAGE - ERROR_TOLERANCE))
sift 12:ae291fa7239c 62 brakeOnOff = 1;
sift 12:ae291fa7239c 63 if(brake < (BRK_OFF_VOLTAGE + ERROR_TOLERANCE))
sift 12:ae291fa7239c 64 brakeOnOff = 0;
sift 12:ae291fa7239c 65
sift 12:ae291fa7239c 66 return brakeOnOff;
sift 2:9d69f27a3d3b 67 }
sift 1:4d86ec2fe4b1 68
sift 18:b7c362c8f0fd 69 //センサ現在値外部参照関数
sift 7:ad013d88a539 70 int getCurrentSensor(int sensor)
sift 2:9d69f27a3d3b 71 {
sift 2:9d69f27a3d3b 72 switch (sensor) {
sift 2:9d69f27a3d3b 73 case APS_PRIMARY:
sift 2:9d69f27a3d3b 74 return gApsP;
sift 2:9d69f27a3d3b 75 case APS_SECONDARY:
sift 2:9d69f27a3d3b 76 return gApsS;
sift 2:9d69f27a3d3b 77 case BRAKE:
sift 2:9d69f27a3d3b 78 return gBrake;
sift 2:9d69f27a3d3b 79 default:
sift 2:9d69f27a3d3b 80 return -1;
sift 1:4d86ec2fe4b1 81 }
sift 2:9d69f27a3d3b 82 }
sift 2:9d69f27a3d3b 83
sift 18:b7c362c8f0fd 84 //補正前センサ現在値外部参照関数
sift 7:ad013d88a539 85 int getRawSensor(int sensor)
sift 2:9d69f27a3d3b 86 {
sift 2:9d69f27a3d3b 87 switch (sensor) {
sift 2:9d69f27a3d3b 88 case APS_PRIMARY:
sift 2:9d69f27a3d3b 89 return rawApsP;
sift 2:9d69f27a3d3b 90 case APS_SECONDARY:
sift 2:9d69f27a3d3b 91 return rawApsS;
sift 2:9d69f27a3d3b 92 case BRAKE:
sift 2:9d69f27a3d3b 93 return rawBrake;
sift 2:9d69f27a3d3b 94 default:
sift 2:9d69f27a3d3b 95 return -1;
sift 1:4d86ec2fe4b1 96 }
sift 2:9d69f27a3d3b 97 }
sift 2:9d69f27a3d3b 98
sift 2:9d69f27a3d3b 99 bool loadSensorFlag = false;
sift 2:9d69f27a3d3b 100
sift 2:9d69f27a3d3b 101 //タイマー割り込みでコールされる
sift 2:9d69f27a3d3b 102 void loadSensorsISR(void)
sift 2:9d69f27a3d3b 103 {
sift 2:9d69f27a3d3b 104 loadSensorFlag = true;
sift 1:4d86ec2fe4b1 105 }
sift 1:4d86ec2fe4b1 106
sift 2:9d69f27a3d3b 107 //関数内処理時間より短い時間のタイマーのセットは禁止
sift 2:9d69f27a3d3b 108 void loadSensors(void)
sift 1:4d86ec2fe4b1 109 {
sift 2:9d69f27a3d3b 110 if(true == loadSensorFlag) {
sift 2:9d69f27a3d3b 111 loadSensorFlag = false;
sift 2:9d69f27a3d3b 112 static int preApsP=0, preApsS=0; //過去のセンサ値
sift 2:9d69f27a3d3b 113 static int preBrake=0;
sift 2:9d69f27a3d3b 114 int tmpApsP=0, tmpApsS=0, tmpBrake=0; //補正後のセンサ値
sift 2:9d69f27a3d3b 115 int tmpApsErrCountU=0, tmpApsErrCountE=0; //APSの一時的なエラーカウンタ
sift 2:9d69f27a3d3b 116
sift 2:9d69f27a3d3b 117 //Low Pass Filter
sift 2:9d69f27a3d3b 118 tmpApsP = (int)(apsP.read_u16()*ratioLPF + preApsP*(1.0f-ratioLPF));
sift 2:9d69f27a3d3b 119 tmpApsS = (int)(apsS.read_u16()*ratioLPF + preApsS*(1.0f-ratioLPF));
sift 2:9d69f27a3d3b 120 tmpBrake = (int)(brake.read_u16()*ratioLPF + preBrake*(1.0f-ratioLPF));
sift 2:9d69f27a3d3b 121
sift 2:9d69f27a3d3b 122 //生のセンサ値取得
sift 2:9d69f27a3d3b 123 rawApsP = tmpApsP;
sift 2:9d69f27a3d3b 124 rawApsS = tmpApsS;
sift 2:9d69f27a3d3b 125 rawBrake = tmpBrake;
sift 2:9d69f27a3d3b 126
sift 2:9d69f27a3d3b 127 //センサーチェック
sift 2:9d69f27a3d3b 128 //APS上限値チェック
sift 2:9d69f27a3d3b 129 if(tmpApsP > APS_MAX_POSITION + ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 130 tmpApsP = APS_MAX_POSITION; //異常時，上限値にクリップ
sift 2:9d69f27a3d3b 131 tmpApsErrCountE++;
sift 2:9d69f27a3d3b 132 }
sift 2:9d69f27a3d3b 133 if(tmpApsS > APS_MAX_POSITION + ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 134 tmpApsS = APS_MAX_POSITION; //異常時，上限値にクリップ
sift 2:9d69f27a3d3b 135 tmpApsErrCountE++;
sift 2:9d69f27a3d3b 136 }
sift 2:9d69f27a3d3b 137 if(0 == tmpApsErrCountE)
sift 2:9d69f27a3d3b 138 errCounter.apsExceedVolt = 0; //どちらも正常時エラーカウンタクリア
sift 2:9d69f27a3d3b 139 else
sift 2:9d69f27a3d3b 140 errCounter.apsExceedVolt += tmpApsErrCountE;
sift 2:9d69f27a3d3b 141
sift 2:9d69f27a3d3b 142 //APS下限値チェック
sift 2:9d69f27a3d3b 143 if(tmpApsP < APS_MIN_POSITION - ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 144 tmpApsP = APS_MIN_POSITION; //下限値にクリップ
sift 2:9d69f27a3d3b 145 tmpApsErrCountU++;
sift 2:9d69f27a3d3b 146 }
sift 2:9d69f27a3d3b 147 if(tmpApsS < APS_MIN_POSITION - ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 148 tmpApsS = APS_MIN_POSITION; //下限値にクリップ
sift 2:9d69f27a3d3b 149 tmpApsErrCountU++;
sift 2:9d69f27a3d3b 150 }
sift 2:9d69f27a3d3b 151 if(0 == tmpApsErrCountU)
sift 2:9d69f27a3d3b 152 errCounter.apsUnderVolt = 0; //どちらも正常時エラーカウンタクリア
sift 2:9d69f27a3d3b 153 else
sift 2:9d69f27a3d3b 154 errCounter.apsUnderVolt += tmpApsErrCountU;
sift 2:9d69f27a3d3b 155
sift 2:9d69f27a3d3b 156 //センサー偏差チェック
sift 2:9d69f27a3d3b 157 if(myAbs(tmpApsP - tmpApsS) > APS_DEVIATION_TOLERANCE) { //偏差チェックには補正後の値(tmp)を使用
sift 2:9d69f27a3d3b 158 errCounter.apsErrorTolerance++;
sift 2:9d69f27a3d3b 159 } else {
sift 2:9d69f27a3d3b 160 errCounter.apsErrorTolerance = 0;
sift 2:9d69f27a3d3b 161 }
sift 1:4d86ec2fe4b1 162
sift 2:9d69f27a3d3b 163 //小さい方にクリップ
sift 2:9d69f27a3d3b 164 //APS値は好きな方を使いな
sift 2:9d69f27a3d3b 165 if(tmpApsP > tmpApsS) {
sift 2:9d69f27a3d3b 166 tmpApsP = tmpApsS;
sift 2:9d69f27a3d3b 167 } else {
sift 2:9d69f27a3d3b 168 tmpApsS = tmpApsP;
sift 2:9d69f27a3d3b 169 }
sift 2:9d69f27a3d3b 170
sift 2:9d69f27a3d3b 171 //Brake上限値チェック
sift 12:ae291fa7239c 172 if(tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 173 errCounter.brakeExceedVolt++;
sift 12:ae291fa7239c 174 tmpBrake = BRK_ON_VOLTAGE;
sift 2:9d69f27a3d3b 175 } else {
sift 2:9d69f27a3d3b 176 errCounter.brakeExceedVolt = 0;
sift 2:9d69f27a3d3b 177 }
sift 2:9d69f27a3d3b 178
sift 2:9d69f27a3d3b 179 //Brake下限値チェック
sift 12:ae291fa7239c 180 if(tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) {
sift 2:9d69f27a3d3b 181 errCounter.brakeUnderVolt++;
sift 12:ae291fa7239c 182 tmpBrake = BRK_OFF_VOLTAGE;
sift 2:9d69f27a3d3b 183 } else {
sift 2:9d69f27a3d3b 184 errCounter.brakeUnderVolt = 0;
sift 2:9d69f27a3d3b 185 }
sift 1:4d86ec2fe4b1 186
sift 2:9d69f27a3d3b 187 //brake範囲外電圧チェック
sift 12:ae291fa7239c 188 if((tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_OFF_VOLTAGE + ERROR_TOLERANCE)) {
sift 2:9d69f27a3d3b 189 errCounter.brakeFuzzyVolt++;
sift 2:9d69f27a3d3b 190 tmpBrake = BRK_OFF_VOLTAGE;
sift 2:9d69f27a3d3b 191 } else {
sift 2:9d69f27a3d3b 192 errCounter.brakeFuzzyVolt=0;
sift 2:9d69f27a3d3b 193 }
sift 2:9d69f27a3d3b 194
sift 2:9d69f27a3d3b 195 //APS固着チェック
sift 2:9d69f27a3d3b 196 if((preApsP == tmpApsP) && (tmpApsP == APS_MAX_POSITION))
sift 2:9d69f27a3d3b 197 errCounter.apsStick++;
sift 2:9d69f27a3d3b 198 else
sift 2:9d69f27a3d3b 199 errCounter.apsStick=0;
sift 2:9d69f27a3d3b 200
sift 2:9d69f27a3d3b 201 //ブレーキオーバーライドチェック
sift 12:ae291fa7239c 202 if((isBrakeOn() == 1) && (tmpApsP >= APS_OVERRIDE25)) //Brake-ON and APS > 25%
sift 2:9d69f27a3d3b 203 errCounter.brakeOverRide++;
sift 12:ae291fa7239c 204 if(tmpApsP < APS_OVERRIDE05) //Brake-ON and APS < 5%
sift 2:9d69f27a3d3b 205 errCounter.brakeOverRide=0;
sift 2:9d69f27a3d3b 206
sift 2:9d69f27a3d3b 207 //センサ値取得
sift 2:9d69f27a3d3b 208 gApsP = tmpApsP;
sift 2:9d69f27a3d3b 209 gApsS = tmpApsS;
sift 2:9d69f27a3d3b 210 gBrake = tmpBrake;
sift 2:9d69f27a3d3b 211
sift 2:9d69f27a3d3b 212 //未来の自分に期待
sift 2:9d69f27a3d3b 213 preApsP = rawApsP;
sift 2:9d69f27a3d3b 214 preApsS = rawApsS;
sift 2:9d69f27a3d3b 215 preBrake = rawBrake;
sift 2:9d69f27a3d3b 216 }
sift 1:4d86ec2fe4b1 217 }
sift 1:4d86ec2fe4b1 218
sift 5:a5462959b3ab 219 volatile int gRightPulseTime=100000, gLeftPulseTime=100000;
sift 7:ad013d88a539 220 volatile bool pulseTimeISRFlag = false;
sift 1:4d86ec2fe4b1 221
sift 2:9d69f27a3d3b 222 void countRightPulseISR(void)
sift 1:4d86ec2fe4b1 223 {
sift 1:4d86ec2fe4b1 224 //Do not use "printf" in interrupt!!!
sift 1:4d86ec2fe4b1 225 static int preTime=0;
sift 2:9d69f27a3d3b 226 int currentTime = RightPulseTimer.read_us();
sift 4:d7778cde0aff 227
sift 2:9d69f27a3d3b 228 gRightPulseTime = currentTime - preTime;
sift 2:9d69f27a3d3b 229
sift 18:b7c362c8f0fd 230 if(gRightPulseTime < MIN_PULSE_TIME) //12000rpm上限より早い場合
sift 18:b7c362c8f0fd 231 gRightPulseTime = MIN_PULSE_TIME;
sift 2:9d69f27a3d3b 232
sift 2:9d69f27a3d3b 233 if(currentTime < 1800000000) {
sift 2:9d69f27a3d3b 234 preTime = currentTime;
sift 2:9d69f27a3d3b 235 } else { //30分経過後
sift 2:9d69f27a3d3b 236 RightPulseTimer.reset();
sift 2:9d69f27a3d3b 237 preTime = 0;
sift 2:9d69f27a3d3b 238 }
sift 1:4d86ec2fe4b1 239 }
sift 1:4d86ec2fe4b1 240
sift 2:9d69f27a3d3b 241 void countLeftPulseISR(void)
sift 1:4d86ec2fe4b1 242 {
sift 1:4d86ec2fe4b1 243 //Do not use "printf" in interrupt!!!
sift 1:4d86ec2fe4b1 244 static int preTime=0;
sift 2:9d69f27a3d3b 245 int currentTime = LeftPulseTimer.read_us();
sift 2:9d69f27a3d3b 246
sift 2:9d69f27a3d3b 247 gLeftPulseTime = currentTime - preTime;
sift 2:9d69f27a3d3b 248
sift 18:b7c362c8f0fd 249 if(gLeftPulseTime < MIN_PULSE_TIME) //12000rpm上限より早い場合
sift 18:b7c362c8f0fd 250 gLeftPulseTime = MIN_PULSE_TIME;
sift 2:9d69f27a3d3b 251
sift 2:9d69f27a3d3b 252 if(currentTime < 1800000000) {
sift 2:9d69f27a3d3b 253 preTime = currentTime;
sift 2:9d69f27a3d3b 254 } else { //30分経過後
sift 2:9d69f27a3d3b 255 LeftPulseTimer.reset();
sift 2:9d69f27a3d3b 256 preTime = 0;
sift 2:9d69f27a3d3b 257 }
sift 2:9d69f27a3d3b 258 }
sift 2:9d69f27a3d3b 259
sift 7:ad013d88a539 260 void getPulseTimeISR(void)
sift 2:9d69f27a3d3b 261 {
sift 7:ad013d88a539 262 pulseTimeISRFlag = true;
sift 2:9d69f27a3d3b 263 }
sift 2:9d69f27a3d3b 264
sift 2:9d69f27a3d3b 265 int getPulseTime(SelectMotor rl)
sift 2:9d69f27a3d3b 266 {
sift 2:9d69f27a3d3b 267 static int preRightPulse, preLeftPulse;
sift 2:9d69f27a3d3b 268
sift 7:ad013d88a539 269 if(pulseTimeISRFlag == true) {
sift 7:ad013d88a539 270 pulseTimeISRFlag = false;
sift 2:9d69f27a3d3b 271
sift 18:b7c362c8f0fd 272 if(gRightPulseTime > MAX_PULSE_TIME) //最大パルス時間にクリップ
sift 18:b7c362c8f0fd 273 gRightPulseTime = MAX_PULSE_TIME;
sift 18:b7c362c8f0fd 274 if(gLeftPulseTime > MAX_PULSE_TIME)
sift 18:b7c362c8f0fd 275 gLeftPulseTime = MAX_PULSE_TIME;
sift 10:87ad65eef0e9 276
sift 3:821e2f07a260 277 preRightPulse = (int)(gRightPulseTime*ratioLPF_V + preRightPulse*(1.0f-ratioLPF_V));
sift 3:821e2f07a260 278 preLeftPulse = (int)(gLeftPulseTime*ratioLPF_V + preLeftPulse*(1.0f-ratioLPF_V));
sift 2:9d69f27a3d3b 279 }
sift 2:9d69f27a3d3b 280
sift 2:9d69f27a3d3b 281 if(rl == RIGHT_MOTOR)
sift 2:9d69f27a3d3b 282 return preRightPulse;
sift 2:9d69f27a3d3b 283 else
sift 2:9d69f27a3d3b 284 return preLeftPulse;
sift 2:9d69f27a3d3b 285 }
sift 2:9d69f27a3d3b 286
sift 2:9d69f27a3d3b 287 float getVelocity(void)
sift 2:9d69f27a3d3b 288 {
sift 2:9d69f27a3d3b 289 int rightPulse=0, leftPulse=0;
sift 2:9d69f27a3d3b 290 int avePulseTime;
sift 2:9d69f27a3d3b 291
sift 2:9d69f27a3d3b 292 rightPulse = getPulseTime(RIGHT_MOTOR);
sift 2:9d69f27a3d3b 293 leftPulse = getPulseTime(LEFT_MOTOR);
sift 2:9d69f27a3d3b 294
sift 5:a5462959b3ab 295 avePulseTime = (int)((rightPulse+leftPulse)/2.0);
sift 2:9d69f27a3d3b 296
sift 18:b7c362c8f0fd 297 if(avePulseTime < MIN_PULSE_TIME) //最低パルス時間にクリップ
sift 18:b7c362c8f0fd 298 avePulseTime = MIN_PULSE_TIME;
sift 2:9d69f27a3d3b 299
sift 18:b7c362c8f0fd 300 return (float)(M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0)*TVD_GEAR_RATIO));
sift 1:4d86ec2fe4b1 301 }
sift 1:4d86ec2fe4b1 302
sift 1:4d86ec2fe4b1 303 void generatePulse(void)
sift 1:4d86ec2fe4b1 304 {
sift 1:4d86ec2fe4b1 305 static bool flag = false;
sift 1:4d86ec2fe4b1 306 flag = !flag;
sift 1:4d86ec2fe4b1 307 MotorPulse[0] = MotorPulse[1] = LED[0] = flag;
sift 1:4d86ec2fe4b1 308 }
sift 1:4d86ec2fe4b1 309
sift 12:ae291fa7239c 310 int distributeTorque(float steering)
sift 2:9d69f27a3d3b 311 {
sift 9:220e4e77e056 312 int disTrq = 0;
sift 13:6dc51981f391 313 const float deadband = (M_PI/180.0f)*5.0f;
sift 2:9d69f27a3d3b 314
sift 12:ae291fa7239c 315 if(steering < deadband)
sift 9:220e4e77e056 316 disTrq = 0;
sift 12:ae291fa7239c 317 else {
sift 12:ae291fa7239c 318 steering -= deadband;
sift 14:7cc98e159c6e 319 disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI - deadband) * steering);
sift 2:9d69f27a3d3b 320 }
sift 14:7cc98e159c6e 321 //pc.printf("%d\r\n", disTrq);
sift 2:9d69f27a3d3b 322
sift 9:220e4e77e056 323 return disTrq;
sift 2:9d69f27a3d3b 324 }
sift 2:9d69f27a3d3b 325
sift 2:9d69f27a3d3b 326 //トルク値線形補間関数
sift 7:ad013d88a539 327 inline int interpolateLinear(int torque, int currentMaxTorque)
sift 2:9d69f27a3d3b 328 {
sift 8:a22aec357a64 329 return (int)(((double)(DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(currentMaxTorque-LINEAR_REGION_TORQUE)) * (torque-LINEAR_REGION_TORQUE)) + LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN;
sift 2:9d69f27a3d3b 330 }
sift 2:9d69f27a3d3b 331
sift 2:9d69f27a3d3b 332 unsigned int calcTorqueToVoltage(int torque, SelectMotor rl)
sift 2:9d69f27a3d3b 333 {
sift 2:9d69f27a3d3b 334 int outputVoltage=0;
sift 2:9d69f27a3d3b 335 int rpm=0;
sift 6:26fa8c78500e 336 int currentMaxTorque=0;
sift 12:ae291fa7239c 337
sift 12:ae291fa7239c 338 static int preOutputVol=0;
sift 9:220e4e77e056 339
sift 2:9d69f27a3d3b 340 if(torque <= LINEAR_REGION_TORQUE) { //要求トルク<=2.5Nmの時
sift 7:ad013d88a539 341 outputVoltage = (int)((double)(LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN)/LINEAR_REGION_TORQUE * torque);
sift 2:9d69f27a3d3b 342 } else {
sift 15:3255cbe4ff34 343 //rpm = (int)(1.0/getPulseTime(rl)*1000000.0 * 60.0); //pulseTime:[us]
sift 16:7afd623ef48a 344
sift 15:3255cbe4ff34 345 rpm = 0;
sift 6:26fa8c78500e 346
sift 6:26fa8c78500e 347 if(rpm < 3000) { //3000rpm未満は回転数による出力制限がないフラットな領域
sift 2:9d69f27a3d3b 348 outputVoltage = interpolateLinear(torque, MAX_MOTOR_TORQUE);
sift 6:26fa8c78500e 349 } else {
sift 6:26fa8c78500e 350 if(rpm <= 11000) {
sift 6:26fa8c78500e 351 int index = (int)((rpm - 3000)/10.0); //マップは10rpm刻みに作成
sift 6:26fa8c78500e 352 currentMaxTorque = calcMaxTorque[index];
sift 6:26fa8c78500e 353 } else {
sift 6:26fa8c78500e 354 currentMaxTorque = MAX_REVOLUTION_TORQUE; //回転数上限時の最大トルク
sift 6:26fa8c78500e 355 }
sift 6:26fa8c78500e 356
sift 6:26fa8c78500e 357 if(currentMaxTorque < torque) { //要求トルクが現在の回転数での最大値を超えている時
sift 6:26fa8c78500e 358 outputVoltage = DACOUTPUT_VALID_RANGE; //現在の回転数での最大トルクにクリップ
sift 6:26fa8c78500e 359 } else {
sift 6:26fa8c78500e 360 outputVoltage = interpolateLinear(torque, currentMaxTorque);
sift 6:26fa8c78500e 361 }
sift 6:26fa8c78500e 362 }
sift 2:9d69f27a3d3b 363 }
sift 2:9d69f27a3d3b 364
sift 2:9d69f27a3d3b 365 outputVoltage += DACOUTPUT_MIN; //最低入力電圧でかさ上げ
sift 12:ae291fa7239c 366
sift 18:b7c362c8f0fd 367 preOutputVol = (int)(outputVoltage*0.1 + preOutputVol*0.9);
sift 16:7afd623ef48a 368
sift 15:3255cbe4ff34 369 //printf("%d\r\n", (int)(0xFFF*((double)preOutputVol/0xFFFF)));
sift 9:220e4e77e056 370
sift 11:88701a5e7eae 371 return (unsigned int)(0xFFF*((double)preOutputVol/0xFFFF)); //DACの分解能に適応(16bit->12bit)
sift 2:9d69f27a3d3b 372 }
sift 2:9d69f27a3d3b 373
sift 2:9d69f27a3d3b 374 int calcRequestTorque(void)
sift 2:9d69f27a3d3b 375 {
sift 2:9d69f27a3d3b 376 int currentAPS;
sift 2:9d69f27a3d3b 377 int requestTorque;
sift 2:9d69f27a3d3b 378
sift 2:9d69f27a3d3b 379 currentAPS = ((gApsP>gApsS) ? gApsS : gApsP); //センサ値は小さい方を採用
sift 12:ae291fa7239c 380
sift 2:9d69f27a3d3b 381 if(currentAPS < APS_MIN_POSITION)
sift 2:9d69f27a3d3b 382 currentAPS = 0;
sift 2:9d69f27a3d3b 383 else
sift 2:9d69f27a3d3b 384 currentAPS -= APS_MIN_POSITION; //オフセット修正
sift 2:9d69f27a3d3b 385
sift 2:9d69f27a3d3b 386 if(currentAPS < APS_DEADBAND) //デッドバンド内であれば要求トルク->0
sift 2:9d69f27a3d3b 387 requestTorque = 0;
sift 2:9d69f27a3d3b 388 else
sift 2:9d69f27a3d3b 389 requestTorque = (int)(((double)MAX_OUTPUT_TORQUE / APS_VALID_RANGE) * (currentAPS - APS_DEADBAND));
sift 2:9d69f27a3d3b 390
sift 6:26fa8c78500e 391 if(requestTorque > MAX_OUTPUT_TORQUE)
sift 6:26fa8c78500e 392 requestTorque = MAX_OUTPUT_TORQUE;
sift 2:9d69f27a3d3b 393 else if(requestTorque < 0)
sift 2:9d69f27a3d3b 394 requestTorque = 0;
sift 2:9d69f27a3d3b 395
sift 12:ae291fa7239c 396 if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1))
sift 12:ae291fa7239c 397 requestTorque = 0;
sift 12:ae291fa7239c 398
sift 2:9d69f27a3d3b 399 return requestTorque;
sift 2:9d69f27a3d3b 400 }
sift 2:9d69f27a3d3b 401
sift 17:a2246ce3333f 402 //トルク配分車速制限関数
sift 17:a2246ce3333f 403 //車速が低速域の場合，トルク配分0
sift 17:a2246ce3333f 404 float limitTorqueDistribution(void)
sift 17:a2246ce3333f 405 {
sift 17:a2246ce3333f 406 float limitRate;
sift 17:a2246ce3333f 407 float currentVelocity = getVelocity() * 3.6f; //km/hで車速取得
sift 17:a2246ce3333f 408
sift 17:a2246ce3333f 409 if(currentVelocity < 5.0f)
sift 17:a2246ce3333f 410 limitRate = 0.0f;
sift 18:b7c362c8f0fd 411 else if(currentVelocity < 15.0f)
sift 18:b7c362c8f0fd 412 limitRate = (currentVelocity - 5.0f) / (15.0f - 5.0f);
sift 17:a2246ce3333f 413 else
sift 17:a2246ce3333f 414 limitRate = 1.0f;
sift 17:a2246ce3333f 415
sift 17:a2246ce3333f 416 return limitRate;
sift 17:a2246ce3333f 417 }
sift 17:a2246ce3333f 418
sift 12:ae291fa7239c 419 extern DigitalIn RTDSW;
sift 12:ae291fa7239c 420 #define isPressedRTD(void) (!RTDSW.read())
sift 12:ae291fa7239c 421 extern DigitalOut indicatorLed;
sift 12:ae291fa7239c 422 #define indicateSystem(x) (indicatorLed.write(x))
sift 12:ae291fa7239c 423
sift 1:4d86ec2fe4b1 424 void driveTVD(void)
sift 1:4d86ec2fe4b1 425 {
sift 6:26fa8c78500e 426 int requestTorque=0; //ドライバー要求トルク
sift 6:26fa8c78500e 427 int distributionTrq=0; //分配トルク
sift 7:ad013d88a539 428 int torqueHigh, torqueLow; //トルクの大きい方小さい方
sift 12:ae291fa7239c 429
sift 2:9d69f27a3d3b 430 loadSensors(); //APS,BRAKE更新
sift 2:9d69f27a3d3b 431 loadSteerAngle(); //舵角更新
sift 12:ae291fa7239c 432
sift 12:ae291fa7239c 433 if(isPressedRTD() && isBrakeOn())
sift 12:ae291fa7239c 434 readyToDriveFlag = 0;
sift 2:9d69f27a3d3b 435
sift 12:ae291fa7239c 436 if((errCounter.apsUnderVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 437 || (errCounter.apsExceedVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 438 || (errCounter.apsErrorTolerance > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 439 // || (errCounter.apsStick > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 440 || (errCounter.brakeUnderVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 441 || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 442 || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION)
sift 12:ae291fa7239c 443 ) {
sift 16:7afd623ef48a 444 readyToDriveFlag = 1;
sift 12:ae291fa7239c 445 }
sift 16:7afd623ef48a 446
sift 12:ae291fa7239c 447 indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
sift 16:7afd623ef48a 448
sift 6:26fa8c78500e 449 requestTorque=calcRequestTorque(); //ドライバー要求トルク取得
sift 4:d7778cde0aff 450
sift 17:a2246ce3333f 451 distributionTrq = (int)(distributeTorque(getSteerAngle()) / 2.0); //片モーターのトルク分配量計算
sift 17:a2246ce3333f 452
sift 18:b7c362c8f0fd 453 //distributionTrq = (int)(distributionTrq * limitTorqueDistribution()); //トルク配分の最低車速制限
sift 12:ae291fa7239c 454
sift 10:87ad65eef0e9 455 //デバッグ中
sift 13:6dc51981f391 456 //distributionTrq = 0;
sift 17:a2246ce3333f 457
sift 16:7afd623ef48a 458 if(requestTorque < MIN_INNERWHEEL_TORQUE) {
sift 18:b7c362c8f0fd 459 torqueHigh = torqueLow = requestTorque; //内輪側モーター最低トルクより小さい要求トルクなら等配分
sift 16:7afd623ef48a 460 } else {
sift 16:7afd623ef48a 461 if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE) //片モーター上限時最大値にクリップ
sift 16:7afd623ef48a 462 torqueHigh = MAX_OUTPUT_TORQUE;
sift 16:7afd623ef48a 463 else
sift 16:7afd623ef48a 464 torqueHigh = requestTorque + distributionTrq;
sift 16:7afd623ef48a 465
sift 16:7afd623ef48a 466 if(requestTorque - distributionTrq < MIN_INNERWHEEL_TORQUE) {
sift 16:7afd623ef48a 467 torqueLow = MIN_INNERWHEEL_TORQUE; //内輪最低トルクにクリップ
sift 18:b7c362c8f0fd 468 torqueHigh = (int)((requestTorque-MIN_INNERWHEEL_TORQUE)*2.0) + MIN_INNERWHEEL_TORQUE; //片モーター下限値時，トルク高側のモーターも出力クリップ
sift 16:7afd623ef48a 469 } else
sift 16:7afd623ef48a 470 torqueLow = requestTorque - distributionTrq;
sift 16:7afd623ef48a 471 }
sift 1:4d86ec2fe4b1 472
sift 14:7cc98e159c6e 473 //printf("%d %d\r\n", torqueLow, torqueHigh);
sift 2:9d69f27a3d3b 474
sift 14:7cc98e159c6e 475 if(getSteerDirection()==1) {
sift 2:9d69f27a3d3b 476 //steer left
sift 9:220e4e77e056 477 McpData.valA = calcTorqueToVoltage(torqueHigh, RIGHT_MOTOR);
sift 9:220e4e77e056 478 McpData.valB = calcTorqueToVoltage(torqueLow, LEFT_MOTOR);
sift 2:9d69f27a3d3b 479 } else {
sift 2:9d69f27a3d3b 480 //steer right
sift 9:220e4e77e056 481 McpData.valA = calcTorqueToVoltage(torqueLow, RIGHT_MOTOR);
sift 9:220e4e77e056 482 McpData.valB = calcTorqueToVoltage(torqueHigh, LEFT_MOTOR);
sift 2:9d69f27a3d3b 483 }
sift 16:7afd623ef48a 484
sift 18:b7c362c8f0fd 485 //pc.printf("%u %u\r\n", McpData.valA, McpData.valB);
sift 2:9d69f27a3d3b 486
sift 2:9d69f27a3d3b 487 mcp.writeA(McpData.valA); //右モーター
sift 2:9d69f27a3d3b 488 mcp.writeB(McpData.valB); //左モーター
sift 1:4d86ec2fe4b1 489 }
sift 1:4d86ec2fe4b1 490
sift 1:4d86ec2fe4b1 491 void initTVD(void)
sift 1:4d86ec2fe4b1 492 {
sift 1:4d86ec2fe4b1 493 rightMotorPulse.mode(PullUp);
sift 1:4d86ec2fe4b1 494 leftMotorPulse.mode(PullUp);
sift 2:9d69f27a3d3b 495 rightMotorPulse.fall(&countRightPulseISR);
sift 2:9d69f27a3d3b 496 leftMotorPulse.fall(&countLeftPulseISR);
sift 1:4d86ec2fe4b1 497
sift 2:9d69f27a3d3b 498 RightPulseTimer.reset();
sift 2:9d69f27a3d3b 499 LeftPulseTimer.reset();
sift 2:9d69f27a3d3b 500 RightPulseTimer.start();
sift 2:9d69f27a3d3b 501 LeftPulseTimer.start();
sift 1:4d86ec2fe4b1 502
sift 2:9d69f27a3d3b 503 ticker1.attach(&loadSensorsISR, 0.01f); //サンプリング周期10msec
sift 2:9d69f27a3d3b 504 //ticker2.attach(&generatePulse, 0.03f);
sift 7:ad013d88a539 505 ticker3.attach(&getPulseTimeISR, 0.01f);
sift 10:87ad65eef0e9 506
sift 10:87ad65eef0e9 507 mcp.writeA(0); //右モーター
sift 10:87ad65eef0e9 508 mcp.writeB(0); //左モーター
sift 18:b7c362c8f0fd 509
sift 18:b7c362c8f0fd 510 printf("MAX OUTPUT TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_OUTPUT_TORQUE);
sift 18:b7c362c8f0fd 511 printf("MAX DISTRIBUTION TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_DISTRIBUTION_TORQUE);
sift 18:b7c362c8f0fd 512 printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MIN_INNERWHEEL_TORQUE);
sift 1:4d86ec2fe4b1 513 }