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@57:02b7178cd083, 2018-01-09 (annotated)
- Committer:
- sift
- Date:
- Tue Jan 09 06:14:28 2018 +0000
- Revision:
- 57:02b7178cd083
- Parent:
- 56:78bd99bf995a
- Child:
- 58:ac3d182732c9
?????????; ????????????
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 | 51:640198055ed6 | 5 | #include "float.h" |
| sift | 1:4d86ec2fe4b1 | 6 | |
| sift | 1:4d86ec2fe4b1 | 7 | extern AnalogIn apsP; |
| sift | 1:4d86ec2fe4b1 | 8 | extern AnalogIn apsS; |
| sift | 2:9d69f27a3d3b | 9 | extern AnalogIn brake; |
| sift | 1:4d86ec2fe4b1 | 10 | extern DigitalOut LED[]; |
| sift | 27:37a8b4f6d28d | 11 | extern DigitalOut brakeSignal; |
| sift | 27:37a8b4f6d28d | 12 | extern DigitalOut indicatorLed; |
| sift | 27:37a8b4f6d28d | 13 | extern DigitalOut shutDown; |
| sift | 27:37a8b4f6d28d | 14 | extern DigitalIn sdState; |
| sift | 1:4d86ec2fe4b1 | 15 | extern InterruptIn rightMotorPulse; |
| sift | 1:4d86ec2fe4b1 | 16 | extern InterruptIn leftMotorPulse; |
| sift | 39:c05074379713 | 17 | extern InterruptIn rightWheelPulse1; |
| sift | 39:c05074379713 | 18 | extern InterruptIn rightWheelPulse2; |
| sift | 39:c05074379713 | 19 | extern InterruptIn leftWheelPulse1; |
| sift | 39:c05074379713 | 20 | extern InterruptIn leftWheelPulse2; |
| sift | 1:4d86ec2fe4b1 | 21 | extern MCP4922 mcp; |
| sift | 8:a22aec357a64 | 22 | extern Serial pc; |
| sift | 19:571a4d00b89c | 23 | extern AnalogOut STR2AN; |
| sift | 27:37a8b4f6d28d | 24 | extern CAN can; |
| sift | 1:4d86ec2fe4b1 | 25 | |
| sift | 26:331e77bb479b | 26 | #define indicateSystem(x) (indicatorLed.write(x)) |
| sift | 26:331e77bb479b | 27 | |
| sift | 43:5da6b1574227 | 28 | Timer wheelPulseTimer[4]; |
| sift | 2:9d69f27a3d3b | 29 | Ticker ticker1; |
| sift | 2:9d69f27a3d3b | 30 | Ticker ticker2; |
| sift | 1:4d86ec2fe4b1 | 31 | |
| sift | 2:9d69f27a3d3b | 32 | #define apsPVol() (apsP.read() * 3.3) |
| sift | 2:9d69f27a3d3b | 33 | #define apsSVol() (apsS.read() * 3.3) |
| sift | 0:276c1dab2d62 | 34 | |
| sift | 0:276c1dab2d62 | 35 | struct { |
| sift | 0:276c1dab2d62 | 36 | unsigned int valA:12; |
| sift | 0:276c1dab2d62 | 37 | unsigned int valB:12; |
| sift | 2:9d69f27a3d3b | 38 | } McpData; |
| sift | 1:4d86ec2fe4b1 | 39 | |
| sift | 2:9d69f27a3d3b | 40 | //各変数が一定値を超えた時点でエラー検出とする |
| sift | 2:9d69f27a3d3b | 41 | //2つのAPSの区別はつけないことにする |
| sift | 12:ae291fa7239c | 42 | struct errCounter_t errCounter= {0,0,0,0,0,0,0}; |
| sift | 1:4d86ec2fe4b1 | 43 | |
| sift | 12:ae291fa7239c | 44 | int readyToDriveFlag = 1; |
| sift | 12:ae291fa7239c | 45 | |
| sift | 12:ae291fa7239c | 46 | int gApsP=0, gApsS=0, gBrake=0; //現在のセンサ値 |
| sift | 12:ae291fa7239c | 47 | int rawApsP=0, rawApsS=0, rawBrake=0; //現在の補正無しのセンサ値 |
| sift | 2:9d69f27a3d3b | 48 | |
| sift | 28:47e9531a3a9d | 49 | int gRightMotorTorque=0, gLeftMotorTorque=0; |
| sift | 28:47e9531a3a9d | 50 | |
| sift | 43:5da6b1574227 | 51 | int getMotorTorque(select_t rl) |
| sift | 28:47e9531a3a9d | 52 | { |
| sift | 43:5da6b1574227 | 53 | return ((rl==RL_MOTOR) ? gLeftMotorTorque : gRightMotorTorque); |
| sift | 28:47e9531a3a9d | 54 | } |
| sift | 28:47e9531a3a9d | 55 | |
| sift | 18:b7c362c8f0fd | 56 | //エラーカウンタ外部参照用関数 |
| sift | 18:b7c362c8f0fd | 57 | //errCounter_t型変数のポインタを引数に取る |
| sift | 2:9d69f27a3d3b | 58 | void getCurrentErrCount(struct errCounter_t *ptr) |
| sift | 1:4d86ec2fe4b1 | 59 | { |
| sift | 12:ae291fa7239c | 60 | ptr->apsUnderVolt = errCounter.apsUnderVolt; |
| sift | 12:ae291fa7239c | 61 | ptr->apsExceedVolt = errCounter.apsExceedVolt; |
| sift | 12:ae291fa7239c | 62 | ptr->apsErrorTolerance = errCounter.apsErrorTolerance; |
| sift | 12:ae291fa7239c | 63 | ptr->apsStick = errCounter.apsStick; |
| sift | 12:ae291fa7239c | 64 | ptr->brakeUnderVolt = errCounter.brakeUnderVolt; |
| sift | 12:ae291fa7239c | 65 | ptr->brakeExceedVolt = errCounter.brakeExceedVolt; |
| sift | 12:ae291fa7239c | 66 | ptr->brakeFuzzyVolt = errCounter.brakeFuzzyVolt; |
| sift | 12:ae291fa7239c | 67 | ptr->brakeOverRide = errCounter.brakeOverRide; |
| sift | 12:ae291fa7239c | 68 | } |
| sift | 12:ae291fa7239c | 69 | |
| sift | 18:b7c362c8f0fd | 70 | //ブレーキONOFF判定関数 |
| sift | 12:ae291fa7239c | 71 | //Brake-ON:1 Brake-OFF:0 |
| sift | 12:ae291fa7239c | 72 | int isBrakeOn(void) |
| sift | 12:ae291fa7239c | 73 | { |
| sift | 12:ae291fa7239c | 74 | int brake = gBrake; |
| sift | 12:ae291fa7239c | 75 | int brakeOnOff = 0; |
| sift | 12:ae291fa7239c | 76 | |
| sift | 30:c596a0f5d685 | 77 | if(brake < (BRK_ON_VOLTAGE + ERROR_TOLERANCE)) |
| sift | 12:ae291fa7239c | 78 | brakeOnOff = 1; |
| sift | 30:c596a0f5d685 | 79 | if(brake > (BRK_OFF_VOLTAGE - ERROR_TOLERANCE)) |
| sift | 12:ae291fa7239c | 80 | brakeOnOff = 0; |
| sift | 12:ae291fa7239c | 81 | |
| sift | 12:ae291fa7239c | 82 | return brakeOnOff; |
| sift | 2:9d69f27a3d3b | 83 | } |
| sift | 1:4d86ec2fe4b1 | 84 | |
| sift | 18:b7c362c8f0fd | 85 | //センサ現在値外部参照関数 |
| sift | 7:ad013d88a539 | 86 | int getCurrentSensor(int sensor) |
| sift | 2:9d69f27a3d3b | 87 | { |
| sift | 2:9d69f27a3d3b | 88 | switch (sensor) { |
| sift | 2:9d69f27a3d3b | 89 | case APS_PRIMARY: |
| sift | 2:9d69f27a3d3b | 90 | return gApsP; |
| sift | 2:9d69f27a3d3b | 91 | case APS_SECONDARY: |
| sift | 2:9d69f27a3d3b | 92 | return gApsS; |
| sift | 2:9d69f27a3d3b | 93 | case BRAKE: |
| sift | 2:9d69f27a3d3b | 94 | return gBrake; |
| sift | 2:9d69f27a3d3b | 95 | default: |
| sift | 2:9d69f27a3d3b | 96 | return -1; |
| sift | 1:4d86ec2fe4b1 | 97 | } |
| sift | 2:9d69f27a3d3b | 98 | } |
| sift | 2:9d69f27a3d3b | 99 | |
| sift | 18:b7c362c8f0fd | 100 | //補正前センサ現在値外部参照関数 |
| sift | 7:ad013d88a539 | 101 | int getRawSensor(int sensor) |
| sift | 2:9d69f27a3d3b | 102 | { |
| sift | 2:9d69f27a3d3b | 103 | switch (sensor) { |
| sift | 2:9d69f27a3d3b | 104 | case APS_PRIMARY: |
| sift | 2:9d69f27a3d3b | 105 | return rawApsP; |
| sift | 2:9d69f27a3d3b | 106 | case APS_SECONDARY: |
| sift | 2:9d69f27a3d3b | 107 | return rawApsS; |
| sift | 2:9d69f27a3d3b | 108 | case BRAKE: |
| sift | 2:9d69f27a3d3b | 109 | return rawBrake; |
| sift | 2:9d69f27a3d3b | 110 | default: |
| sift | 2:9d69f27a3d3b | 111 | return -1; |
| sift | 1:4d86ec2fe4b1 | 112 | } |
| sift | 2:9d69f27a3d3b | 113 | } |
| sift | 2:9d69f27a3d3b | 114 | |
| sift | 44:d433bb5f77c0 | 115 | volatile bool loadSensorFlag = false; |
| sift | 2:9d69f27a3d3b | 116 | |
| sift | 2:9d69f27a3d3b | 117 | //タイマー割り込みでコールされる |
| sift | 2:9d69f27a3d3b | 118 | void loadSensorsISR(void) |
| sift | 2:9d69f27a3d3b | 119 | { |
| sift | 2:9d69f27a3d3b | 120 | loadSensorFlag = true; |
| sift | 1:4d86ec2fe4b1 | 121 | } |
| sift | 1:4d86ec2fe4b1 | 122 | |
| sift | 25:c21d35c7f0de | 123 | //センサ読み込み関数 |
| sift | 2:9d69f27a3d3b | 124 | void loadSensors(void) |
| sift | 1:4d86ec2fe4b1 | 125 | { |
| sift | 2:9d69f27a3d3b | 126 | if(true == loadSensorFlag) { |
| sift | 2:9d69f27a3d3b | 127 | loadSensorFlag = false; |
| sift | 2:9d69f27a3d3b | 128 | static int preApsP=0, preApsS=0; //過去のセンサ値 |
| sift | 2:9d69f27a3d3b | 129 | static int preBrake=0; |
| sift | 2:9d69f27a3d3b | 130 | int tmpApsP=0, tmpApsS=0, tmpBrake=0; //補正後のセンサ値 |
| sift | 2:9d69f27a3d3b | 131 | int tmpApsErrCountU=0, tmpApsErrCountE=0; //APSの一時的なエラーカウンタ |
| sift | 44:d433bb5f77c0 | 132 | int tmpRawApsP, tmpRawApsS, tmpRawBrake; |
| sift | 2:9d69f27a3d3b | 133 | |
| sift | 44:d433bb5f77c0 | 134 | tmpRawApsP = (int)apsP.read_u16(); |
| sift | 44:d433bb5f77c0 | 135 | tmpRawApsS = (int)apsS.read_u16(); |
| sift | 44:d433bb5f77c0 | 136 | tmpRawBrake = (int)brake.read_u16(); |
| sift | 46:16f1a7a01f5f | 137 | |
| sift | 2:9d69f27a3d3b | 138 | //Low Pass Filter |
| sift | 45:8e5d35beb957 | 139 | tmpApsP = (int)(tmpRawApsP * ratioLPF_ACC_BRK + preApsP * (1.0-ratioLPF_ACC_BRK)); |
| sift | 45:8e5d35beb957 | 140 | tmpApsS = (int)(tmpRawApsS * ratioLPF_ACC_BRK + preApsS * (1.0-ratioLPF_ACC_BRK)); |
| sift | 45:8e5d35beb957 | 141 | tmpBrake = (int)(tmpRawBrake * ratioLPF_ACC_BRK + preBrake * (1.0-ratioLPF_ACC_BRK)); |
| sift | 2:9d69f27a3d3b | 142 | |
| sift | 2:9d69f27a3d3b | 143 | //生のセンサ値取得 |
| sift | 2:9d69f27a3d3b | 144 | rawApsP = tmpApsP; |
| sift | 2:9d69f27a3d3b | 145 | rawApsS = tmpApsS; |
| sift | 2:9d69f27a3d3b | 146 | rawBrake = tmpBrake; |
| sift | 2:9d69f27a3d3b | 147 | |
| sift | 2:9d69f27a3d3b | 148 | //センサーチェック |
| sift | 2:9d69f27a3d3b | 149 | //APS上限値チェック |
| sift | 2:9d69f27a3d3b | 150 | if(tmpApsP > APS_MAX_POSITION + ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 151 | tmpApsP = APS_MAX_POSITION; //異常時，上限値にクリップ |
| sift | 2:9d69f27a3d3b | 152 | tmpApsErrCountE++; |
| sift | 2:9d69f27a3d3b | 153 | } |
| sift | 2:9d69f27a3d3b | 154 | if(tmpApsS > APS_MAX_POSITION + ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 155 | tmpApsS = APS_MAX_POSITION; //異常時，上限値にクリップ |
| sift | 2:9d69f27a3d3b | 156 | tmpApsErrCountE++; |
| sift | 2:9d69f27a3d3b | 157 | } |
| sift | 2:9d69f27a3d3b | 158 | if(0 == tmpApsErrCountE) |
| sift | 2:9d69f27a3d3b | 159 | errCounter.apsExceedVolt = 0; //どちらも正常時エラーカウンタクリア |
| sift | 2:9d69f27a3d3b | 160 | else |
| sift | 2:9d69f27a3d3b | 161 | errCounter.apsExceedVolt += tmpApsErrCountE; |
| sift | 2:9d69f27a3d3b | 162 | |
| sift | 2:9d69f27a3d3b | 163 | //APS下限値チェック |
| sift | 2:9d69f27a3d3b | 164 | if(tmpApsP < APS_MIN_POSITION - ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 165 | tmpApsP = APS_MIN_POSITION; //下限値にクリップ |
| sift | 2:9d69f27a3d3b | 166 | tmpApsErrCountU++; |
| sift | 2:9d69f27a3d3b | 167 | } |
| sift | 2:9d69f27a3d3b | 168 | if(tmpApsS < APS_MIN_POSITION - ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 169 | tmpApsS = APS_MIN_POSITION; //下限値にクリップ |
| sift | 2:9d69f27a3d3b | 170 | tmpApsErrCountU++; |
| sift | 2:9d69f27a3d3b | 171 | } |
| sift | 2:9d69f27a3d3b | 172 | if(0 == tmpApsErrCountU) |
| sift | 2:9d69f27a3d3b | 173 | errCounter.apsUnderVolt = 0; //どちらも正常時エラーカウンタクリア |
| sift | 2:9d69f27a3d3b | 174 | else |
| sift | 2:9d69f27a3d3b | 175 | errCounter.apsUnderVolt += tmpApsErrCountU; |
| sift | 2:9d69f27a3d3b | 176 | |
| sift | 2:9d69f27a3d3b | 177 | //センサー偏差チェック |
| sift | 2:9d69f27a3d3b | 178 | if(myAbs(tmpApsP - tmpApsS) > APS_DEVIATION_TOLERANCE) { //偏差チェックには補正後の値(tmp)を使用 |
| sift | 2:9d69f27a3d3b | 179 | errCounter.apsErrorTolerance++; |
| sift | 2:9d69f27a3d3b | 180 | } else { |
| sift | 2:9d69f27a3d3b | 181 | errCounter.apsErrorTolerance = 0; |
| sift | 2:9d69f27a3d3b | 182 | } |
| sift | 1:4d86ec2fe4b1 | 183 | |
| sift | 2:9d69f27a3d3b | 184 | //小さい方にクリップ |
| sift | 2:9d69f27a3d3b | 185 | //APS値は好きな方を使いな |
| sift | 2:9d69f27a3d3b | 186 | if(tmpApsP > tmpApsS) { |
| sift | 2:9d69f27a3d3b | 187 | tmpApsP = tmpApsS; |
| sift | 2:9d69f27a3d3b | 188 | } else { |
| sift | 2:9d69f27a3d3b | 189 | tmpApsS = tmpApsP; |
| sift | 2:9d69f27a3d3b | 190 | } |
| sift | 2:9d69f27a3d3b | 191 | |
| sift | 2:9d69f27a3d3b | 192 | //Brake上限値チェック |
| sift | 30:c596a0f5d685 | 193 | if(tmpBrake > BRK_OFF_VOLTAGE + ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 194 | errCounter.brakeExceedVolt++; |
| sift | 30:c596a0f5d685 | 195 | tmpBrake = BRK_OFF_VOLTAGE; |
| sift | 2:9d69f27a3d3b | 196 | } else { |
| sift | 2:9d69f27a3d3b | 197 | errCounter.brakeExceedVolt = 0; |
| sift | 2:9d69f27a3d3b | 198 | } |
| sift | 2:9d69f27a3d3b | 199 | |
| sift | 2:9d69f27a3d3b | 200 | //Brake下限値チェック |
| sift | 30:c596a0f5d685 | 201 | if(tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) { |
| sift | 2:9d69f27a3d3b | 202 | errCounter.brakeUnderVolt++; |
| sift | 30:c596a0f5d685 | 203 | tmpBrake = BRK_ON_VOLTAGE; |
| sift | 2:9d69f27a3d3b | 204 | } else { |
| sift | 2:9d69f27a3d3b | 205 | errCounter.brakeUnderVolt = 0; |
| sift | 2:9d69f27a3d3b | 206 | } |
| sift | 1:4d86ec2fe4b1 | 207 | |
| sift | 2:9d69f27a3d3b | 208 | //brake範囲外電圧チェック |
| sift | 30:c596a0f5d685 | 209 | if((tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE)) { |
| sift | 2:9d69f27a3d3b | 210 | errCounter.brakeFuzzyVolt++; |
| sift | 2:9d69f27a3d3b | 211 | tmpBrake = BRK_OFF_VOLTAGE; |
| sift | 2:9d69f27a3d3b | 212 | } else { |
| sift | 2:9d69f27a3d3b | 213 | errCounter.brakeFuzzyVolt=0; |
| sift | 2:9d69f27a3d3b | 214 | } |
| sift | 2:9d69f27a3d3b | 215 | |
| sift | 2:9d69f27a3d3b | 216 | //APS固着チェック |
| sift | 2:9d69f27a3d3b | 217 | if((preApsP == tmpApsP) && (tmpApsP == APS_MAX_POSITION)) |
| sift | 2:9d69f27a3d3b | 218 | errCounter.apsStick++; |
| sift | 2:9d69f27a3d3b | 219 | else |
| sift | 2:9d69f27a3d3b | 220 | errCounter.apsStick=0; |
| sift | 2:9d69f27a3d3b | 221 | |
| sift | 2:9d69f27a3d3b | 222 | //ブレーキオーバーライドチェック |
| sift | 12:ae291fa7239c | 223 | if((isBrakeOn() == 1) && (tmpApsP >= APS_OVERRIDE25)) //Brake-ON and APS > 25% |
| sift | 2:9d69f27a3d3b | 224 | errCounter.brakeOverRide++; |
| sift | 41:0c53acd31247 | 225 | if(tmpApsP < APS_OVERRIDE05) //APS < 5% |
| sift | 2:9d69f27a3d3b | 226 | errCounter.brakeOverRide=0; |
| sift | 2:9d69f27a3d3b | 227 | |
| sift | 2:9d69f27a3d3b | 228 | //センサ値取得 |
| sift | 2:9d69f27a3d3b | 229 | gApsP = tmpApsP; |
| sift | 2:9d69f27a3d3b | 230 | gApsS = tmpApsS; |
| sift | 2:9d69f27a3d3b | 231 | gBrake = tmpBrake; |
| sift | 2:9d69f27a3d3b | 232 | |
| sift | 2:9d69f27a3d3b | 233 | //未来の自分に期待 |
| sift | 2:9d69f27a3d3b | 234 | preApsP = rawApsP; |
| sift | 2:9d69f27a3d3b | 235 | preApsS = rawApsS; |
| sift | 2:9d69f27a3d3b | 236 | preBrake = rawBrake; |
| sift | 2:9d69f27a3d3b | 237 | } |
| sift | 1:4d86ec2fe4b1 | 238 | } |
| sift | 1:4d86ec2fe4b1 | 239 | |
| sift | 43:5da6b1574227 | 240 | //******************************************************* |
| sift | 39:c05074379713 | 241 | //車輪速計測は”【空転再粘着制御】山下道寛”を参照のこと(一部改修) |
| sift | 43:5da6b1574227 | 242 | //******************************************************* |
| sift | 43:5da6b1574227 | 243 | //wheelNumbler:ホイール識別番号 |
| sift | 43:5da6b1574227 | 244 | //FR:0 FL:1 RR:2 RL:3 |
| sift | 43:5da6b1574227 | 245 | typedef struct { |
| sift | 43:5da6b1574227 | 246 | int counter; |
| sift | 43:5da6b1574227 | 247 | int dT1; |
| sift | 43:5da6b1574227 | 248 | int dT2; |
| sift | 43:5da6b1574227 | 249 | bool preInputState; |
| sift | 43:5da6b1574227 | 250 | int stopCounter; |
| sift | 46:16f1a7a01f5f | 251 | double preRps; |
| sift | 43:5da6b1574227 | 252 | } rps_t; |
| sift | 43:5da6b1574227 | 253 | |
| sift | 39:c05074379713 | 254 | //パルス数カウンタ |
| sift | 43:5da6b1574227 | 255 | volatile int gWheelPulseCounter[4] = {0}; |
| sift | 39:c05074379713 | 256 | //パルス入力までの時間 |
| sift | 43:5da6b1574227 | 257 | volatile int gWheelPulse_dT2[4] = {0}; |
| sift | 43:5da6b1574227 | 258 | //現在の回転数[rps] |
| sift | 43:5da6b1574227 | 259 | float gRps[4] = {0}; |
| sift | 43:5da6b1574227 | 260 | //車輪速計測周期フラグ |
| sift | 43:5da6b1574227 | 261 | volatile bool gloadWheelRpsFlag = false; |
| sift | 39:c05074379713 | 262 | |
| sift | 43:5da6b1574227 | 263 | //*********************************** |
| sift | 39:c05074379713 | 264 | //モータパルスカウント |
| sift | 25:c21d35c7f0de | 265 | void countRightMotorPulseISR(void) |
| sift | 1:4d86ec2fe4b1 | 266 | { |
| sift | 43:5da6b1574227 | 267 | gWheelPulseCounter[RR_MOTOR]++; |
| sift | 43:5da6b1574227 | 268 | gWheelPulse_dT2[RR_MOTOR] = wheelPulseTimer[RR_MOTOR].read_us(); //現在の時間いただきます |
| sift | 1:4d86ec2fe4b1 | 269 | } |
| sift | 25:c21d35c7f0de | 270 | void countLeftMotorPulseISR(void) |
| sift | 1:4d86ec2fe4b1 | 271 | { |
| sift | 43:5da6b1574227 | 272 | gWheelPulseCounter[RL_MOTOR]++; |
| sift | 43:5da6b1574227 | 273 | gWheelPulse_dT2[RL_MOTOR] = wheelPulseTimer[RL_MOTOR].read_us(); //現在の時間いただきます |
| sift | 2:9d69f27a3d3b | 274 | } |
| sift | 43:5da6b1574227 | 275 | //*********************************** |
| sift | 39:c05074379713 | 276 | //ホイールパルスカウント |
| sift | 39:c05074379713 | 277 | void countRightWheelPulseISR(void) |
| sift | 39:c05074379713 | 278 | { |
| sift | 43:5da6b1574227 | 279 | gWheelPulseCounter[FR_WHEEL]++; |
| sift | 43:5da6b1574227 | 280 | gWheelPulse_dT2[FR_WHEEL] = wheelPulseTimer[FR_WHEEL].read_us(); //現在の時間いただきます |
| sift | 39:c05074379713 | 281 | } |
| sift | 39:c05074379713 | 282 | void countLeftWheelPulseISR(void) |
| sift | 39:c05074379713 | 283 | { |
| sift | 43:5da6b1574227 | 284 | gWheelPulseCounter[FL_WHEEL]++; |
| sift | 43:5da6b1574227 | 285 | gWheelPulse_dT2[FL_WHEEL] = wheelPulseTimer[FL_WHEEL].read_us(); //現在の時間いただきます |
| sift | 39:c05074379713 | 286 | } |
| sift | 43:5da6b1574227 | 287 | //*********************************** |
| sift | 41:0c53acd31247 | 288 | |
| sift | 43:5da6b1574227 | 289 | //RPS読み込み許可設定関数 |
| sift | 43:5da6b1574227 | 290 | void loadRpsISR(void) |
| sift | 43:5da6b1574227 | 291 | { |
| sift | 43:5da6b1574227 | 292 | gloadWheelRpsFlag = true; |
| sift | 39:c05074379713 | 293 | } |
| sift | 2:9d69f27a3d3b | 294 | |
| sift | 43:5da6b1574227 | 295 | //RPS読み込み関数 |
| sift | 43:5da6b1574227 | 296 | void loadRps(void) |
| sift | 2:9d69f27a3d3b | 297 | { |
| sift | 46:16f1a7a01f5f | 298 | const rps_t INITRPS = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0}; //構造体初期化用定数 |
| sift | 45:8e5d35beb957 | 299 | static rps_t rps[4] = {INITRPS, INITRPS, INITRPS, INITRPS}; |
| sift | 43:5da6b1574227 | 300 | static int currentTime[4] = {0}; |
| sift | 43:5da6b1574227 | 301 | float pulseNumPerRev; |
| sift | 43:5da6b1574227 | 302 | |
| sift | 43:5da6b1574227 | 303 | if(false == gloadWheelRpsFlag) |
| sift | 43:5da6b1574227 | 304 | return; |
| sift | 43:5da6b1574227 | 305 | else |
| sift | 43:5da6b1574227 | 306 | gloadWheelRpsFlag = false; |
| sift | 43:5da6b1574227 | 307 | |
| sift | 43:5da6b1574227 | 308 | for(int i=0; i<4; i++) { |
| sift | 43:5da6b1574227 | 309 | rps[i].counter = gWheelPulseCounter[i]; |
| sift | 43:5da6b1574227 | 310 | rps[i].dT2 = gWheelPulse_dT2[i]; |
| sift | 2:9d69f27a3d3b | 311 | |
| sift | 43:5da6b1574227 | 312 | //前回パルス入力がない場合 |
| sift | 43:5da6b1574227 | 313 | if(rps[i].preInputState == false) { |
| sift | 43:5da6b1574227 | 314 | //以前のdT1に前回の計測周期の時間を積算 |
| sift | 43:5da6b1574227 | 315 | rps[i].dT1 = rps[i].dT1 + currentTime[i]; |
| sift | 43:5da6b1574227 | 316 | //overflow防止処理 |
| sift | 43:5da6b1574227 | 317 | if(rps[i].dT1 > MAX_WHEEL_PULSE_TIME_US) |
| sift | 43:5da6b1574227 | 318 | rps[i].dT1 = MAX_WHEEL_PULSE_TIME_US; |
| sift | 43:5da6b1574227 | 319 | } |
| sift | 43:5da6b1574227 | 320 | |
| sift | 43:5da6b1574227 | 321 | //現在の時間取得 |
| sift | 43:5da6b1574227 | 322 | currentTime[i] = wheelPulseTimer[i].read_us(); |
| sift | 43:5da6b1574227 | 323 | |
| sift | 43:5da6b1574227 | 324 | //次回計測周期までのパルス時間計測開始 |
| sift | 43:5da6b1574227 | 325 | wheelPulseTimer[i].reset(); |
| sift | 43:5da6b1574227 | 326 | //パルス数クリア |
| sift | 43:5da6b1574227 | 327 | gWheelPulseCounter[i] = 0; |
| sift | 43:5da6b1574227 | 328 | //dT2の初期値はパルス入力ない状態 => 計測時間=0 |
| sift | 43:5da6b1574227 | 329 | gWheelPulse_dT2[i] = 0; |
| sift | 43:5da6b1574227 | 330 | |
| sift | 43:5da6b1574227 | 331 | //一回転当たりのパルス数設定 |
| sift | 45:8e5d35beb957 | 332 | if(i <= FL_WHEEL) |
| sift | 43:5da6b1574227 | 333 | pulseNumPerRev = WHEEL_PULSE_NUM; //Front車輪パルス数*割込み回数 |
| sift | 43:5da6b1574227 | 334 | else |
| sift | 43:5da6b1574227 | 335 | pulseNumPerRev = MOTOR_PULSE_NUM; //モータパルス数*割込み回数 |
| sift | 43:5da6b1574227 | 336 | |
| sift | 43:5da6b1574227 | 337 | //パルス入力あれば直前のパルス入力からの経過時間取得 |
| sift | 43:5da6b1574227 | 338 | if(rps[i].counter != 0) { |
| sift | 43:5da6b1574227 | 339 | rps[i].dT2 = currentTime[i] - rps[i].dT2; |
| sift | 25:c21d35c7f0de | 340 | } |
| sift | 25:c21d35c7f0de | 341 | |
| sift | 43:5da6b1574227 | 342 | //パルス入力ない場合---(設定回数未満)前回値保持/(設定回数以上)疑似パルス入力判定 (ピーク値を保存したい) |
| sift | 43:5da6b1574227 | 343 | if(rps[i].counter == 0) { |
| sift | 55:e9ca699bec57 | 344 | if(rps[i].stopCounter < 5) //低回転数時、急に0rpsと演算しないように前回値保持(設定値はだいたい) |
| sift | 43:5da6b1574227 | 345 | rps[i].stopCounter++; |
| sift | 43:5da6b1574227 | 346 | else |
| sift | 46:16f1a7a01f5f | 347 | gRps[i] = 0.0; |
| sift | 43:5da6b1574227 | 348 | } else { |
| sift | 43:5da6b1574227 | 349 | //RPS計算[rps](1sec当たりパルス数/タイヤパルス数) |
| sift | 46:16f1a7a01f5f | 350 | gRps[i] = ((double)rps[i].counter / ((currentTime[i] + rps[i].dT1 - rps[i].dT2) / 1000000.0)) / pulseNumPerRev; |
| sift | 46:16f1a7a01f5f | 351 | gRps[i] = gRps[i] * ratioLPF_RPS + (1.0-ratioLPF_RPS)*rps[i].preRps; |
| sift | 43:5da6b1574227 | 352 | rps[i].stopCounter = 0; |
| sift | 43:5da6b1574227 | 353 | } |
| sift | 10:87ad65eef0e9 | 354 | |
| sift | 43:5da6b1574227 | 355 | rps[i].preRps = gRps[i]; |
| sift | 25:c21d35c7f0de | 356 | |
| sift | 43:5da6b1574227 | 357 | //パルス入力あれば次回のdT1はdT2を採用(パルス入力なければ現在値保持) |
| sift | 43:5da6b1574227 | 358 | if(rps[i].counter != 0) |
| sift | 43:5da6b1574227 | 359 | rps[i].dT1 = rps[i].dT2; |
| sift | 43:5da6b1574227 | 360 | |
| sift | 43:5da6b1574227 | 361 | if(rps[i].counter == 0) |
| sift | 43:5da6b1574227 | 362 | rps[i].preInputState = false; |
| sift | 43:5da6b1574227 | 363 | else |
| sift | 43:5da6b1574227 | 364 | rps[i].preInputState = true; |
| sift | 2:9d69f27a3d3b | 365 | } |
| sift | 2:9d69f27a3d3b | 366 | } |
| sift | 2:9d69f27a3d3b | 367 | |
| sift | 43:5da6b1574227 | 368 | //車輪RPS取得関数 |
| sift | 46:16f1a7a01f5f | 369 | double getWheelRps(select_t position) |
| sift | 43:5da6b1574227 | 370 | { |
| sift | 46:16f1a7a01f5f | 371 | double deltaN; //左右モータ回転数差 |
| sift | 46:16f1a7a01f5f | 372 | double aveN; //左右モータ回転数平均値 |
| sift | 43:5da6b1574227 | 373 | |
| sift | 45:8e5d35beb957 | 374 | if(position <= FL_WHEEL) //前輪の場合 |
| sift | 45:8e5d35beb957 | 375 | return gRps[position]; //演算結果をそのまま適用 |
| sift | 45:8e5d35beb957 | 376 | else { //後輪の場合,モータ回転数から速度線図に従い演算 |
| sift | 43:5da6b1574227 | 377 | //右車輪回転数が大きいと仮定 |
| sift | 46:16f1a7a01f5f | 378 | aveN = ((gRps[RR_MOTOR] + gRps[RL_MOTOR]) / GEAR_RATIO) / 2.0; //平均回転数計算 |
| sift | 45:8e5d35beb957 | 379 | deltaN = ((gRps[RR_MOTOR] - gRps[RL_MOTOR]) / GEAR_RATIO) / ALPHA; //速度線図上の車輪回転数差計算 |
| sift | 43:5da6b1574227 | 380 | |
| sift | 43:5da6b1574227 | 381 | if(position == RR_MOTOR) |
| sift | 46:16f1a7a01f5f | 382 | return aveN + deltaN / 2.0; //右車輪回転数 |
| sift | 43:5da6b1574227 | 383 | else |
| sift | 46:16f1a7a01f5f | 384 | return aveN - deltaN / 2.0; //左車輪回転数 |
| sift | 43:5da6b1574227 | 385 | } |
| sift | 43:5da6b1574227 | 386 | } |
| sift | 43:5da6b1574227 | 387 | |
| sift | 43:5da6b1574227 | 388 | float getRps(select_t position) |
| sift | 43:5da6b1574227 | 389 | { |
| sift | 43:5da6b1574227 | 390 | return gRps[position]; |
| sift | 43:5da6b1574227 | 391 | } |
| sift | 43:5da6b1574227 | 392 | |
| sift | 43:5da6b1574227 | 393 | //車速取得関数[m/s] |
| sift | 43:5da6b1574227 | 394 | //左右従動輪回転数の平均値から車速を演算 |
| sift | 46:16f1a7a01f5f | 395 | double getVelocity(void) |
| sift | 2:9d69f27a3d3b | 396 | { |
| sift | 46:16f1a7a01f5f | 397 | return (0.5*TIRE_DIAMETER*2.0*M_PI*(getWheelRps(FR_WHEEL) + getWheelRps(FL_WHEEL))*0.5); |
| sift | 1:4d86ec2fe4b1 | 398 | } |
| sift | 1:4d86ec2fe4b1 | 399 | |
| sift | 34:594ddb4008b2 | 400 | int distributeTorque_omega(float steeringWheelAngle) |
| sift | 21:bbf2ad7e6602 | 401 | { |
| sift | 21:bbf2ad7e6602 | 402 | static float lastSteering=0.0f; |
| sift | 22:95c1f753ecad | 403 | float omega=0; |
| sift | 22:95c1f753ecad | 404 | int disTrq=0; |
| sift | 30:c596a0f5d685 | 405 | |
| sift | 34:594ddb4008b2 | 406 | steeringWheelAngle = ratioLPF * steeringWheelAngle + (1.0f - ratioLPF) * lastSteering; |
| sift | 21:bbf2ad7e6602 | 407 | |
| sift | 34:594ddb4008b2 | 408 | omega = steeringWheelAngle - lastSteering; //舵角の差分算出 |
| sift | 21:bbf2ad7e6602 | 409 | omega /= 0.01f; //制御周期で角速度演算 |
| sift | 36:dc33a3a194c9 | 410 | |
| sift | 22:95c1f753ecad | 411 | if(myAbs(omega) < 0.349f) { //20deg/s |
| sift | 21:bbf2ad7e6602 | 412 | disTrq = 0; |
| sift | 22:95c1f753ecad | 413 | } else if(myAbs(omega) <= 8.727f) { //500deg/s |
| sift | 34:594ddb4008b2 | 414 | disTrq = (int)(MAX_DISTRIBUTION_TORQUE_OMEGA / (8.727f-0.349f) * (myAbs(omega) - 0.349f)); |
| sift | 21:bbf2ad7e6602 | 415 | } else |
| sift | 34:594ddb4008b2 | 416 | disTrq = (int)MAX_DISTRIBUTION_TORQUE_OMEGA; |
| sift | 22:95c1f753ecad | 417 | |
| sift | 34:594ddb4008b2 | 418 | lastSteering = steeringWheelAngle; |
| sift | 21:bbf2ad7e6602 | 419 | |
| sift | 34:594ddb4008b2 | 420 | if(omega < 0) |
| sift | 22:95c1f753ecad | 421 | disTrq = -disTrq; |
| sift | 22:95c1f753ecad | 422 | |
| sift | 21:bbf2ad7e6602 | 423 | return disTrq; |
| sift | 21:bbf2ad7e6602 | 424 | } |
| sift | 21:bbf2ad7e6602 | 425 | |
| sift | 44:d433bb5f77c0 | 426 | //int distributeTorque(float steeringWheelAngle, float velocity) |
| sift | 44:d433bb5f77c0 | 427 | //{ |
| sift | 44:d433bb5f77c0 | 428 | // double V2 = velocity * velocity; |
| sift | 44:d433bb5f77c0 | 429 | // double R = 0.0; //旋回半径 |
| sift | 44:d433bb5f77c0 | 430 | // double Gy = 0.0; //横G |
| sift | 44:d433bb5f77c0 | 431 | // double deadband = 0.0; |
| sift | 44:d433bb5f77c0 | 432 | // double steeringAngle = (double)steeringWheelAngle * STEER_RATIO; |
| sift | 44:d433bb5f77c0 | 433 | // double steeringSign = 1.0; |
| sift | 44:d433bb5f77c0 | 434 | // int disTrq = 0; |
| sift | 44:d433bb5f77c0 | 435 | // |
| sift | 44:d433bb5f77c0 | 436 | // if(steeringAngle > 0) |
| sift | 44:d433bb5f77c0 | 437 | // steeringSign = 1.0; |
| sift | 44:d433bb5f77c0 | 438 | // else |
| sift | 44:d433bb5f77c0 | 439 | // steeringSign = -1.0; |
| sift | 44:d433bb5f77c0 | 440 | // |
| sift | 44:d433bb5f77c0 | 441 | // steeringAngle = myAbs(steeringAngle); |
| sift | 44:d433bb5f77c0 | 442 | // |
| sift | 44:d433bb5f77c0 | 443 | // if(steeringAngle <= 0.0001) |
| sift | 44:d433bb5f77c0 | 444 | // steeringAngle = 0.0001; |
| sift | 44:d433bb5f77c0 | 445 | // |
| sift | 44:d433bb5f77c0 | 446 | // R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle; //理論旋回半径 計算 |
| sift | 44:d433bb5f77c0 | 447 | // Gy = V2 / R / 9.81; //理論横G |
| sift | 44:d433bb5f77c0 | 448 | // |
| sift | 44:d433bb5f77c0 | 449 | // if(Gy <= deadband) |
| sift | 44:d433bb5f77c0 | 450 | // disTrq = 0; |
| sift | 44:d433bb5f77c0 | 451 | // else if(Gy <= 1.5) { |
| sift | 44:d433bb5f77c0 | 452 | // Gy -= deadband; |
| sift | 44:d433bb5f77c0 | 453 | // disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (1.5 - deadband) * Gy); |
| sift | 44:d433bb5f77c0 | 454 | // } else { |
| sift | 44:d433bb5f77c0 | 455 | // disTrq = MAX_DISTRIBUTION_TORQUE; |
| sift | 44:d433bb5f77c0 | 456 | // } |
| sift | 44:d433bb5f77c0 | 457 | // |
| sift | 44:d433bb5f77c0 | 458 | // return (int)(disTrq * steeringSign); |
| sift | 44:d433bb5f77c0 | 459 | //} |
| sift | 44:d433bb5f77c0 | 460 | |
| sift | 44:d433bb5f77c0 | 461 | int distributeTorque(float steeringWheelAngle) |
| sift | 2:9d69f27a3d3b | 462 | { |
| sift | 44:d433bb5f77c0 | 463 | float deadband = 0.0; |
| sift | 31:042c08a7434f | 464 | double steeringSign = 1.0; |
| sift | 9:220e4e77e056 | 465 | int disTrq = 0; |
| sift | 25:c21d35c7f0de | 466 | |
| sift | 44:d433bb5f77c0 | 467 | if(steeringWheelAngle > 0) |
| sift | 34:594ddb4008b2 | 468 | steeringSign = 1.0; |
| sift | 25:c21d35c7f0de | 469 | else |
| sift | 34:594ddb4008b2 | 470 | steeringSign = -1.0; |
| sift | 25:c21d35c7f0de | 471 | |
| sift | 44:d433bb5f77c0 | 472 | if(myAbs(steeringWheelAngle) <= deadband) |
| sift | 9:220e4e77e056 | 473 | disTrq = 0; |
| sift | 44:d433bb5f77c0 | 474 | else if(myAbs(steeringWheelAngle) <= (0.5f*M_PI)) { |
| sift | 44:d433bb5f77c0 | 475 | disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (0.5f*M_PI - deadband) * (myAbs(steeringWheelAngle) - deadband)); |
| sift | 25:c21d35c7f0de | 476 | } else { |
| sift | 19:571a4d00b89c | 477 | disTrq = MAX_DISTRIBUTION_TORQUE; |
| sift | 25:c21d35c7f0de | 478 | } |
| sift | 2:9d69f27a3d3b | 479 | |
| sift | 36:dc33a3a194c9 | 480 | return (int)(disTrq * steeringSign); |
| sift | 2:9d69f27a3d3b | 481 | } |
| sift | 2:9d69f27a3d3b | 482 | |
| sift | 25:c21d35c7f0de | 483 | //rpm +++++ モータ回転数 |
| sift | 25:c21d35c7f0de | 484 | //regSwitch +++++ 回生=1 力行=0 |
| sift | 25:c21d35c7f0de | 485 | inline int calcMaxTorque(int rpm, bool regSwitch) |
| sift | 2:9d69f27a3d3b | 486 | { |
| sift | 25:c21d35c7f0de | 487 | int maxTrq=0; |
| sift | 25:c21d35c7f0de | 488 | |
| sift | 25:c21d35c7f0de | 489 | //後で削除 |
| sift | 25:c21d35c7f0de | 490 | rpm = 2000; |
| sift | 26:331e77bb479b | 491 | //++++++++++++++++++++ |
| sift | 25:c21d35c7f0de | 492 | |
| sift | 25:c21d35c7f0de | 493 | if(regSwitch == 0) { |
| sift | 25:c21d35c7f0de | 494 | if(rpm <3000) |
| sift | 25:c21d35c7f0de | 495 | maxTrq = MAX_MOTOR_TORQUE_POWER; |
| sift | 25:c21d35c7f0de | 496 | else if(rpm <= 11000) |
| sift | 25:c21d35c7f0de | 497 | maxTrq = maxTorqueMap[(int)(rpm / 10.0)]; |
| sift | 25:c21d35c7f0de | 498 | else |
| sift | 25:c21d35c7f0de | 499 | maxTrq = MAX_REVOLUTION_TORQUE_POWER; |
| sift | 25:c21d35c7f0de | 500 | } else { |
| sift | 28:47e9531a3a9d | 501 | if(rpm < 600) { |
| sift | 25:c21d35c7f0de | 502 | maxTrq = 0; |
| sift | 28:47e9531a3a9d | 503 | } else if(rpm < 1250) { |
| sift | 28:47e9531a3a9d | 504 | //+++++++++++++++++++++++++++++++++++++ |
| sift | 28:47e9531a3a9d | 505 | //暫定処理 今後回生トルクマップも要作成 |
| sift | 28:47e9531a3a9d | 506 | maxTrq = 0; |
| sift | 28:47e9531a3a9d | 507 | //+++++++++++++++++++++++++++++++++++++ |
| sift | 25:c21d35c7f0de | 508 | } else if(rpm <= 6000) { |
| sift | 26:331e77bb479b | 509 | maxTrq = MAX_MOTOR_TORQUE_REGENERATIVE; |
| sift | 26:331e77bb479b | 510 | } else if(rpm <= 11000) { |
| sift | 25:c21d35c7f0de | 511 | //+++++++++++++++++++++++++++++++++++++ |
| sift | 25:c21d35c7f0de | 512 | //暫定処理 今後回生トルクマップも要作成 |
| sift | 26:331e77bb479b | 513 | maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE; |
| sift | 25:c21d35c7f0de | 514 | //+++++++++++++++++++++++++++++++++++++ |
| sift | 25:c21d35c7f0de | 515 | } else { |
| sift | 25:c21d35c7f0de | 516 | maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE; |
| sift | 25:c21d35c7f0de | 517 | } |
| sift | 25:c21d35c7f0de | 518 | } |
| sift | 25:c21d35c7f0de | 519 | return maxTrq; |
| sift | 2:9d69f27a3d3b | 520 | } |
| sift | 2:9d69f27a3d3b | 521 | |
| sift | 25:c21d35c7f0de | 522 | //演算方法 |
| sift | 25:c21d35c7f0de | 523 | //y = a(x - b) + c |
| sift | 25:c21d35c7f0de | 524 | //x = 1/a * (y + ab - c) |
| sift | 25:c21d35c7f0de | 525 | unsigned int calcTorqueToVoltage(int reqTorque, int rpm) |
| sift | 2:9d69f27a3d3b | 526 | { |
| sift | 51:640198055ed6 | 527 | double slope = 0; //a |
| sift | 51:640198055ed6 | 528 | double startPoint = 0; //b |
| sift | 51:640198055ed6 | 529 | double intercept = 0; //c |
| sift | 12:ae291fa7239c | 530 | |
| sift | 55:e9ca699bec57 | 531 | double outputVoltage=0; |
| sift | 16:7afd623ef48a | 532 | |
| sift | 25:c21d35c7f0de | 533 | if(reqTorque > LINEAR_REGION_TORQUE_POWER) { //力行トルクがrpmに対して非線形となる領域 |
| sift | 55:e9ca699bec57 | 534 | slope = (double)(calcMaxTorque(rpm, 0) - LINEAR_REGION_TORQUE_POWER)/((double)DACOUTPUT_MAX - LINEAR_REGION_VOLTAGE_POWER); |
| sift | 25:c21d35c7f0de | 535 | startPoint = LINEAR_REGION_VOLTAGE_POWER; |
| sift | 25:c21d35c7f0de | 536 | intercept = LINEAR_REGION_TORQUE_POWER; |
| sift | 25:c21d35c7f0de | 537 | |
| sift | 55:e9ca699bec57 | 538 | outputVoltage = ((reqTorque + slope*startPoint - intercept) / slope); |
| sift | 6:26fa8c78500e | 539 | |
| sift | 25:c21d35c7f0de | 540 | } else if(reqTorque > 0) { //力行トルクがrpmに対して線形となる領域 |
| sift | 55:e9ca699bec57 | 541 | slope = (double)LINEAR_REGION_TORQUE_POWER/((double)LINEAR_REGION_VOLTAGE_POWER - ZERO_TORQUE_VOLTAGE_P); |
| sift | 25:c21d35c7f0de | 542 | startPoint = ZERO_TORQUE_VOLTAGE_P; |
| sift | 25:c21d35c7f0de | 543 | intercept = 0; |
| sift | 25:c21d35c7f0de | 544 | |
| sift | 55:e9ca699bec57 | 545 | outputVoltage = (reqTorque/slope + startPoint); |
| sift | 25:c21d35c7f0de | 546 | |
| sift | 25:c21d35c7f0de | 547 | } else if(0 == reqTorque) { |
| sift | 25:c21d35c7f0de | 548 | |
| sift | 25:c21d35c7f0de | 549 | outputVoltage = ZERO_TORQUE_VOLTAGE_NEUTRAL; //ニュートラル信号 |
| sift | 6:26fa8c78500e | 550 | |
| sift | 25:c21d35c7f0de | 551 | } else if(reqTorque > LINEAR_REGION_TORQUE_REGENERATIVE) { //回生トルクがrpmに対して線形となる領域 |
| sift | 55:e9ca699bec57 | 552 | slope = (double)(0.0 - LINEAR_REGION_TORQUE_REGENERATIVE)/((double)ZERO_TORQUE_VOLTAGE_REG - LINEAR_REGION_VOLTAGE_REGENERATIVE); |
| sift | 25:c21d35c7f0de | 553 | startPoint = LINEAR_REGION_VOLTAGE_REGENERATIVE; |
| sift | 25:c21d35c7f0de | 554 | intercept = LINEAR_REGION_TORQUE_REGENERATIVE; |
| sift | 25:c21d35c7f0de | 555 | |
| sift | 55:e9ca699bec57 | 556 | outputVoltage = ((reqTorque + slope*startPoint - intercept) / slope); |
| sift | 25:c21d35c7f0de | 557 | |
| sift | 25:c21d35c7f0de | 558 | } else { //回生トルクがrpmに対して非線形となる領域 |
| sift | 55:e9ca699bec57 | 559 | slope = (double)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/((double)LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN); |
| sift | 25:c21d35c7f0de | 560 | startPoint = DACOUTPUT_MIN; |
| sift | 25:c21d35c7f0de | 561 | intercept = calcMaxTorque(rpm, 1); |
| sift | 25:c21d35c7f0de | 562 | |
| sift | 55:e9ca699bec57 | 563 | outputVoltage = ((reqTorque + slope*startPoint - intercept) / slope); |
| sift | 2:9d69f27a3d3b | 564 | } |
| sift | 2:9d69f27a3d3b | 565 | |
| sift | 25:c21d35c7f0de | 566 | if(outputVoltage > DACOUTPUT_MAX) |
| sift | 25:c21d35c7f0de | 567 | outputVoltage = DACOUTPUT_MAX; |
| sift | 25:c21d35c7f0de | 568 | if(outputVoltage < DACOUTPUT_MIN) |
| sift | 25:c21d35c7f0de | 569 | outputVoltage = DACOUTPUT_MIN; |
| sift | 16:7afd623ef48a | 570 | |
| sift | 55:e9ca699bec57 | 571 | return (unsigned int)(0xFFF*(outputVoltage/0xFFFF)); //DACの分解能に適応(16bit->12bit) |
| sift | 2:9d69f27a3d3b | 572 | } |
| sift | 2:9d69f27a3d3b | 573 | |
| sift | 2:9d69f27a3d3b | 574 | int calcRequestTorque(void) |
| sift | 2:9d69f27a3d3b | 575 | { |
| sift | 2:9d69f27a3d3b | 576 | int currentAPS; |
| sift | 2:9d69f27a3d3b | 577 | int requestTorque; |
| sift | 2:9d69f27a3d3b | 578 | |
| sift | 2:9d69f27a3d3b | 579 | currentAPS = ((gApsP>gApsS) ? gApsS : gApsP); //センサ値は小さい方を採用 |
| sift | 12:ae291fa7239c | 580 | |
| sift | 2:9d69f27a3d3b | 581 | if(currentAPS < APS_MIN_POSITION) |
| sift | 2:9d69f27a3d3b | 582 | currentAPS = 0; |
| sift | 2:9d69f27a3d3b | 583 | else |
| sift | 2:9d69f27a3d3b | 584 | currentAPS -= APS_MIN_POSITION; //オフセット修正 |
| sift | 2:9d69f27a3d3b | 585 | |
| sift | 25:c21d35c7f0de | 586 | if(currentAPS <= APS_REG_RANGE) //デッドバンド内であれば要求トルク->0 |
| sift | 25:c21d35c7f0de | 587 | requestTorque = (int)((float)(-MAX_OUTPUT_TORQUE_REGENERATIVE) / APS_REG_RANGE * currentAPS + MAX_OUTPUT_TORQUE_REGENERATIVE); |
| sift | 2:9d69f27a3d3b | 588 | else |
| sift | 25:c21d35c7f0de | 589 | requestTorque = (int)((float)MAX_OUTPUT_TORQUE_POWER / APS_PWR_RANGE * (currentAPS - APS_REG_RANGE)); |
| sift | 2:9d69f27a3d3b | 590 | |
| sift | 25:c21d35c7f0de | 591 | if(requestTorque > MAX_OUTPUT_TORQUE_POWER) |
| sift | 25:c21d35c7f0de | 592 | requestTorque = MAX_OUTPUT_TORQUE_POWER; |
| sift | 25:c21d35c7f0de | 593 | else if(requestTorque < MAX_OUTPUT_TORQUE_REGENERATIVE) |
| sift | 25:c21d35c7f0de | 594 | requestTorque = MAX_OUTPUT_TORQUE_REGENERATIVE; |
| sift | 2:9d69f27a3d3b | 595 | |
| sift | 2:9d69f27a3d3b | 596 | return requestTorque; |
| sift | 2:9d69f27a3d3b | 597 | } |
| sift | 2:9d69f27a3d3b | 598 | |
| sift | 17:a2246ce3333f | 599 | //トルク配分車速制限関数 |
| sift | 17:a2246ce3333f | 600 | //車速が低速域の場合，トルク配分0 |
| sift | 17:a2246ce3333f | 601 | float limitTorqueDistribution(void) |
| sift | 17:a2246ce3333f | 602 | { |
| sift | 17:a2246ce3333f | 603 | float limitRate; |
| sift | 17:a2246ce3333f | 604 | float currentVelocity = getVelocity() * 3.6f; //km/hで車速取得 |
| sift | 20:3c5061281a7a | 605 | |
| sift | 37:ba10cf09c151 | 606 | if(currentVelocity < 15.0f) |
| sift | 17:a2246ce3333f | 607 | limitRate = 0.0f; |
| sift | 36:dc33a3a194c9 | 608 | else if(currentVelocity < 30.0f) |
| sift | 38:11753ee9734f | 609 | limitRate = (currentVelocity - 15.0f) / (30.0f - 15.0f); |
| sift | 17:a2246ce3333f | 610 | else |
| sift | 17:a2246ce3333f | 611 | limitRate = 1.0f; |
| sift | 36:dc33a3a194c9 | 612 | |
| sift | 17:a2246ce3333f | 613 | return limitRate; |
| sift | 17:a2246ce3333f | 614 | } |
| sift | 17:a2246ce3333f | 615 | |
| sift | 46:16f1a7a01f5f | 616 | //-------------------------------------------------- |
| sift | 46:16f1a7a01f5f | 617 | //参考文献：モデルを用いたトラクションコントロールの基礎研究 |
| sift | 46:16f1a7a01f5f | 618 | //-------------------------------------------------- |
| sift | 48:45614aa0db15 | 619 | void getTractionCtrl(int* i_motorTrq) |
| sift | 46:16f1a7a01f5f | 620 | { |
| sift | 46:16f1a7a01f5f | 621 | //------------------------------ |
| sift | 46:16f1a7a01f5f | 622 | //Constant variables |
| sift | 53:bedb85ee45f7 | 623 | const double TGT_SLIP_RATIO = 0.3; |
| sift | 53:bedb85ee45f7 | 624 | const double TGT_VEHICLE_SPEED = 20.0 / 3.6; //この車速相当の回転数まで空転を制限する |
| sift | 57:02b7178cd083 | 625 | const double CTRL_START_VEHICLE_SPEED = 10.0 / 3.6; //トラコンONとなる車速[m/s] |
| sift | 57:02b7178cd083 | 626 | const double STRAIGHT_ROAD_THRESHOLD = 200.0; //直進判定閾値 |
| sift | 57:02b7178cd083 | 627 | const double KP = 20.0; |
| sift | 56:78bd99bf995a | 628 | const double KI = 0.0; |
| sift | 56:78bd99bf995a | 629 | const double KD = 0.0; |
| sift | 46:16f1a7a01f5f | 630 | //------------------------------ |
| sift | 46:16f1a7a01f5f | 631 | |
| sift | 48:45614aa0db15 | 632 | double reqMotorTrq[2] = {i_motorTrq[0] * LSB_MOTOR_TORQUE, i_motorTrq[1] * LSB_MOTOR_TORQUE}; //実数値へ変換 |
| sift | 48:45614aa0db15 | 633 | double outputTrq[2] = {0.0}; |
| sift | 55:e9ca699bec57 | 634 | double steeringAngle, R, Vb, Vbw, Vw, wheelRpsRR, wheelRpsRL; |
| sift | 49:c97740265324 | 635 | static double lastMotorTrq[2] = {0.0}; //前回の出力トルク |
| sift | 56:78bd99bf995a | 636 | static double e[2][2] = {0.0}; //1つ前の偏差まで保持 |
| sift | 56:78bd99bf995a | 637 | static double integral = 0.0; |
| sift | 55:e9ca699bec57 | 638 | double tmpTcsMotorTrq; // |
| sift | 57:02b7178cd083 | 639 | bool straightFlag = false; |
| sift | 50:b542658924df | 640 | |
| sift | 47:949e6c2e69fc | 641 | wheelRpsRR = getWheelRps(RR_MOTOR); |
| sift | 47:949e6c2e69fc | 642 | wheelRpsRL = getWheelRps(RL_MOTOR); |
| sift | 47:949e6c2e69fc | 643 | |
| sift | 55:e9ca699bec57 | 644 | steeringAngle = getSteerAngle()/127.0 * M_PI*STEER_RATIO; //実舵角取得 |
| sift | 54:f466964ceaa5 | 645 | |
| sift | 55:e9ca699bec57 | 646 | Vb = getVelocity(); //前輪回転方向における車速換算値 |
| sift | 57:02b7178cd083 | 647 | // Vb = 15.0/3.6; |
| sift | 54:f466964ceaa5 | 648 | |
| sift | 52:6209efecde6f | 649 | R = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMax(myAbs(steeringAngle), 0.001); //理論旋回半径取得 |
| sift | 57:02b7178cd083 | 650 | |
| sift | 57:02b7178cd083 | 651 | if(myAbs(R) < 1.0) //旋回半径の最小値 |
| sift | 57:02b7178cd083 | 652 | R = mySign(steeringAngle) * 1.0; //旋回半径最小値設定 |
| sift | 57:02b7178cd083 | 653 | if(myAbs(R) > STRAIGHT_ROAD_THRESHOLD) { //直進判定 |
| sift | 57:02b7178cd083 | 654 | R = mySign(steeringAngle) * STRAIGHT_ROAD_THRESHOLD; //旋回半径設定 |
| sift | 57:02b7178cd083 | 655 | straightFlag = true; //直進判定フラグON |
| sift | 57:02b7178cd083 | 656 | } else { |
| sift | 57:02b7178cd083 | 657 | straightFlag = false; |
| sift | 57:02b7178cd083 | 658 | } |
| sift | 47:949e6c2e69fc | 659 | |
| sift | 48:45614aa0db15 | 660 | for (int rlFlag = 0; rlFlag < 2; rlFlag++) { |
| sift | 57:02b7178cd083 | 661 | if(straightFlag == true) { |
| sift | 57:02b7178cd083 | 662 | Vbw = Vb; //対地車速設定[m/s] |
| sift | 57:02b7178cd083 | 663 | } else { |
| sift | 57:02b7178cd083 | 664 | if(rlFlag == 0) { |
| sift | 57:02b7178cd083 | 665 | Vbw = Vb*(1.0 + TREAD/(2.0*R)); //旋回半径を考慮した対地車速設定[m/s] |
| sift | 57:02b7178cd083 | 666 | } else { |
| sift | 57:02b7178cd083 | 667 | Vbw = Vb*(1.0 - TREAD/(2.0*R)); //旋回半径を考慮した対地車速設定[m/s |
| sift | 57:02b7178cd083 | 668 | } |
| sift | 57:02b7178cd083 | 669 | } |
| sift | 57:02b7178cd083 | 670 | |
| sift | 48:45614aa0db15 | 671 | if(rlFlag == 0) { |
| sift | 48:45614aa0db15 | 672 | Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRR; //駆動輪速度[m/s] |
| sift | 48:45614aa0db15 | 673 | } else { |
| sift | 48:45614aa0db15 | 674 | Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRL; //駆動輪速度[m/s] |
| sift | 48:45614aa0db15 | 675 | } |
| sift | 48:45614aa0db15 | 676 | |
| sift | 54:f466964ceaa5 | 677 | if(Vb < CTRL_START_VEHICLE_SPEED) { //対地車速が一定値より小さい場合(停車状態) |
| sift | 54:f466964ceaa5 | 678 | e[rlFlag][0] = TGT_VEHICLE_SPEED - Vw; //空転抑制制御 |
| sift | 53:bedb85ee45f7 | 679 | } else { |
| sift | 57:02b7178cd083 | 680 | e[rlFlag][0] = Vbw/(1.0 - TGT_SLIP_RATIO) - Vw; //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分) |
| sift | 53:bedb85ee45f7 | 681 | } |
| sift | 57:02b7178cd083 | 682 | |
| sift | 56:78bd99bf995a | 683 | integral = integral + (e[rlFlag][0] + e[rlFlag][1]); |
| sift | 56:78bd99bf995a | 684 | lastMotorTrq[rlFlag] = KP * e[rlFlag][0] + KI*integral + KD*(e[rlFlag][1] - e[rlFlag][0]); //PIDコントローラへどーーーん |
| sift | 48:45614aa0db15 | 685 | e[rlFlag][1] = e[rlFlag][0]; |
| sift | 46:16f1a7a01f5f | 686 | } |
| sift | 46:16f1a7a01f5f | 687 | |
| sift | 54:f466964ceaa5 | 688 | printf("%f %f\r\n", e[0][0], e[1][0]); |
| sift | 50:b542658924df | 689 | |
| sift | 54:f466964ceaa5 | 690 | outputTrq[0] = myMin(reqMotorTrq[0], lastMotorTrq[0]); //ちっさい方を出力トルクとして採用 |
| sift | 54:f466964ceaa5 | 691 | outputTrq[1] = myMin(reqMotorTrq[1], lastMotorTrq[1]); //ちっさい方を出力トルクとして採用 |
| sift | 46:16f1a7a01f5f | 692 | |
| sift | 48:45614aa0db15 | 693 | for (int rlFlag = 0; rlFlag < 2; rlFlag++) { |
| sift | 54:f466964ceaa5 | 694 | if(outputTrq[rlFlag] > 45.0) |
| sift | 54:f466964ceaa5 | 695 | outputTrq[rlFlag] = 45.0; |
| sift | 57:02b7178cd083 | 696 | if(outputTrq[rlFlag] < 0.0) |
| sift | 57:02b7178cd083 | 697 | outputTrq[rlFlag] = 0.0; |
| sift | 54:f466964ceaa5 | 698 | } |
| sift | 46:16f1a7a01f5f | 699 | |
| sift | 57:02b7178cd083 | 700 | if(myAbs(e[0][0]) > myAbs(e[1][1])) { //右輪の空転が大きい場合 |
| sift | 57:02b7178cd083 | 701 | tmpTcsMotorTrq = outputTrq[1] * (ALPHA-1.0)/(ALPHA+1.0); |
| sift | 57:02b7178cd083 | 702 | if(tmpTcsMotorTrq > outputTrq[0]) //空転輪側車軸トルクを下げ過ぎていた場合 |
| sift | 57:02b7178cd083 | 703 | if(reqMotorTrq[0] > tmpTcsMotorTrq) |
| sift | 57:02b7178cd083 | 704 | outputTrq[0] = tmpTcsMotorTrq; //右車軸トルクを0へ制限 |
| sift | 57:02b7178cd083 | 705 | } else if(myAbs(e[0][0]) < myAbs(e[1][1])) { //左輪の空転が大きい場合 |
| sift | 57:02b7178cd083 | 706 | tmpTcsMotorTrq = outputTrq[0] * (ALPHA-1.0)/(ALPHA+1.0); |
| sift | 57:02b7178cd083 | 707 | if(tmpTcsMotorTrq > outputTrq[1]) //空転輪側車軸トルクを下げ過ぎていた場合 |
| sift | 57:02b7178cd083 | 708 | if(reqMotorTrq[1] > tmpTcsMotorTrq) |
| sift | 57:02b7178cd083 | 709 | outputTrq[1] = tmpTcsMotorTrq; //左車軸トルクを0へ制限 |
| sift | 57:02b7178cd083 | 710 | } |
| sift | 54:f466964ceaa5 | 711 | |
| sift | 54:f466964ceaa5 | 712 | for (int rlFlag = 0; rlFlag < 2; rlFlag++) { |
| sift | 57:02b7178cd083 | 713 | i_motorTrq[rlFlag] = (int)(outputTrq[rlFlag] / LSB_MOTOR_TORQUE + 0.5); |
| sift | 48:45614aa0db15 | 714 | } |
| sift | 46:16f1a7a01f5f | 715 | } |
| sift | 46:16f1a7a01f5f | 716 | |
| sift | 26:331e77bb479b | 717 | void driveTVD(int TVDmode, bool isRedyToDrive) |
| sift | 1:4d86ec2fe4b1 | 718 | { |
| sift | 48:45614aa0db15 | 719 | int requestTorque = 0; //ドライバー要求トルク |
| sift | 48:45614aa0db15 | 720 | int distributionTrq = 0; //分配トルク |
| sift | 48:45614aa0db15 | 721 | int disTrq_omega = 0; |
| sift | 48:45614aa0db15 | 722 | int motorTrq[2] = {0}; //左右モータトルク |
| sift | 48:45614aa0db15 | 723 | static unsigned int preMcpA = 0, preMcpB = 0; |
| sift | 20:3c5061281a7a | 724 | |
| sift | 48:45614aa0db15 | 725 | loadSensors(); //APS,BRAKE更新 |
| sift | 48:45614aa0db15 | 726 | loadSteerAngle(); //舵角更新 |
| sift | 48:45614aa0db15 | 727 | loadRps(); //従動輪・モータ回転数更新 |
| sift | 41:0c53acd31247 | 728 | |
| sift | 55:e9ca699bec57 | 729 | // printf("%f %f\r\n", getWheelRps(RR_MOTOR), getWheelRps(RL_MOTOR)); |
| sift | 55:e9ca699bec57 | 730 | |
| sift | 48:45614aa0db15 | 731 | if(isRedyToDrive && isBrakeOn()) |
| sift | 12:ae291fa7239c | 732 | readyToDriveFlag = 0; |
| sift | 2:9d69f27a3d3b | 733 | |
| sift | 12:ae291fa7239c | 734 | if((errCounter.apsUnderVolt > ERRCOUNTER_DECISION) |
| sift | 12:ae291fa7239c | 735 | || (errCounter.apsExceedVolt > ERRCOUNTER_DECISION) |
| sift | 12:ae291fa7239c | 736 | || (errCounter.apsErrorTolerance > ERRCOUNTER_DECISION) |
| sift | 12:ae291fa7239c | 737 | // || (errCounter.apsStick > ERRCOUNTER_DECISION) |
| sift | 30:c596a0f5d685 | 738 | || (errCounter.brakeUnderVolt > ERRCOUNTER_DECISION) |
| sift | 30:c596a0f5d685 | 739 | || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION) |
| sift | 30:c596a0f5d685 | 740 | || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION) |
| sift | 12:ae291fa7239c | 741 | ) { |
| sift | 16:7afd623ef48a | 742 | readyToDriveFlag = 1; |
| sift | 12:ae291fa7239c | 743 | } |
| sift | 16:7afd623ef48a | 744 | |
| sift | 12:ae291fa7239c | 745 | indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION)); |
| sift | 20:3c5061281a7a | 746 | LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION); |
| sift | 16:7afd623ef48a | 747 | |
| sift | 51:640198055ed6 | 748 | requestTorque = calcRequestTorque(); //ドライバー要求トルク取得 |
| sift | 4:d7778cde0aff | 749 | |
| sift | 45:8e5d35beb957 | 750 | if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1)) |
| sift | 45:8e5d35beb957 | 751 | requestTorque = 0; |
| sift | 52:6209efecde6f | 752 | |
| sift | 44:d433bb5f77c0 | 753 | distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) / 2.0f); //片モーターのトルク分配量計算 |
| sift | 38:11753ee9734f | 754 | disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f)*limitTorqueDistribution()) / 2.0f); //微分制御 |
| sift | 12:ae291fa7239c | 755 | |
| sift | 45:8e5d35beb957 | 756 | distributionTrq = 0; |
| sift | 35:b75595b1da36 | 757 | disTrq_omega = 0; |
| sift | 31:042c08a7434f | 758 | |
| sift | 48:45614aa0db15 | 759 | motorTrq[0] = requestTorque + distributionTrq; |
| sift | 48:45614aa0db15 | 760 | motorTrq[1] = requestTorque - distributionTrq; |
| sift | 21:bbf2ad7e6602 | 761 | |
| sift | 48:45614aa0db15 | 762 | motorTrq[0] += disTrq_omega; |
| sift | 48:45614aa0db15 | 763 | motorTrq[1] -= disTrq_omega; |
| sift | 47:949e6c2e69fc | 764 | |
| sift | 48:45614aa0db15 | 765 | getTractionCtrl(motorTrq); |
| sift | 20:3c5061281a7a | 766 | |
| sift | 48:45614aa0db15 | 767 | gRightMotorTorque = motorTrq[0]; |
| sift | 48:45614aa0db15 | 768 | gLeftMotorTorque = motorTrq[1]; |
| sift | 34:594ddb4008b2 | 769 | |
| sift | 48:45614aa0db15 | 770 | McpData.valA = calcTorqueToVoltage(motorTrq[0], getRps(RR_MOTOR) * 60.0f); |
| sift | 48:45614aa0db15 | 771 | McpData.valB = calcTorqueToVoltage(motorTrq[1], getRps(RL_MOTOR) * 60.0f); |
| sift | 16:7afd623ef48a | 772 | |
| sift | 38:11753ee9734f | 773 | preMcpA = McpData.valA; |
| sift | 38:11753ee9734f | 774 | preMcpB = McpData.valB; |
| sift | 20:3c5061281a7a | 775 | |
| sift | 20:3c5061281a7a | 776 | mcp.writeA(preMcpA); //右モーター |
| sift | 20:3c5061281a7a | 777 | mcp.writeB(preMcpB); //左モーター |
| sift | 1:4d86ec2fe4b1 | 778 | } |
| sift | 1:4d86ec2fe4b1 | 779 | |
| sift | 1:4d86ec2fe4b1 | 780 | void initTVD(void) |
| sift | 1:4d86ec2fe4b1 | 781 | { |
| sift | 43:5da6b1574227 | 782 | mcp.writeA(0); //右モーター初期値 |
| sift | 43:5da6b1574227 | 783 | mcp.writeB(0); //左モーター初期値 |
| sift | 39:c05074379713 | 784 | |
| sift | 43:5da6b1574227 | 785 | wheelPulseTimer[FR_WHEEL].reset(); |
| sift | 43:5da6b1574227 | 786 | wheelPulseTimer[FL_WHEEL].reset(); |
| sift | 43:5da6b1574227 | 787 | wheelPulseTimer[RR_MOTOR].reset(); |
| sift | 43:5da6b1574227 | 788 | wheelPulseTimer[RL_MOTOR].reset(); |
| sift | 1:4d86ec2fe4b1 | 789 | |
| sift | 43:5da6b1574227 | 790 | wheelPulseTimer[FR_WHEEL].start(); |
| sift | 43:5da6b1574227 | 791 | wheelPulseTimer[FL_WHEEL].start(); |
| sift | 43:5da6b1574227 | 792 | wheelPulseTimer[RR_MOTOR].start(); |
| sift | 43:5da6b1574227 | 793 | wheelPulseTimer[RL_MOTOR].start(); |
| sift | 43:5da6b1574227 | 794 | |
| sift | 43:5da6b1574227 | 795 | rightMotorPulse.rise(&countRightMotorPulseISR); |
| sift | 43:5da6b1574227 | 796 | leftMotorPulse.rise(&countLeftMotorPulseISR); |
| sift | 43:5da6b1574227 | 797 | |
| sift | 43:5da6b1574227 | 798 | // rightWheelPulse1.fall(&countRightWheelPulseISR); //パルス測定は立ち上がりor立下りのどちらかを計測するのが吉 |
| sift | 43:5da6b1574227 | 799 | // rightWheelPulse2.fall(&countRightWheelPulseISR); //立下り特性悪すぎなので測定誤差が増える |
| sift | 42:3ab09d0e3071 | 800 | rightWheelPulse1.rise(&countRightWheelPulseISR); |
| sift | 43:5da6b1574227 | 801 | // rightWheelPulse2.rise(&countRightWheelPulseISR); |
| sift | 25:c21d35c7f0de | 802 | |
| sift | 43:5da6b1574227 | 803 | // leftWheelPulse1.fall(&countLeftWheelPulseISR); |
| sift | 43:5da6b1574227 | 804 | // leftWheelPulse2.fall(&countLeftWheelPulseISR); |
| sift | 43:5da6b1574227 | 805 | leftWheelPulse1.rise(&countLeftWheelPulseISR); |
| sift | 43:5da6b1574227 | 806 | // leftWheelPulse2.rise(&countLeftWheelPulseISR); //AB相の位相差が90度から離れすぎなので測定誤差が増える(スリットが一定間隔でないことになる) |
| sift | 10:87ad65eef0e9 | 807 | |
| sift | 43:5da6b1574227 | 808 | ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S); //制御周期毎にデータ読み込み(LPF演算のため) |
| sift | 43:5da6b1574227 | 809 | ticker2.attach(&loadRpsISR, RPS_MEAS_CYCLE_S); //RPS計測周期設定 |
| sift | 28:47e9531a3a9d | 810 | |
| sift | 30:c596a0f5d685 | 811 | printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_POWER); |
| sift | 30:c596a0f5d685 | 812 | printf("MAX OUTPUT REG-TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_REGENERATIVE); |
| sift | 30:c596a0f5d685 | 813 | printf("MAX DISTRIBUTION TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_DISTRIBUTION_TORQUE); |
| sift | 30:c596a0f5d685 | 814 | printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MIN_INNERWHEEL_MOTOR_TORQUE); |
| sift | 43:5da6b1574227 | 815 | } |