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@8:a22aec357a64, 2016-07-27 (annotated)

Committer:
sift
Date:
Wed Jul 27 04:45:07 2016 +0000
Revision:
8:a22aec357a64
Parent:
7:ad013d88a539
Child:
9:220e4e77e056
??????????????????; (?????????????)

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