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@12:ae291fa7239c, 2016-08-07 (annotated)

Committer:
sift
Date:
Sun Aug 07 15:23:01 2016 +0000
Revision:
12:ae291fa7239c
Parent:
11:88701a5e7eae
Child:
13:6dc51981f391
TVD???????; ??????; ???????????(?????)

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