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@25:c21d35c7f0de, 2017-07-01 (annotated)

Committer:
sift
Date:
Sat Jul 01 00:26:28 2017 +0000
Revision:
25:c21d35c7f0de
Parent:
24:1de0291bc5eb
Child:
26:331e77bb479b
??????????????????; (????????? ???)

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