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