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@4:d7778cde0aff, 2016-07-24 (annotated)

Committer:
sift
Date:
Sun Jul 24 08:23:56 2016 +0000
Revision:
4:d7778cde0aff
Parent:
3:821e2f07a260
Child:
5:a5462959b3ab
????0????????; ????????????????

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