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@21:bbf2ad7e6602, 2016-09-02 (annotated)

Committer:
sift
Date:
Fri Sep 02 22:41:09 2016 +0000
Revision:
21:bbf2ad7e6602
Parent:
20:3c5061281a7a
Child:
22:95c1f753ecad
????????; ?????????????

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