2014 sift
/
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.
Fork of
TVDctrller2017_brdRev1_ver6
by 
2014 sift
TVDCTRL.cpp@42:3ab09d0e3071, 2017-10-28 (annotated)
- Committer:
- sift
- Date:
- Sat Oct 28 06:44:09 2017 +0000
- Revision:
- 42:3ab09d0e3071
- Parent:
- 41:0c53acd31247
- Child:
- 43:5da6b1574227
???????????????; ??LPF5Hz???; ?????????????????????
Who changed what in which revision?
| User | Revision | Line number | New 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 | 39:c05074379713 | 4 | #include "Global.h" |
| sift | 1:4d86ec2fe4b1 | 5 | |
| sift | 1:4d86ec2fe4b1 | 6 | extern AnalogIn apsP; |
| sift | 1:4d86ec2fe4b1 | 7 | extern AnalogIn apsS; |
| sift | 2:9d69f27a3d3b | 8 | extern AnalogIn brake; |
| sift | 1:4d86ec2fe4b1 | 9 | extern DigitalOut LED[]; |
| sift | 27:37a8b4f6d28d | 10 | extern DigitalOut brakeSignal; |
| sift | 27:37a8b4f6d28d | 11 | extern DigitalOut indicatorLed; |
| sift | 27:37a8b4f6d28d | 12 | extern DigitalOut shutDown; |
| sift | 27:37a8b4f6d28d | 13 | extern DigitalIn sdState; |
| sift | 1:4d86ec2fe4b1 | 14 | extern InterruptIn rightMotorPulse; |
| sift | 1:4d86ec2fe4b1 | 15 | extern InterruptIn leftMotorPulse; |
| sift | 39:c05074379713 | 16 | extern InterruptIn rightWheelPulse1; |
| sift | 39:c05074379713 | 17 | extern InterruptIn rightWheelPulse2; |
| sift | 39:c05074379713 | 18 | extern InterruptIn leftWheelPulse1; |
| sift | 39:c05074379713 | 19 | extern InterruptIn leftWheelPulse2; |
| sift | 1:4d86ec2fe4b1 | 20 | extern MCP4922 mcp; |
| sift | 8:a22aec357a64 | 21 | extern Serial pc; |
| sift | 19:571a4d00b89c | 22 | extern AnalogOut STR2AN; |
| sift | 27:37a8b4f6d28d | 23 | extern CAN can; |
| sift | 1:4d86ec2fe4b1 | 24 | |
| sift | 26:331e77bb479b | 25 | #define indicateSystem(x) (indicatorLed.write(x)) |
| sift | 26:331e77bb479b | 26 | |
| sift | 39:c05074379713 | 27 | Timer wheelPulseTimer; |
| sift | 2:9d69f27a3d3b | 28 | Ticker ticker1; |
| sift | 2:9d69f27a3d3b | 29 | Ticker ticker2; |
| sift | 1:4d86ec2fe4b1 | 30 | |
| sift | 2:9d69f27a3d3b | 31 | #define apsPVol() (apsP.read() * 3.3) |
| sift | 2:9d69f27a3d3b | 32 | #define apsSVol() (apsS.read() * 3.3) |
| sift | 0:276c1dab2d62 | 33 | |
| sift | 0:276c1dab2d62 | 34 | struct { |
| sift | 0:276c1dab2d62 | 35 | unsigned int valA:12; |
| sift | 0:276c1dab2d62 | 36 | unsigned int valB:12; |
| sift | 2:9d69f27a3d3b | 37 | } McpData; |
| sift | 1:4d86ec2fe4b1 | 38 | |
| sift | 2:9d69f27a3d3b | 39 | //各変数が一定値を超えた時点でエラー検出とする |
| sift | 2:9d69f27a3d3b | 40 | //2つのAPSの区別はつけないことにする |
| sift | 12:ae291fa7239c | 41 | struct errCounter_t errCounter= {0,0,0,0,0,0,0}; |
| sift | 1:4d86ec2fe4b1 | 42 | |
| sift | 12:ae291fa7239c | 43 | int readyToDriveFlag = 1; |
| sift | 12:ae291fa7239c | 44 | |
| sift | 12:ae291fa7239c | 45 | int gApsP=0, gApsS=0, gBrake=0; //現在のセンサ値 |
| sift | 12:ae291fa7239c | 46 | int rawApsP=0, rawApsS=0, rawBrake=0; //現在の補正無しのセンサ値 |
| sift | 2:9d69f27a3d3b | 47 | |
| sift | 28:47e9531a3a9d | 48 | int gRightMotorTorque=0, gLeftMotorTorque=0; |
| sift | 28:47e9531a3a9d | 49 | |
| sift | 28:47e9531a3a9d | 50 | int getMotorTorque(Select rl) |
| sift | 28:47e9531a3a9d | 51 | { |
| sift | 28:47e9531a3a9d | 52 | return ((rl==LEFT) ? gLeftMotorTorque : gRightMotorTorque); |
| sift | 28:47e9531a3a9d | 53 | } |
| sift | 28:47e9531a3a9d | 54 | |
| sift | 18:b7c362c8f0fd | 55 | //エラーカウンタ外部参照用関数 |
| sift | 18:b7c362c8f0fd | 56 | //errCounter_t型変数のポインタを引数に取る |
| sift | 2:9d69f27a3d3b | 57 | void getCurrentErrCount(struct errCounter_t *ptr) |
| sift | 1:4d86ec2fe4b1 | 58 | { |
| sift | 12:ae291fa7239c | 59 | ptr->apsUnderVolt = errCounter.apsUnderVolt; |
| sift | 12:ae291fa7239c | 60 | ptr->apsExceedVolt = errCounter.apsExceedVolt; |
| sift | 12:ae291fa7239c | 61 | ptr->apsErrorTolerance = errCounter.apsErrorTolerance; |
| sift | 12:ae291fa7239c | 62 | ptr->apsStick = errCounter.apsStick; |
| sift | 12:ae291fa7239c | 63 | ptr->brakeUnderVolt = errCounter.brakeUnderVolt; |
| sift | 12:ae291fa7239c | 64 | ptr->brakeExceedVolt = errCounter.brakeExceedVolt; |
| sift | 12:ae291fa7239c | 65 | ptr->brakeFuzzyVolt = errCounter.brakeFuzzyVolt; |
| sift | 12:ae291fa7239c | 66 | ptr->brakeOverRide = errCounter.brakeOverRide; |
| sift | 12:ae291fa7239c | 67 | } |
| sift | 12:ae291fa7239c | 68 | |
| sift | 18:b7c362c8f0fd | 69 | //ブレーキONOFF判定関数 |
| sift | 12:ae291fa7239c | 70 | //Brake-ON:1 Brake-OFF:0 |
| sift | 12:ae291fa7239c | 71 | int isBrakeOn(void) |
| sift | 12:ae291fa7239c | 72 | { |
| sift | 12:ae291fa7239c | 73 | int brake = gBrake; |
| sift | 12:ae291fa7239c | 74 | int brakeOnOff = 0; |
| sift | 12:ae291fa7239c | 75 | |
| sift | 30:c596a0f5d685 | 76 | if(brake < (BRK_ON_VOLTAGE + ERROR_TOLERANCE)) |
| sift | 12:ae291fa7239c | 77 | brakeOnOff = 1; |
| sift | 30:c596a0f5d685 | 78 | if(brake > (BRK_OFF_VOLTAGE - ERROR_TOLERANCE)) |
| sift | 12:ae291fa7239c | 79 | brakeOnOff = 0; |
| sift | 12:ae291fa7239c | 80 | |
| sift | 12:ae291fa7239c | 81 | return brakeOnOff; |
| sift | 2:9d69f27a3d3b | 82 | } |
| sift | 1:4d86ec2fe4b1 | 83 | |
| sift | 18:b7c362c8f0fd | 84 | //センサ現在値外部参照関数 |
| sift | 7:ad013d88a539 | 85 | int getCurrentSensor(int sensor) |
| sift | 2:9d69f27a3d3b | 86 | { |
| sift | 2:9d69f27a3d3b | 87 | switch (sensor) { |
| sift | 2:9d69f27a3d3b | 88 | case APS_PRIMARY: |
| sift | 2:9d69f27a3d3b | 89 | return gApsP; |
| sift | 2:9d69f27a3d3b | 90 | case APS_SECONDARY: |
| sift | 2:9d69f27a3d3b | 91 | return gApsS; |
| sift | 2:9d69f27a3d3b | 92 | case BRAKE: |
| sift | 2:9d69f27a3d3b | 93 | return gBrake; |
| sift | 2:9d69f27a3d3b | 94 | default: |
| sift | 2:9d69f27a3d3b | 95 | return -1; |
| sift | 1:4d86ec2fe4b1 | 96 | } |
| sift | 2:9d69f27a3d3b | 97 | } |
| sift | 2:9d69f27a3d3b | 98 | |
| sift | 18:b7c362c8f0fd | 99 | //補正前センサ現在値外部参照関数 |
| sift | 7:ad013d88a539 | 100 | int getRawSensor(int sensor) |
| sift | 2:9d69f27a3d3b | 101 | { |
| sift | 2:9d69f27a3d3b | 102 | switch (sensor) { |
| sift | 2:9d69f27a3d3b | 103 | case APS_PRIMARY: |
| sift | 2:9d69f27a3d3b | 104 | return rawApsP; |
| sift | 2:9d69f27a3d3b | 105 | case APS_SECONDARY: |
| sift | 2:9d69f27a3d3b | 106 | return rawApsS; |
| sift | 2:9d69f27a3d3b | 107 | case BRAKE: |
| sift | 2:9d69f27a3d3b | 108 | return rawBrake; |
| sift | 2:9d69f27a3d3b | 109 | default: |
| sift | 2:9d69f27a3d3b | 110 | return -1; |
| sift | 1:4d86ec2fe4b1 | 111 | } |
| sift | 2:9d69f27a3d3b | 112 | } |
| sift | 2:9d69f27a3d3b | 113 | |
| sift | 2:9d69f27a3d3b | 114 | bool loadSensorFlag = false; |
| sift | 2:9d69f27a3d3b | 115 | |
| sift | 2:9d69f27a3d3b | 116 | //タイマー割り込みでコールされる |
| sift | 2:9d69f27a3d3b | 117 | void loadSensorsISR(void) |
| sift | 2:9d69f27a3d3b | 118 | { |
| sift | 2:9d69f27a3d3b | 119 | loadSensorFlag = true; |
| sift | 1:4d86ec2fe4b1 | 120 | } |
| sift | 1:4d86ec2fe4b1 | 121 | |
| sift | 25:c21d35c7f0de | 122 | //センサ読み込み関数 |
| sift | 2:9d69f27a3d3b | 123 | void loadSensors(void) |
| sift | 1:4d86ec2fe4b1 | 124 | { |
| sift | 2:9d69f27a3d3b | 125 | if(true == loadSensorFlag) { |
| sift | 2:9d69f27a3d3b | 126 | loadSensorFlag = false; |
| sift | 2:9d69f27a3d3b | 127 | static int preApsP=0, preApsS=0; //過去のセンサ値 |
| sift | 2:9d69f27a3d3b | 128 | static int preBrake=0; |
| sift | 2:9d69f27a3d3b | 129 | int tmpApsP=0, tmpApsS=0, tmpBrake=0; //補正後のセンサ値 |
| sift | 2:9d69f27a3d3b | 130 | int tmpApsErrCountU=0, tmpApsErrCountE=0; //APSの一時的なエラーカウンタ |
| sift | 2:9d69f27a3d3b | 131 | |
| sift | 2:9d69f27a3d3b | 132 | //Low Pass Filter |
| sift | 2:9d69f27a3d3b | 133 | tmpApsP = (int)(apsP.read_u16()*ratioLPF + preApsP*(1.0f-ratioLPF)); |
| sift | 2:9d69f27a3d3b | 134 | tmpApsS = (int)(apsS.read_u16()*ratioLPF + preApsS*(1.0f-ratioLPF)); |
| sift | 2:9d69f27a3d3b | 135 | tmpBrake = (int)(brake.read_u16()*ratioLPF + preBrake*(1.0f-ratioLPF)); |
| sift | 2:9d69f27a3d3b | 136 | |
| sift | 2:9d69f27a3d3b | 137 | //生のセンサ値取得 |
| sift | 2:9d69f27a3d3b | 138 | rawApsP = tmpApsP; |
| sift | 2:9d69f27a3d3b | 139 | rawApsS = tmpApsS; |
| sift | 2:9d69f27a3d3b | 140 | rawBrake = tmpBrake; |
| sift | 2:9d69f27a3d3b | 141 | |
| sift | 2:9d69f27a3d3b | 142 | //センサーチェック |
| sift | 2:9d69f27a3d3b | 143 | //APS上限値チェック |
| sift | 2:9d69f27a3d3b | 144 | if(tmpApsP > APS_MAX_POSITION + ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 145 | tmpApsP = APS_MAX_POSITION; //異常時，上限値にクリップ |
| sift | 2:9d69f27a3d3b | 146 | tmpApsErrCountE++; |
| sift | 2:9d69f27a3d3b | 147 | } |
| sift | 2:9d69f27a3d3b | 148 | if(tmpApsS > APS_MAX_POSITION + ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 149 | tmpApsS = APS_MAX_POSITION; //異常時，上限値にクリップ |
| sift | 2:9d69f27a3d3b | 150 | tmpApsErrCountE++; |
| sift | 2:9d69f27a3d3b | 151 | } |
| sift | 2:9d69f27a3d3b | 152 | if(0 == tmpApsErrCountE) |
| sift | 2:9d69f27a3d3b | 153 | errCounter.apsExceedVolt = 0; //どちらも正常時エラーカウンタクリア |
| sift | 2:9d69f27a3d3b | 154 | else |
| sift | 2:9d69f27a3d3b | 155 | errCounter.apsExceedVolt += tmpApsErrCountE; |
| sift | 2:9d69f27a3d3b | 156 | |
| sift | 2:9d69f27a3d3b | 157 | //APS下限値チェック |
| sift | 2:9d69f27a3d3b | 158 | if(tmpApsP < APS_MIN_POSITION - ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 159 | tmpApsP = APS_MIN_POSITION; //下限値にクリップ |
| sift | 2:9d69f27a3d3b | 160 | tmpApsErrCountU++; |
| sift | 2:9d69f27a3d3b | 161 | } |
| sift | 2:9d69f27a3d3b | 162 | if(tmpApsS < APS_MIN_POSITION - ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 163 | tmpApsS = APS_MIN_POSITION; //下限値にクリップ |
| sift | 2:9d69f27a3d3b | 164 | tmpApsErrCountU++; |
| sift | 2:9d69f27a3d3b | 165 | } |
| sift | 2:9d69f27a3d3b | 166 | if(0 == tmpApsErrCountU) |
| sift | 2:9d69f27a3d3b | 167 | errCounter.apsUnderVolt = 0; //どちらも正常時エラーカウンタクリア |
| sift | 2:9d69f27a3d3b | 168 | else |
| sift | 2:9d69f27a3d3b | 169 | errCounter.apsUnderVolt += tmpApsErrCountU; |
| sift | 2:9d69f27a3d3b | 170 | |
| sift | 2:9d69f27a3d3b | 171 | //センサー偏差チェック |
| sift | 2:9d69f27a3d3b | 172 | if(myAbs(tmpApsP - tmpApsS) > APS_DEVIATION_TOLERANCE) { //偏差チェックには補正後の値(tmp)を使用 |
| sift | 2:9d69f27a3d3b | 173 | errCounter.apsErrorTolerance++; |
| sift | 2:9d69f27a3d3b | 174 | } else { |
| sift | 2:9d69f27a3d3b | 175 | errCounter.apsErrorTolerance = 0; |
| sift | 2:9d69f27a3d3b | 176 | } |
| sift | 1:4d86ec2fe4b1 | 177 | |
| sift | 2:9d69f27a3d3b | 178 | //小さい方にクリップ |
| sift | 2:9d69f27a3d3b | 179 | //APS値は好きな方を使いな |
| sift | 2:9d69f27a3d3b | 180 | if(tmpApsP > tmpApsS) { |
| sift | 2:9d69f27a3d3b | 181 | tmpApsP = tmpApsS; |
| sift | 2:9d69f27a3d3b | 182 | } else { |
| sift | 2:9d69f27a3d3b | 183 | tmpApsS = tmpApsP; |
| sift | 2:9d69f27a3d3b | 184 | } |
| sift | 2:9d69f27a3d3b | 185 | |
| sift | 2:9d69f27a3d3b | 186 | //Brake上限値チェック |
| sift | 30:c596a0f5d685 | 187 | if(tmpBrake > BRK_OFF_VOLTAGE + ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 188 | errCounter.brakeExceedVolt++; |
| sift | 30:c596a0f5d685 | 189 | tmpBrake = BRK_OFF_VOLTAGE; |
| sift | 2:9d69f27a3d3b | 190 | } else { |
| sift | 2:9d69f27a3d3b | 191 | errCounter.brakeExceedVolt = 0; |
| sift | 2:9d69f27a3d3b | 192 | } |
| sift | 2:9d69f27a3d3b | 193 | |
| sift | 2:9d69f27a3d3b | 194 | //Brake下限値チェック |
| sift | 30:c596a0f5d685 | 195 | if(tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 196 | errCounter.brakeUnderVolt++; |
| sift | 30:c596a0f5d685 | 197 | tmpBrake = BRK_ON_VOLTAGE; |
| sift | 2:9d69f27a3d3b | 198 | } else { |
| sift | 2:9d69f27a3d3b | 199 | errCounter.brakeUnderVolt = 0; |
| sift | 2:9d69f27a3d3b | 200 | } |
| sift | 1:4d86ec2fe4b1 | 201 | |
| sift | 2:9d69f27a3d3b | 202 | //brake範囲外電圧チェック |
| sift | 30:c596a0f5d685 | 203 | if((tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE)) { |
| sift | 2:9d69f27a3d3b | 204 | errCounter.brakeFuzzyVolt++; |
| sift | 2:9d69f27a3d3b | 205 | tmpBrake = BRK_OFF_VOLTAGE; |
| sift | 2:9d69f27a3d3b | 206 | } else { |
| sift | 2:9d69f27a3d3b | 207 | errCounter.brakeFuzzyVolt=0; |
| sift | 2:9d69f27a3d3b | 208 | } |
| sift | 2:9d69f27a3d3b | 209 | |
| sift | 2:9d69f27a3d3b | 210 | //APS固着チェック |
| sift | 2:9d69f27a3d3b | 211 | if((preApsP == tmpApsP) && (tmpApsP == APS_MAX_POSITION)) |
| sift | 2:9d69f27a3d3b | 212 | errCounter.apsStick++; |
| sift | 2:9d69f27a3d3b | 213 | else |
| sift | 2:9d69f27a3d3b | 214 | errCounter.apsStick=0; |
| sift | 2:9d69f27a3d3b | 215 | |
| sift | 2:9d69f27a3d3b | 216 | //ブレーキオーバーライドチェック |
| sift | 12:ae291fa7239c | 217 | if((isBrakeOn() == 1) && (tmpApsP >= APS_OVERRIDE25)) //Brake-ON and APS > 25% |
| sift | 2:9d69f27a3d3b | 218 | errCounter.brakeOverRide++; |
| sift | 41:0c53acd31247 | 219 | if(tmpApsP < APS_OVERRIDE05) //APS < 5% |
| sift | 2:9d69f27a3d3b | 220 | errCounter.brakeOverRide=0; |
| sift | 2:9d69f27a3d3b | 221 | |
| sift | 2:9d69f27a3d3b | 222 | //センサ値取得 |
| sift | 2:9d69f27a3d3b | 223 | gApsP = tmpApsP; |
| sift | 2:9d69f27a3d3b | 224 | gApsS = tmpApsS; |
| sift | 2:9d69f27a3d3b | 225 | gBrake = tmpBrake; |
| sift | 2:9d69f27a3d3b | 226 | |
| sift | 2:9d69f27a3d3b | 227 | //未来の自分に期待 |
| sift | 2:9d69f27a3d3b | 228 | preApsP = rawApsP; |
| sift | 2:9d69f27a3d3b | 229 | preApsS = rawApsS; |
| sift | 2:9d69f27a3d3b | 230 | preBrake = rawBrake; |
| sift | 2:9d69f27a3d3b | 231 | } |
| sift | 1:4d86ec2fe4b1 | 232 | } |
| sift | 1:4d86ec2fe4b1 | 233 | |
| sift | 39:c05074379713 | 234 | //車輪速計測は”【空転再粘着制御】山下道寛”を参照のこと(一部改修) |
| sift | 39:c05074379713 | 235 | //パルス数カウンタ |
| sift | 25:c21d35c7f0de | 236 | volatile int gRightMotorPulseCounter = 0, gLeftMotorPulseCounter = 0; |
| sift | 39:c05074379713 | 237 | volatile int gRightWheelPulseCounter = 0, gLeftWheelPulseCounter = 0; |
| sift | 39:c05074379713 | 238 | //パルス入力までの時間 |
| sift | 39:c05074379713 | 239 | volatile int gRightWheelPulse_dT2 = 0, gLeftWheelPulse_dT2 = 0; |
| sift | 39:c05074379713 | 240 | volatile float gRightWheelRPS = 0, gLeftWheelRPS = 0; |
| sift | 39:c05074379713 | 241 | |
| sift | 7:ad013d88a539 | 242 | volatile bool pulseTimeISRFlag = false; |
| sift | 1:4d86ec2fe4b1 | 243 | |
| sift | 39:c05074379713 | 244 | /***********************************/ |
| sift | 39:c05074379713 | 245 | //モータパルスカウント |
| sift | 25:c21d35c7f0de | 246 | void countRightMotorPulseISR(void) |
| sift | 1:4d86ec2fe4b1 | 247 | { |
| sift | 25:c21d35c7f0de | 248 | gRightMotorPulseCounter++; |
| sift | 1:4d86ec2fe4b1 | 249 | } |
| sift | 1:4d86ec2fe4b1 | 250 | |
| sift | 25:c21d35c7f0de | 251 | void countLeftMotorPulseISR(void) |
| sift | 1:4d86ec2fe4b1 | 252 | { |
| sift | 25:c21d35c7f0de | 253 | gLeftMotorPulseCounter++; |
| sift | 2:9d69f27a3d3b | 254 | } |
| sift | 39:c05074379713 | 255 | /***********************************/ |
| sift | 39:c05074379713 | 256 | |
| sift | 39:c05074379713 | 257 | /***********************************/ |
| sift | 39:c05074379713 | 258 | //ホイールパルスカウント |
| sift | 39:c05074379713 | 259 | void countRightWheelPulseISR(void) |
| sift | 39:c05074379713 | 260 | { |
| sift | 39:c05074379713 | 261 | gRightWheelPulseCounter++; //パルス数カウント |
| sift | 39:c05074379713 | 262 | gRightWheelPulse_dT2 = wheelPulseTimer.read_us(); //現在の時間いただきます |
| sift | 39:c05074379713 | 263 | } |
| sift | 39:c05074379713 | 264 | |
| sift | 39:c05074379713 | 265 | void countLeftWheelPulseISR(void) |
| sift | 39:c05074379713 | 266 | { |
| sift | 39:c05074379713 | 267 | } |
| sift | 39:c05074379713 | 268 | /***********************************/ |
| sift | 39:c05074379713 | 269 | |
| sift | 39:c05074379713 | 270 | void measRpsISR(void) |
| sift | 39:c05074379713 | 271 | { |
| sift | 39:c05074379713 | 272 | int rwpCounter = gRightWheelPulseCounter; |
| sift | 39:c05074379713 | 273 | static int rwp_dT1 = MAX_WHEEL_PULSE_TIME_US; //初期設定は無限時間前にパルス入力があったと仮定 |
| sift | 39:c05074379713 | 274 | int rwp_dT2 = gRightWheelPulse_dT2; |
| sift | 41:0c53acd31247 | 275 | static bool preInputState = false; |
| sift | 41:0c53acd31247 | 276 | static int rwpStopCounter = 0; //タイヤが止まっている間カウントアップ |
| sift | 42:3ab09d0e3071 | 277 | static int currentTime = 0; |
| sift | 42:3ab09d0e3071 | 278 | static float preRightWheelRPS = 0.0f; |
| sift | 39:c05074379713 | 279 | |
| sift | 42:3ab09d0e3071 | 280 | //前回パルス入力がない場合 |
| sift | 42:3ab09d0e3071 | 281 | if(preInputState == false) { |
| sift | 42:3ab09d0e3071 | 282 | //以前のdT1に前回の計測周期の時間を積算 |
| sift | 42:3ab09d0e3071 | 283 | rwp_dT1 = rwp_dT1 + currentTime; |
| sift | 42:3ab09d0e3071 | 284 | } |
| sift | 42:3ab09d0e3071 | 285 | |
| sift | 42:3ab09d0e3071 | 286 | //現在の時間取得 |
| sift | 42:3ab09d0e3071 | 287 | currentTime = wheelPulseTimer.read_us(); |
| sift | 42:3ab09d0e3071 | 288 | |
| sift | 42:3ab09d0e3071 | 289 | //次回計測周期までのパルス時間計測開始 |
| sift | 42:3ab09d0e3071 | 290 | wheelPulseTimer.reset(); |
| sift | 40:8e33c60c6590 | 291 | //パルス数クリア |
| sift | 40:8e33c60c6590 | 292 | gRightWheelPulseCounter = 0; |
| sift | 40:8e33c60c6590 | 293 | gLeftWheelPulseCounter = 0; |
| sift | 40:8e33c60c6590 | 294 | //dT2の初期値はパルス入力ない状態 => 計測時間=0 |
| sift | 40:8e33c60c6590 | 295 | gRightWheelPulse_dT2 = 0; |
| sift | 41:0c53acd31247 | 296 | |
| sift | 42:3ab09d0e3071 | 297 | //overflow防止処理 |
| sift | 42:3ab09d0e3071 | 298 | if(rwp_dT1 > MAX_WHEEL_PULSE_TIME_US) |
| sift | 42:3ab09d0e3071 | 299 | rwp_dT1 = MAX_WHEEL_PULSE_TIME_US; |
| sift | 41:0c53acd31247 | 300 | |
| sift | 41:0c53acd31247 | 301 | //パルス入力あれば直前のパルス入力からの経過時間取得 |
| sift | 41:0c53acd31247 | 302 | if(rwpCounter != 0) { |
| sift | 42:3ab09d0e3071 | 303 | rwp_dT2 = currentTime - rwp_dT2; |
| sift | 39:c05074379713 | 304 | } |
| sift | 41:0c53acd31247 | 305 | |
| sift | 42:3ab09d0e3071 | 306 | //ピーク値保持したい |
| sift | 41:0c53acd31247 | 307 | //パルス入力ない場合---(設定回数未満)前回値保持/(設定回数以上)疑似パルス入力判定 |
| sift | 41:0c53acd31247 | 308 | if(rwpCounter == 0) { |
| sift | 42:3ab09d0e3071 | 309 | if(rwpStopCounter < 5) { |
| sift | 41:0c53acd31247 | 310 | rwpStopCounter++; |
| sift | 41:0c53acd31247 | 311 | } else { |
| sift | 42:3ab09d0e3071 | 312 | gRightWheelRPS = (((float)rwpCounter / ((currentTime + rwp_dT1 - rwp_dT2) / 1000000.0f)) / (WHEEL_PULSE_NUM*4)) * 0.27f + (1.0f-0.27f)*preRightWheelRPS; |
| sift | 40:8e33c60c6590 | 313 | } |
| sift | 41:0c53acd31247 | 314 | } else { |
| sift | 41:0c53acd31247 | 315 | //RPS計算[rps](1sec当たりパルス数/タイヤパルス数) |
| sift | 42:3ab09d0e3071 | 316 | gRightWheelRPS = (((float)rwpCounter / ((currentTime + rwp_dT1 - rwp_dT2) / 1000000.0f)) / (WHEEL_PULSE_NUM*4)) * 0.27f + (1.0f-0.27f)*preRightWheelRPS; |
| sift | 41:0c53acd31247 | 317 | rwpStopCounter = 0; |
| sift | 41:0c53acd31247 | 318 | } |
| sift | 41:0c53acd31247 | 319 | |
| sift | 42:3ab09d0e3071 | 320 | // gRightWheelRPS = (((float)rwpCounter / ((currentTime + rwp_dT1 - rwp_dT2) / 1000000.0f)) / (WHEEL_PULSE_NUM*4)) * 0.2f + (1.0f-0.2f)*preRightWheelRPS; |
| sift | 42:3ab09d0e3071 | 321 | preRightWheelRPS = gRightWheelRPS; |
| sift | 42:3ab09d0e3071 | 322 | |
| sift | 39:c05074379713 | 323 | //パルス入力あれば次回のdT1はdT2を採用(パルス入力なければ現在値保持) |
| sift | 39:c05074379713 | 324 | if(rwpCounter != 0) |
| sift | 40:8e33c60c6590 | 325 | rwp_dT1 = rwp_dT2; |
| sift | 41:0c53acd31247 | 326 | |
| sift | 41:0c53acd31247 | 327 | if(rwpCounter == 0) |
| sift | 41:0c53acd31247 | 328 | preInputState = false; |
| sift | 41:0c53acd31247 | 329 | else |
| sift | 41:0c53acd31247 | 330 | preInputState = true; |
| sift | 39:c05074379713 | 331 | } |
| sift | 2:9d69f27a3d3b | 332 | |
| sift | 25:c21d35c7f0de | 333 | void getPulseCounterISR(void) |
| sift | 2:9d69f27a3d3b | 334 | { |
| sift | 7:ad013d88a539 | 335 | pulseTimeISRFlag = true; |
| sift | 2:9d69f27a3d3b | 336 | } |
| sift | 2:9d69f27a3d3b | 337 | |
| sift | 28:47e9531a3a9d | 338 | //default argument : switchWheel=false |
| sift | 28:47e9531a3a9d | 339 | int getRPS(Select rl, bool switchWheel) |
| sift | 2:9d69f27a3d3b | 340 | { |
| sift | 37:ba10cf09c151 | 341 | static int rightMotorPulse[100]= {0}, leftMotorPulse[100]= {0}; //過去1.0秒間のパルス数格納 |
| sift | 25:c21d35c7f0de | 342 | static int sumRightMotorPulse, sumLeftMotorPulse; |
| sift | 28:47e9531a3a9d | 343 | float rps; |
| sift | 2:9d69f27a3d3b | 344 | |
| sift | 7:ad013d88a539 | 345 | if(pulseTimeISRFlag == true) { |
| sift | 37:ba10cf09c151 | 346 | for(int i=99; i>0; i--) { |
| sift | 25:c21d35c7f0de | 347 | rightMotorPulse[i] = rightMotorPulse[i-1]; |
| sift | 25:c21d35c7f0de | 348 | leftMotorPulse[i] = leftMotorPulse[i-1]; |
| sift | 25:c21d35c7f0de | 349 | } |
| sift | 25:c21d35c7f0de | 350 | |
| sift | 25:c21d35c7f0de | 351 | rightMotorPulse[0] = gRightMotorPulseCounter; |
| sift | 25:c21d35c7f0de | 352 | leftMotorPulse[0] = gLeftMotorPulseCounter; |
| sift | 2:9d69f27a3d3b | 353 | |
| sift | 25:c21d35c7f0de | 354 | gRightMotorPulseCounter = 0; |
| sift | 25:c21d35c7f0de | 355 | gLeftMotorPulseCounter = 0; |
| sift | 10:87ad65eef0e9 | 356 | |
| sift | 25:c21d35c7f0de | 357 | sumRightMotorPulse = 0; |
| sift | 25:c21d35c7f0de | 358 | sumLeftMotorPulse = 0; |
| sift | 25:c21d35c7f0de | 359 | |
| sift | 37:ba10cf09c151 | 360 | for(int i=0; i<100; i++) { |
| sift | 25:c21d35c7f0de | 361 | sumRightMotorPulse += rightMotorPulse[i]; |
| sift | 25:c21d35c7f0de | 362 | sumLeftMotorPulse += leftMotorPulse[i]; |
| sift | 25:c21d35c7f0de | 363 | } |
| sift | 28:47e9531a3a9d | 364 | pulseTimeISRFlag = false; |
| sift | 2:9d69f27a3d3b | 365 | } |
| sift | 2:9d69f27a3d3b | 366 | |
| sift | 31:042c08a7434f | 367 | if(switchWheel == false) { |
| sift | 28:47e9531a3a9d | 368 | if(rl == RIGHT) |
| sift | 37:ba10cf09c151 | 369 | rps = (float)1.0f*sumRightMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO; //過去0.5秒間のモータパルス数を使ってRPS算出 |
| sift | 28:47e9531a3a9d | 370 | else |
| sift | 37:ba10cf09c151 | 371 | rps = (float)1.0f*sumLeftMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO; //過去0.5秒間のモータパルス数を使ってRPS算出 |
| sift | 28:47e9531a3a9d | 372 | } else { |
| sift | 28:47e9531a3a9d | 373 | //こっちはタイヤ回転数 |
| sift | 28:47e9531a3a9d | 374 | //そのうち対応 |
| sift | 28:47e9531a3a9d | 375 | if(rl == RIGHT) |
| sift | 39:c05074379713 | 376 | rps = gRightWheelRPS; //過去1秒間のモータパルス数を使ってRPS算出 |
| sift | 28:47e9531a3a9d | 377 | else |
| sift | 39:c05074379713 | 378 | rps = gLeftWheelRPS; //過去1秒間のモータパルス数を使ってRPS算出 |
| sift | 28:47e9531a3a9d | 379 | } |
| sift | 28:47e9531a3a9d | 380 | return (int)(rps / LSB_MOTORSPEED); //LSB変換 |
| sift | 2:9d69f27a3d3b | 381 | } |
| sift | 2:9d69f27a3d3b | 382 | |
| sift | 2:9d69f27a3d3b | 383 | float getVelocity(void) |
| sift | 2:9d69f27a3d3b | 384 | { |
| sift | 38:11753ee9734f | 385 | // static float debugVelocity = 0.0f; |
| sift | 38:11753ee9734f | 386 | // debugVelocity += 0.002; |
| sift | 38:11753ee9734f | 387 | // |
| sift | 38:11753ee9734f | 388 | // printf("%1.2f\r\n", debugVelocity); |
| sift | 38:11753ee9734f | 389 | // |
| sift | 38:11753ee9734f | 390 | // return debugVelocity; |
| sift | 32:9688c30ac38b | 391 | return (0.5f*TIRE_DIAMETER*2*M_PI*(getRPS(RIGHT) + getRPS(LEFT))*0.5f)*LSB_MOTORSPEED; |
| sift | 38:11753ee9734f | 392 | // return 15.0f; |
| sift | 1:4d86ec2fe4b1 | 393 | } |
| sift | 1:4d86ec2fe4b1 | 394 | |
| sift | 34:594ddb4008b2 | 395 | int distributeTorque_omega(float steeringWheelAngle) |
| sift | 21:bbf2ad7e6602 | 396 | { |
| sift | 21:bbf2ad7e6602 | 397 | static float lastSteering=0.0f; |
| sift | 22:95c1f753ecad | 398 | float omega=0; |
| sift | 22:95c1f753ecad | 399 | int disTrq=0; |
| sift | 30:c596a0f5d685 | 400 | |
| sift | 34:594ddb4008b2 | 401 | steeringWheelAngle = ratioLPF * steeringWheelAngle + (1.0f - ratioLPF) * lastSteering; |
| sift | 21:bbf2ad7e6602 | 402 | |
| sift | 34:594ddb4008b2 | 403 | omega = steeringWheelAngle - lastSteering; //舵角の差分算出 |
| sift | 21:bbf2ad7e6602 | 404 | omega /= 0.01f; //制御周期で角速度演算 |
| sift | 36:dc33a3a194c9 | 405 | |
| sift | 22:95c1f753ecad | 406 | if(myAbs(omega) < 0.349f) { //20deg/s |
| sift | 21:bbf2ad7e6602 | 407 | disTrq = 0; |
| sift | 22:95c1f753ecad | 408 | } else if(myAbs(omega) <= 8.727f) { //500deg/s |
| sift | 34:594ddb4008b2 | 409 | disTrq = (int)(MAX_DISTRIBUTION_TORQUE_OMEGA / (8.727f-0.349f) * (myAbs(omega) - 0.349f)); |
| sift | 21:bbf2ad7e6602 | 410 | } else |
| sift | 34:594ddb4008b2 | 411 | disTrq = (int)MAX_DISTRIBUTION_TORQUE_OMEGA; |
| sift | 22:95c1f753ecad | 412 | |
| sift | 34:594ddb4008b2 | 413 | lastSteering = steeringWheelAngle; |
| sift | 21:bbf2ad7e6602 | 414 | |
| sift | 34:594ddb4008b2 | 415 | if(omega < 0) |
| sift | 22:95c1f753ecad | 416 | disTrq = -disTrq; |
| sift | 22:95c1f753ecad | 417 | |
| sift | 21:bbf2ad7e6602 | 418 | return disTrq; |
| sift | 21:bbf2ad7e6602 | 419 | } |
| sift | 21:bbf2ad7e6602 | 420 | |
| sift | 31:042c08a7434f | 421 | int distributeTorque(float steeringWheelAngle, float velocity) |
| sift | 2:9d69f27a3d3b | 422 | { |
| sift | 31:042c08a7434f | 423 | double V2 = velocity * velocity; |
| sift | 31:042c08a7434f | 424 | double R = 0.0; //旋回半径 |
| sift | 31:042c08a7434f | 425 | double Gy = 0.0; //横G |
| sift | 31:042c08a7434f | 426 | double deadband = 0.0; |
| sift | 32:9688c30ac38b | 427 | double steeringAngle = (double)steeringWheelAngle * STEER_RATIO; |
| sift | 31:042c08a7434f | 428 | double steeringSign = 1.0; |
| sift | 9:220e4e77e056 | 429 | int disTrq = 0; |
| sift | 25:c21d35c7f0de | 430 | |
| sift | 31:042c08a7434f | 431 | if(steeringAngle > 0) |
| sift | 34:594ddb4008b2 | 432 | steeringSign = 1.0; |
| sift | 25:c21d35c7f0de | 433 | else |
| sift | 34:594ddb4008b2 | 434 | steeringSign = -1.0; |
| sift | 25:c21d35c7f0de | 435 | |
| sift | 31:042c08a7434f | 436 | steeringAngle = myAbs(steeringAngle); |
| sift | 32:9688c30ac38b | 437 | |
| sift | 38:11753ee9734f | 438 | if(steeringAngle <= 0.0001) |
| sift | 31:042c08a7434f | 439 | steeringAngle = 0.0001; |
| sift | 2:9d69f27a3d3b | 440 | |
| sift | 31:042c08a7434f | 441 | R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle; //理論旋回半径 計算 |
| sift | 32:9688c30ac38b | 442 | Gy = V2 / R / 9.81; //理論横G |
| sift | 31:042c08a7434f | 443 | |
| sift | 34:594ddb4008b2 | 444 | if(Gy <= deadband) |
| sift | 9:220e4e77e056 | 445 | disTrq = 0; |
| sift | 34:594ddb4008b2 | 446 | else if(Gy <= 1.5) { |
| sift | 31:042c08a7434f | 447 | Gy -= deadband; |
| sift | 34:594ddb4008b2 | 448 | disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (1.5 - deadband) * Gy); |
| sift | 25:c21d35c7f0de | 449 | } else { |
| sift | 19:571a4d00b89c | 450 | disTrq = MAX_DISTRIBUTION_TORQUE; |
| sift | 25:c21d35c7f0de | 451 | } |
| sift | 2:9d69f27a3d3b | 452 | |
| sift | 36:dc33a3a194c9 | 453 | return (int)(disTrq * steeringSign); |
| sift | 2:9d69f27a3d3b | 454 | } |
| sift | 2:9d69f27a3d3b | 455 | |
| sift | 25:c21d35c7f0de | 456 | //rpm +++++ モータ回転数 |
| sift | 25:c21d35c7f0de | 457 | //regSwitch +++++ 回生=1 力行=0 |
| sift | 25:c21d35c7f0de | 458 | inline int calcMaxTorque(int rpm, bool regSwitch) |
| sift | 2:9d69f27a3d3b | 459 | { |
| sift | 25:c21d35c7f0de | 460 | int maxTrq=0; |
| sift | 25:c21d35c7f0de | 461 | |
| sift | 25:c21d35c7f0de | 462 | //後で削除 |
| sift | 25:c21d35c7f0de | 463 | rpm = 2000; |
| sift | 26:331e77bb479b | 464 | //++++++++++++++++++++ |
| sift | 25:c21d35c7f0de | 465 | |
| sift | 25:c21d35c7f0de | 466 | if(regSwitch == 0) { |
| sift | 25:c21d35c7f0de | 467 | if(rpm <3000) |
| sift | 25:c21d35c7f0de | 468 | maxTrq = MAX_MOTOR_TORQUE_POWER; |
| sift | 25:c21d35c7f0de | 469 | else if(rpm <= 11000) |
| sift | 25:c21d35c7f0de | 470 | maxTrq = maxTorqueMap[(int)(rpm / 10.0)]; |
| sift | 25:c21d35c7f0de | 471 | else |
| sift | 25:c21d35c7f0de | 472 | maxTrq = MAX_REVOLUTION_TORQUE_POWER; |
| sift | 25:c21d35c7f0de | 473 | } else { |
| sift | 28:47e9531a3a9d | 474 | if(rpm < 600) { |
| sift | 25:c21d35c7f0de | 475 | maxTrq = 0; |
| sift | 28:47e9531a3a9d | 476 | } else if(rpm < 1250) { |
| sift | 28:47e9531a3a9d | 477 | //+++++++++++++++++++++++++++++++++++++ |
| sift | 28:47e9531a3a9d | 478 | //暫定処理 今後回生トルクマップも要作成 |
| sift | 28:47e9531a3a9d | 479 | maxTrq = 0; |
| sift | 28:47e9531a3a9d | 480 | //+++++++++++++++++++++++++++++++++++++ |
| sift | 25:c21d35c7f0de | 481 | } else if(rpm <= 6000) { |
| sift | 26:331e77bb479b | 482 | maxTrq = MAX_MOTOR_TORQUE_REGENERATIVE; |
| sift | 26:331e77bb479b | 483 | } else if(rpm <= 11000) { |
| sift | 25:c21d35c7f0de | 484 | //+++++++++++++++++++++++++++++++++++++ |
| sift | 25:c21d35c7f0de | 485 | //暫定処理 今後回生トルクマップも要作成 |
| sift | 26:331e77bb479b | 486 | maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE; |
| sift | 25:c21d35c7f0de | 487 | //+++++++++++++++++++++++++++++++++++++ |
| sift | 25:c21d35c7f0de | 488 | } else { |
| sift | 25:c21d35c7f0de | 489 | maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE; |
| sift | 25:c21d35c7f0de | 490 | } |
| sift | 25:c21d35c7f0de | 491 | } |
| sift | 25:c21d35c7f0de | 492 | return maxTrq; |
| sift | 2:9d69f27a3d3b | 493 | } |
| sift | 2:9d69f27a3d3b | 494 | |
| sift | 25:c21d35c7f0de | 495 | //演算方法 |
| sift | 25:c21d35c7f0de | 496 | //y = a(x - b) + c |
| sift | 25:c21d35c7f0de | 497 | //x = 1/a * (y + ab - c) |
| sift | 25:c21d35c7f0de | 498 | unsigned int calcTorqueToVoltage(int reqTorque, int rpm) |
| sift | 2:9d69f27a3d3b | 499 | { |
| sift | 25:c21d35c7f0de | 500 | float slope = 0; //a |
| sift | 25:c21d35c7f0de | 501 | int startPoint = 0; //b |
| sift | 25:c21d35c7f0de | 502 | int intercept = 0; //c |
| sift | 12:ae291fa7239c | 503 | |
| sift | 25:c21d35c7f0de | 504 | int outputVoltage=0; |
| sift | 16:7afd623ef48a | 505 | |
| sift | 25:c21d35c7f0de | 506 | if(reqTorque > LINEAR_REGION_TORQUE_POWER) { //力行トルクがrpmに対して非線形となる領域 |
| sift | 25:c21d35c7f0de | 507 | slope = (float)(calcMaxTorque(rpm, 0) - LINEAR_REGION_TORQUE_POWER)/(DACOUTPUT_MAX - LINEAR_REGION_VOLTAGE_POWER); |
| sift | 25:c21d35c7f0de | 508 | startPoint = LINEAR_REGION_VOLTAGE_POWER; |
| sift | 25:c21d35c7f0de | 509 | intercept = LINEAR_REGION_TORQUE_POWER; |
| sift | 25:c21d35c7f0de | 510 | |
| sift | 25:c21d35c7f0de | 511 | outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope); |
| sift | 6:26fa8c78500e | 512 | |
| sift | 25:c21d35c7f0de | 513 | } else if(reqTorque > 0) { //力行トルクがrpmに対して線形となる領域 |
| sift | 25:c21d35c7f0de | 514 | slope = (float)LINEAR_REGION_TORQUE_POWER/(LINEAR_REGION_VOLTAGE_POWER - ZERO_TORQUE_VOLTAGE_P); |
| sift | 25:c21d35c7f0de | 515 | startPoint = ZERO_TORQUE_VOLTAGE_P; |
| sift | 25:c21d35c7f0de | 516 | intercept = 0; |
| sift | 25:c21d35c7f0de | 517 | |
| sift | 25:c21d35c7f0de | 518 | outputVoltage = (int)(reqTorque/slope + startPoint); |
| sift | 25:c21d35c7f0de | 519 | |
| sift | 25:c21d35c7f0de | 520 | } else if(0 == reqTorque) { |
| sift | 25:c21d35c7f0de | 521 | |
| sift | 25:c21d35c7f0de | 522 | outputVoltage = ZERO_TORQUE_VOLTAGE_NEUTRAL; //ニュートラル信号 |
| sift | 6:26fa8c78500e | 523 | |
| sift | 25:c21d35c7f0de | 524 | } else if(reqTorque > LINEAR_REGION_TORQUE_REGENERATIVE) { //回生トルクがrpmに対して線形となる領域 |
| sift | 25:c21d35c7f0de | 525 | slope = (float)(0 - LINEAR_REGION_TORQUE_REGENERATIVE)/(ZERO_TORQUE_VOLTAGE_REG - LINEAR_REGION_VOLTAGE_REGENERATIVE); |
| sift | 25:c21d35c7f0de | 526 | startPoint = LINEAR_REGION_VOLTAGE_REGENERATIVE; |
| sift | 25:c21d35c7f0de | 527 | intercept = LINEAR_REGION_TORQUE_REGENERATIVE; |
| sift | 25:c21d35c7f0de | 528 | |
| sift | 25:c21d35c7f0de | 529 | outputVoltage = (int)(reqTorque/slope + startPoint); |
| sift | 25:c21d35c7f0de | 530 | |
| sift | 25:c21d35c7f0de | 531 | } else { //回生トルクがrpmに対して非線形となる領域 |
| sift | 25:c21d35c7f0de | 532 | slope = (float)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/(LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN); |
| sift | 25:c21d35c7f0de | 533 | startPoint = DACOUTPUT_MIN; |
| sift | 25:c21d35c7f0de | 534 | intercept = calcMaxTorque(rpm, 1); |
| sift | 25:c21d35c7f0de | 535 | |
| sift | 25:c21d35c7f0de | 536 | outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope); |
| sift | 2:9d69f27a3d3b | 537 | } |
| sift | 2:9d69f27a3d3b | 538 | |
| sift | 25:c21d35c7f0de | 539 | if(outputVoltage > DACOUTPUT_MAX) |
| sift | 25:c21d35c7f0de | 540 | outputVoltage = DACOUTPUT_MAX; |
| sift | 25:c21d35c7f0de | 541 | if(outputVoltage < DACOUTPUT_MIN) |
| sift | 25:c21d35c7f0de | 542 | outputVoltage = DACOUTPUT_MIN; |
| sift | 16:7afd623ef48a | 543 | |
| sift | 20:3c5061281a7a | 544 | return (unsigned int)(0xFFF*((double)outputVoltage/0xFFFF)); //DACの分解能に適応(16bit->12bit) |
| sift | 2:9d69f27a3d3b | 545 | } |
| sift | 2:9d69f27a3d3b | 546 | |
| sift | 2:9d69f27a3d3b | 547 | int calcRequestTorque(void) |
| sift | 2:9d69f27a3d3b | 548 | { |
| sift | 2:9d69f27a3d3b | 549 | int currentAPS; |
| sift | 2:9d69f27a3d3b | 550 | int requestTorque; |
| sift | 2:9d69f27a3d3b | 551 | |
| sift | 2:9d69f27a3d3b | 552 | currentAPS = ((gApsP>gApsS) ? gApsS : gApsP); //センサ値は小さい方を採用 |
| sift | 12:ae291fa7239c | 553 | |
| sift | 2:9d69f27a3d3b | 554 | if(currentAPS < APS_MIN_POSITION) |
| sift | 2:9d69f27a3d3b | 555 | currentAPS = 0; |
| sift | 2:9d69f27a3d3b | 556 | else |
| sift | 2:9d69f27a3d3b | 557 | currentAPS -= APS_MIN_POSITION; //オフセット修正 |
| sift | 2:9d69f27a3d3b | 558 | |
| sift | 25:c21d35c7f0de | 559 | if(currentAPS <= APS_REG_RANGE) //デッドバンド内であれば要求トルク->0 |
| sift | 25:c21d35c7f0de | 560 | requestTorque = (int)((float)(-MAX_OUTPUT_TORQUE_REGENERATIVE) / APS_REG_RANGE * currentAPS + MAX_OUTPUT_TORQUE_REGENERATIVE); |
| sift | 2:9d69f27a3d3b | 561 | else |
| sift | 25:c21d35c7f0de | 562 | requestTorque = (int)((float)MAX_OUTPUT_TORQUE_POWER / APS_PWR_RANGE * (currentAPS - APS_REG_RANGE)); |
| sift | 2:9d69f27a3d3b | 563 | |
| sift | 25:c21d35c7f0de | 564 | if(requestTorque > MAX_OUTPUT_TORQUE_POWER) |
| sift | 25:c21d35c7f0de | 565 | requestTorque = MAX_OUTPUT_TORQUE_POWER; |
| sift | 25:c21d35c7f0de | 566 | else if(requestTorque < MAX_OUTPUT_TORQUE_REGENERATIVE) |
| sift | 25:c21d35c7f0de | 567 | requestTorque = MAX_OUTPUT_TORQUE_REGENERATIVE; |
| sift | 2:9d69f27a3d3b | 568 | |
| sift | 12:ae291fa7239c | 569 | if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1)) |
| sift | 12:ae291fa7239c | 570 | requestTorque = 0; |
| sift | 12:ae291fa7239c | 571 | |
| sift | 2:9d69f27a3d3b | 572 | return requestTorque; |
| sift | 2:9d69f27a3d3b | 573 | } |
| sift | 2:9d69f27a3d3b | 574 | |
| sift | 17:a2246ce3333f | 575 | //トルク配分車速制限関数 |
| sift | 17:a2246ce3333f | 576 | //車速が低速域の場合，トルク配分0 |
| sift | 17:a2246ce3333f | 577 | float limitTorqueDistribution(void) |
| sift | 17:a2246ce3333f | 578 | { |
| sift | 17:a2246ce3333f | 579 | float limitRate; |
| sift | 17:a2246ce3333f | 580 | float currentVelocity = getVelocity() * 3.6f; //km/hで車速取得 |
| sift | 20:3c5061281a7a | 581 | |
| sift | 37:ba10cf09c151 | 582 | if(currentVelocity < 15.0f) |
| sift | 17:a2246ce3333f | 583 | limitRate = 0.0f; |
| sift | 36:dc33a3a194c9 | 584 | else if(currentVelocity < 30.0f) |
| sift | 38:11753ee9734f | 585 | limitRate = (currentVelocity - 15.0f) / (30.0f - 15.0f); |
| sift | 17:a2246ce3333f | 586 | else |
| sift | 17:a2246ce3333f | 587 | limitRate = 1.0f; |
| sift | 36:dc33a3a194c9 | 588 | |
| sift | 35:b75595b1da36 | 589 | //printf("rate:%1.3f\r\n", limitRate); |
| sift | 36:dc33a3a194c9 | 590 | |
| sift | 17:a2246ce3333f | 591 | return limitRate; |
| sift | 17:a2246ce3333f | 592 | } |
| sift | 17:a2246ce3333f | 593 | |
| sift | 26:331e77bb479b | 594 | void driveTVD(int TVDmode, bool isRedyToDrive) |
| sift | 1:4d86ec2fe4b1 | 595 | { |
| sift | 6:26fa8c78500e | 596 | int requestTorque=0; //ドライバー要求トルク |
| sift | 6:26fa8c78500e | 597 | int distributionTrq=0; //分配トルク |
| sift | 21:bbf2ad7e6602 | 598 | int disTrq_omega=0; |
| sift | 21:bbf2ad7e6602 | 599 | int torqueRight, torqueLeft; //トルクの右左 |
| sift | 20:3c5061281a7a | 600 | static unsigned int preMcpA=0, preMcpB=0; |
| sift | 42:3ab09d0e3071 | 601 | float tempRWP = gRightWheelRPS; |
| sift | 20:3c5061281a7a | 602 | |
| sift | 2:9d69f27a3d3b | 603 | loadSensors(); //APS,BRAKE更新 |
| sift | 2:9d69f27a3d3b | 604 | loadSteerAngle(); //舵角更新 |
| sift | 41:0c53acd31247 | 605 | |
| sift | 41:0c53acd31247 | 606 | // printf("%04d\r\n",gRightWheelPulseCounter); |
| sift | 42:3ab09d0e3071 | 607 | printf("%f %f %f\r\n",tempRWP*60.0f, 300.0f, 0.0f); |
| sift | 41:0c53acd31247 | 608 | // printf("%09d %09d %09d\r\n",rwp_dT1, rwp_dT2, rwpCounter); |
| sift | 12:ae291fa7239c | 609 | |
| sift | 26:331e77bb479b | 610 | if(isRedyToDrive && isBrakeOn()) |
| sift | 12:ae291fa7239c | 611 | readyToDriveFlag = 0; |
| sift | 2:9d69f27a3d3b | 612 | |
| sift | 12:ae291fa7239c | 613 | if((errCounter.apsUnderVolt > ERRCOUNTER_DECISION) |
| sift | 12:ae291fa7239c | 614 | || (errCounter.apsExceedVolt > ERRCOUNTER_DECISION) |
| sift | 12:ae291fa7239c | 615 | || (errCounter.apsErrorTolerance > ERRCOUNTER_DECISION) |
| sift | 12:ae291fa7239c | 616 | // || (errCounter.apsStick > ERRCOUNTER_DECISION) |
| sift | 30:c596a0f5d685 | 617 | || (errCounter.brakeUnderVolt > ERRCOUNTER_DECISION) |
| sift | 30:c596a0f5d685 | 618 | || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION) |
| sift | 30:c596a0f5d685 | 619 | || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION) |
| sift | 12:ae291fa7239c | 620 | ) { |
| sift | 16:7afd623ef48a | 621 | readyToDriveFlag = 1; |
| sift | 12:ae291fa7239c | 622 | } |
| sift | 16:7afd623ef48a | 623 | |
| sift | 12:ae291fa7239c | 624 | indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION)); |
| sift | 20:3c5061281a7a | 625 | LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION); |
| sift | 16:7afd623ef48a | 626 | |
| sift | 6:26fa8c78500e | 627 | requestTorque=calcRequestTorque(); //ドライバー要求トルク取得 |
| sift | 4:d7778cde0aff | 628 | |
| sift | 37:ba10cf09c151 | 629 | distributionTrq = (int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity())*limitTorqueDistribution()) / 2.0f); //片モーターのトルク分配量計算 |
| sift | 38:11753ee9734f | 630 | disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f)*limitTorqueDistribution()) / 2.0f); //微分制御 |
| sift | 12:ae291fa7239c | 631 | |
| sift | 34:594ddb4008b2 | 632 | //printf("%d\r\n", distributionTrq); |
| sift | 34:594ddb4008b2 | 633 | |
| sift | 38:11753ee9734f | 634 | // distributionTrq = 0; |
| sift | 35:b75595b1da36 | 635 | disTrq_omega = 0; |
| sift | 31:042c08a7434f | 636 | |
| sift | 25:c21d35c7f0de | 637 | torqueRight = requestTorque + distributionTrq; |
| sift | 25:c21d35c7f0de | 638 | torqueLeft = requestTorque - distributionTrq; |
| sift | 21:bbf2ad7e6602 | 639 | |
| sift | 22:95c1f753ecad | 640 | torqueRight += disTrq_omega; |
| sift | 22:95c1f753ecad | 641 | torqueLeft -= disTrq_omega; |
| sift | 20:3c5061281a7a | 642 | |
| sift | 38:11753ee9734f | 643 | if(torqueRight < 0) { |
| sift | 38:11753ee9734f | 644 | if((getRPS(RIGHT) * LSB_MOTORSPEED * 60.0f) < 600.0f) { |
| sift | 38:11753ee9734f | 645 | torqueLeft = requestTorque + torqueRight; |
| sift | 38:11753ee9734f | 646 | torqueRight = 0; |
| sift | 38:11753ee9734f | 647 | } else if((getRPS(RIGHT) * LSB_MOTORSPEED * 60.0f) <= 1250.0f) { |
| sift | 38:11753ee9734f | 648 | torqueLeft = requestTorque + torqueRight*((getRPS(RIGHT) * GEAR_RATIO * LSB_MOTORSPEED * 60.0f - 600.0f)/(1250.0f - 600.0f)); |
| sift | 38:11753ee9734f | 649 | torqueRight = torqueRight*((getRPS(RIGHT)-600.0f)/(1250.0f - 600.0f)); |
| sift | 38:11753ee9734f | 650 | } |
| sift | 38:11753ee9734f | 651 | } |
| sift | 38:11753ee9734f | 652 | if(torqueLeft < 0) { |
| sift | 38:11753ee9734f | 653 | if((getRPS(LEFT) * LSB_MOTORSPEED * 60.0f) < 600.0f) { |
| sift | 38:11753ee9734f | 654 | torqueRight = requestTorque + torqueLeft; |
| sift | 38:11753ee9734f | 655 | torqueLeft = 0; |
| sift | 38:11753ee9734f | 656 | } else if((getRPS(LEFT) * LSB_MOTORSPEED * 60.0f) <= 1250.0f) { |
| sift | 38:11753ee9734f | 657 | torqueRight = requestTorque + torqueLeft*((getRPS(LEFT) * GEAR_RATIO * LSB_MOTORSPEED * 60.0f - 600.0f)/(1250.0f - 600.0f)); |
| sift | 38:11753ee9734f | 658 | torqueLeft = torqueLeft*((getRPS(LEFT)-600.0f)/(1250.0f - 600.0f)); |
| sift | 38:11753ee9734f | 659 | } |
| sift | 38:11753ee9734f | 660 | } |
| sift | 38:11753ee9734f | 661 | |
| sift | 26:331e77bb479b | 662 | //アクセルべた踏みでトルクMAX、旋回より駆動を優先(加速番長モード) |
| sift | 26:331e77bb479b | 663 | if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ |
| sift | 26:331e77bb479b | 664 | torqueLeft = MAX_OUTPUT_TORQUE_POWER; |
| sift | 26:331e77bb479b | 665 | torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque); |
| sift | 16:7afd623ef48a | 666 | } |
| sift | 26:331e77bb479b | 667 | if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ |
| sift | 26:331e77bb479b | 668 | torqueRight = MAX_OUTPUT_TORQUE_POWER; |
| sift | 26:331e77bb479b | 669 | torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque); |
| sift | 26:331e77bb479b | 670 | } |
| sift | 34:594ddb4008b2 | 671 | |
| sift | 34:594ddb4008b2 | 672 | gRightMotorTorque = torqueRight; |
| sift | 34:594ddb4008b2 | 673 | gLeftMotorTorque = torqueLeft; |
| sift | 34:594ddb4008b2 | 674 | |
| sift | 28:47e9531a3a9d | 675 | McpData.valA = calcTorqueToVoltage(torqueRight, getRPS(RIGHT)); |
| sift | 28:47e9531a3a9d | 676 | McpData.valB = calcTorqueToVoltage(torqueLeft, getRPS(LEFT)); |
| sift | 16:7afd623ef48a | 677 | |
| sift | 38:11753ee9734f | 678 | // preMcpA = (unsigned int)(McpData.valA * 0.456 + preMcpA * 0.544); |
| sift | 38:11753ee9734f | 679 | // preMcpB = (unsigned int)(McpData.valB * 0.456 + preMcpB * 0.544); |
| sift | 38:11753ee9734f | 680 | preMcpA = McpData.valA; |
| sift | 38:11753ee9734f | 681 | preMcpB = McpData.valB; |
| sift | 20:3c5061281a7a | 682 | |
| sift | 20:3c5061281a7a | 683 | mcp.writeA(preMcpA); //右モーター |
| sift | 20:3c5061281a7a | 684 | mcp.writeB(preMcpB); //左モーター |
| sift | 1:4d86ec2fe4b1 | 685 | } |
| sift | 1:4d86ec2fe4b1 | 686 | |
| sift | 1:4d86ec2fe4b1 | 687 | void initTVD(void) |
| sift | 1:4d86ec2fe4b1 | 688 | { |
| sift | 39:c05074379713 | 689 | wheelPulseTimer.reset(); |
| sift | 39:c05074379713 | 690 | |
| sift | 39:c05074379713 | 691 | wheelPulseTimer.start(); |
| sift | 1:4d86ec2fe4b1 | 692 | |
| sift | 25:c21d35c7f0de | 693 | rightMotorPulse.fall(&countRightMotorPulseISR); |
| sift | 25:c21d35c7f0de | 694 | leftMotorPulse.fall(&countLeftMotorPulseISR); |
| sift | 39:c05074379713 | 695 | rightWheelPulse1.fall(&countRightWheelPulseISR); |
| sift | 42:3ab09d0e3071 | 696 | rightWheelPulse2.fall(&countRightWheelPulseISR); |
| sift | 42:3ab09d0e3071 | 697 | rightWheelPulse1.rise(&countRightWheelPulseISR); |
| sift | 42:3ab09d0e3071 | 698 | rightWheelPulse2.rise(&countRightWheelPulseISR); |
| sift | 39:c05074379713 | 699 | leftWheelPulse1.fall(&countLeftWheelPulseISR); |
| sift | 25:c21d35c7f0de | 700 | |
| sift | 25:c21d35c7f0de | 701 | ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S); //制御周期毎にデータ読み込み(LPF演算のため) |
| sift | 40:8e33c60c6590 | 702 | // ticker2.attach(&getPulseCounterISR, CONTROL_CYCLE_S); // |
| sift | 40:8e33c60c6590 | 703 | ticker2.attach(&measRpsISR, TIRE_MEAS_CYCLE_US / 1000000.0f); // |
| sift | 10:87ad65eef0e9 | 704 | |
| sift | 10:87ad65eef0e9 | 705 | mcp.writeA(0); //右モーター |
| sift | 10:87ad65eef0e9 | 706 | mcp.writeB(0); //左モーター |
| sift | 28:47e9531a3a9d | 707 | |
| sift | 30:c596a0f5d685 | 708 | printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_POWER); |
| sift | 30:c596a0f5d685 | 709 | printf("MAX OUTPUT REG-TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_REGENERATIVE); |
| sift | 30:c596a0f5d685 | 710 | printf("MAX DISTRIBUTION TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_DISTRIBUTION_TORQUE); |
| sift | 30:c596a0f5d685 | 711 | printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MIN_INNERWHEEL_MOTOR_TORQUE); |
| sift | 1:4d86ec2fe4b1 | 712 | } |