diff options
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs')
-rwxr-xr-x | OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | 234 |
1 files changed, 169 insertions, 65 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs index 817a5f7..ef662b5 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | |||
@@ -59,22 +59,17 @@ public abstract class BSMotor | |||
59 | { | 59 | { |
60 | if (PhysicsScene != null) | 60 | if (PhysicsScene != null) |
61 | { | 61 | { |
62 | if (PhysicsScene.VehicleLoggingEnabled) | 62 | PhysicsScene.DetailLog(msg, parms); |
63 | { | ||
64 | PhysicsScene.DetailLog(msg, parms); | ||
65 | } | ||
66 | } | 63 | } |
67 | } | 64 | } |
68 | } | 65 | } |
69 | 66 | ||
70 | // Motor which moves CurrentValue to TargetValue over TimeScale seconds. | 67 | // Motor which moves CurrentValue to TargetValue over TimeScale seconds. |
71 | // The TargetValue decays in TargetValueDecayTimeScale and | 68 | // The TargetValue decays in TargetValueDecayTimeScale. |
72 | // the CurrentValue will be held back by FrictionTimeScale. | ||
73 | // This motor will "zero itself" over time in that the targetValue will | 69 | // This motor will "zero itself" over time in that the targetValue will |
74 | // decay to zero and the currentValue will follow it to that zero. | 70 | // decay to zero and the currentValue will follow it to that zero. |
75 | // The overall effect is for the returned correction value to go from large | 71 | // The overall effect is for the returned correction value to go from large |
76 | // values (the total difference between current and target minus friction) | 72 | // values to small and eventually zero values. |
77 | // to small and eventually zero values. | ||
78 | // TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay. | 73 | // TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay. |
79 | 74 | ||
80 | // For instance, if something is moving at speed X and the desired speed is Y, | 75 | // For instance, if something is moving at speed X and the desired speed is Y, |
@@ -91,7 +86,6 @@ public class BSVMotor : BSMotor | |||
91 | 86 | ||
92 | public virtual float TimeScale { get; set; } | 87 | public virtual float TimeScale { get; set; } |
93 | public virtual float TargetValueDecayTimeScale { get; set; } | 88 | public virtual float TargetValueDecayTimeScale { get; set; } |
94 | public virtual Vector3 FrictionTimescale { get; set; } | ||
95 | public virtual float Efficiency { get; set; } | 89 | public virtual float Efficiency { get; set; } |
96 | 90 | ||
97 | public virtual float ErrorZeroThreshold { get; set; } | 91 | public virtual float ErrorZeroThreshold { get; set; } |
@@ -100,10 +94,13 @@ public class BSVMotor : BSMotor | |||
100 | public virtual Vector3 CurrentValue { get; protected set; } | 94 | public virtual Vector3 CurrentValue { get; protected set; } |
101 | public virtual Vector3 LastError { get; protected set; } | 95 | public virtual Vector3 LastError { get; protected set; } |
102 | 96 | ||
103 | public virtual bool ErrorIsZero | 97 | public virtual bool ErrorIsZero() |
104 | { get { | 98 | { |
105 | return (LastError == Vector3.Zero || LastError.LengthSquared() <= ErrorZeroThreshold); | 99 | return ErrorIsZero(LastError); |
106 | } | 100 | } |
101 | public virtual bool ErrorIsZero(Vector3 err) | ||
102 | { | ||
103 | return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)); | ||
107 | } | 104 | } |
108 | 105 | ||
109 | public BSVMotor(string useName) | 106 | public BSVMotor(string useName) |
@@ -111,16 +108,14 @@ public class BSVMotor : BSMotor | |||
111 | { | 108 | { |
112 | TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite; | 109 | TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite; |
113 | Efficiency = 1f; | 110 | Efficiency = 1f; |
114 | FrictionTimescale = BSMotor.InfiniteVector; | ||
115 | CurrentValue = TargetValue = Vector3.Zero; | 111 | CurrentValue = TargetValue = Vector3.Zero; |
116 | ErrorZeroThreshold = 0.001f; | 112 | ErrorZeroThreshold = 0.001f; |
117 | } | 113 | } |
118 | public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) | 114 | public BSVMotor(string useName, float timeScale, float decayTimeScale, float efficiency) |
119 | : this(useName) | 115 | : this(useName) |
120 | { | 116 | { |
121 | TimeScale = timeScale; | 117 | TimeScale = timeScale; |
122 | TargetValueDecayTimeScale = decayTimeScale; | 118 | TargetValueDecayTimeScale = decayTimeScale; |
123 | FrictionTimescale = frictionTimeScale; | ||
124 | Efficiency = efficiency; | 119 | Efficiency = efficiency; |
125 | CurrentValue = TargetValue = Vector3.Zero; | 120 | CurrentValue = TargetValue = Vector3.Zero; |
126 | } | 121 | } |
@@ -138,7 +133,8 @@ public class BSVMotor : BSMotor | |||
138 | CurrentValue = TargetValue = Vector3.Zero; | 133 | CurrentValue = TargetValue = Vector3.Zero; |
139 | } | 134 | } |
140 | 135 | ||
141 | // Compute the next step and return the new current value | 136 | // Compute the next step and return the new current value. |
137 | // Returns the correction needed to move 'current' to 'target'. | ||
142 | public virtual Vector3 Step(float timeStep) | 138 | public virtual Vector3 Step(float timeStep) |
143 | { | 139 | { |
144 | if (!Enabled) return TargetValue; | 140 | if (!Enabled) return TargetValue; |
@@ -148,9 +144,10 @@ public class BSVMotor : BSMotor | |||
148 | 144 | ||
149 | Vector3 correction = Vector3.Zero; | 145 | Vector3 correction = Vector3.Zero; |
150 | Vector3 error = TargetValue - CurrentValue; | 146 | Vector3 error = TargetValue - CurrentValue; |
151 | if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) | 147 | LastError = error; |
148 | if (!ErrorIsZero(error)) | ||
152 | { | 149 | { |
153 | correction = Step(timeStep, error); | 150 | correction = StepError(timeStep, error); |
154 | 151 | ||
155 | CurrentValue += correction; | 152 | CurrentValue += correction; |
156 | 153 | ||
@@ -163,44 +160,40 @@ public class BSVMotor : BSMotor | |||
163 | TargetValue *= (1f - decayFactor); | 160 | TargetValue *= (1f - decayFactor); |
164 | } | 161 | } |
165 | 162 | ||
166 | // The amount we can correct the error is reduced by the friction | ||
167 | Vector3 frictionFactor = Vector3.Zero; | ||
168 | if (FrictionTimescale != BSMotor.InfiniteVector) | ||
169 | { | ||
170 | // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep; | ||
171 | // Individual friction components can be 'infinite' so compute each separately. | ||
172 | frictionFactor.X = (FrictionTimescale.X == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.X); | ||
173 | frictionFactor.Y = (FrictionTimescale.Y == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Y); | ||
174 | frictionFactor.Z = (FrictionTimescale.Z == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Z); | ||
175 | frictionFactor *= timeStep; | ||
176 | CurrentValue *= (Vector3.One - frictionFactor); | ||
177 | } | ||
178 | |||
179 | MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}", | 163 | MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}", |
180 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, | 164 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, |
181 | timeStep, error, correction); | 165 | timeStep, error, correction); |
182 | MDetailLog("{0}, BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}", | 166 | MDetailLog("{0}, BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}", |
183 | BSScene.DetailLogZero, UseName, | 167 | BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue); |
184 | TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor, | ||
185 | TargetValue, CurrentValue); | ||
186 | } | 168 | } |
187 | else | 169 | else |
188 | { | 170 | { |
189 | // Difference between what we have and target is small. Motor is done. | 171 | // Difference between what we have and target is small. Motor is done. |
172 | if (TargetValue.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) | ||
173 | { | ||
174 | // The target can step down to nearly zero but not get there. If close to zero | ||
175 | // it is really zero. | ||
176 | TargetValue = Vector3.Zero; | ||
177 | } | ||
190 | CurrentValue = TargetValue; | 178 | CurrentValue = TargetValue; |
191 | MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", | 179 | MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}", |
192 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); | 180 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue); |
193 | } | 181 | } |
194 | 182 | ||
195 | return CurrentValue; | 183 | return correction; |
184 | } | ||
185 | // version of step that sets the current value before doing the step | ||
186 | public virtual Vector3 Step(float timeStep, Vector3 current) | ||
187 | { | ||
188 | CurrentValue = current; | ||
189 | return Step(timeStep); | ||
196 | } | 190 | } |
197 | public virtual Vector3 Step(float timeStep, Vector3 error) | 191 | public virtual Vector3 StepError(float timeStep, Vector3 error) |
198 | { | 192 | { |
199 | if (!Enabled) return Vector3.Zero; | 193 | if (!Enabled) return Vector3.Zero; |
200 | 194 | ||
201 | LastError = error; | ||
202 | Vector3 returnCorrection = Vector3.Zero; | 195 | Vector3 returnCorrection = Vector3.Zero; |
203 | if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) | 196 | if (!ErrorIsZero(error)) |
204 | { | 197 | { |
205 | // correction = error / secondsItShouldTakeToCorrect | 198 | // correction = error / secondsItShouldTakeToCorrect |
206 | Vector3 correctionAmount; | 199 | Vector3 correctionAmount; |
@@ -222,9 +215,9 @@ public class BSVMotor : BSMotor | |||
222 | // maximum number of outputs to generate. | 215 | // maximum number of outputs to generate. |
223 | int maxOutput = 50; | 216 | int maxOutput = 50; |
224 | MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName); | 217 | MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName); |
225 | MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}", | 218 | MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},eff={4},curr={5},tgt={6}", |
226 | BSScene.DetailLogZero, UseName, | 219 | BSScene.DetailLogZero, UseName, |
227 | TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency, | 220 | TimeScale, TargetValueDecayTimeScale, Efficiency, |
228 | CurrentValue, TargetValue); | 221 | CurrentValue, TargetValue); |
229 | 222 | ||
230 | LastError = BSMotor.InfiniteVector; | 223 | LastError = BSMotor.InfiniteVector; |
@@ -235,43 +228,141 @@ public class BSVMotor : BSMotor | |||
235 | BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep); | 228 | BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep); |
236 | } | 229 | } |
237 | MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName); | 230 | MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName); |
238 | 231 | ||
239 | 232 | ||
240 | } | 233 | } |
241 | 234 | ||
242 | public override string ToString() | 235 | public override string ToString() |
243 | { | 236 | { |
244 | return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4},frictTS={5}>", | 237 | return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>", |
245 | UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale); | 238 | UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale); |
246 | } | 239 | } |
247 | } | 240 | } |
248 | 241 | ||
242 | // ============================================================================ | ||
243 | // ============================================================================ | ||
249 | public class BSFMotor : BSMotor | 244 | public class BSFMotor : BSMotor |
250 | { | 245 | { |
251 | public float TimeScale { get; set; } | 246 | public virtual float TimeScale { get; set; } |
252 | public float DecayTimeScale { get; set; } | 247 | public virtual float TargetValueDecayTimeScale { get; set; } |
253 | public float Friction { get; set; } | 248 | public virtual float Efficiency { get; set; } |
254 | public float Efficiency { get; set; } | ||
255 | 249 | ||
256 | public float Target { get; private set; } | 250 | public virtual float ErrorZeroThreshold { get; set; } |
257 | public float CurrentValue { get; private set; } | ||
258 | 251 | ||
259 | public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency) | 252 | public virtual float TargetValue { get; protected set; } |
253 | public virtual float CurrentValue { get; protected set; } | ||
254 | public virtual float LastError { get; protected set; } | ||
255 | |||
256 | public virtual bool ErrorIsZero() | ||
257 | { | ||
258 | return ErrorIsZero(LastError); | ||
259 | } | ||
260 | public virtual bool ErrorIsZero(float err) | ||
261 | { | ||
262 | return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold); | ||
263 | } | ||
264 | |||
265 | public BSFMotor(string useName, float timeScale, float decayTimescale, float efficiency) | ||
260 | : base(useName) | 266 | : base(useName) |
261 | { | 267 | { |
268 | TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite; | ||
269 | Efficiency = 1f; | ||
270 | CurrentValue = TargetValue = 0f; | ||
271 | ErrorZeroThreshold = 0.01f; | ||
262 | } | 272 | } |
263 | public void SetCurrent(float target) | 273 | public void SetCurrent(float current) |
264 | { | 274 | { |
275 | CurrentValue = current; | ||
265 | } | 276 | } |
266 | public void SetTarget(float target) | 277 | public void SetTarget(float target) |
267 | { | 278 | { |
279 | TargetValue = target; | ||
268 | } | 280 | } |
281 | public override void Zero() | ||
282 | { | ||
283 | base.Zero(); | ||
284 | CurrentValue = TargetValue = 0f; | ||
285 | } | ||
286 | |||
269 | public virtual float Step(float timeStep) | 287 | public virtual float Step(float timeStep) |
270 | { | 288 | { |
271 | return 0f; | 289 | if (!Enabled) return TargetValue; |
290 | |||
291 | float origTarget = TargetValue; // DEBUG | ||
292 | float origCurrVal = CurrentValue; // DEBUG | ||
293 | |||
294 | float correction = 0f; | ||
295 | float error = TargetValue - CurrentValue; | ||
296 | LastError = error; | ||
297 | if (!ErrorIsZero(error)) | ||
298 | { | ||
299 | correction = StepError(timeStep, error); | ||
300 | |||
301 | CurrentValue += correction; | ||
302 | |||
303 | // The desired value reduces to zero which also reduces the difference with current. | ||
304 | // If the decay time is infinite, don't decay at all. | ||
305 | float decayFactor = 0f; | ||
306 | if (TargetValueDecayTimeScale != BSMotor.Infinite) | ||
307 | { | ||
308 | decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep; | ||
309 | TargetValue *= (1f - decayFactor); | ||
310 | } | ||
311 | |||
312 | MDetailLog("{0}, BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}", | ||
313 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, | ||
314 | timeStep, error, correction); | ||
315 | MDetailLog("{0}, BSFMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}", | ||
316 | BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue); | ||
317 | } | ||
318 | else | ||
319 | { | ||
320 | // Difference between what we have and target is small. Motor is done. | ||
321 | if (Util.InRange<float>(TargetValue, -ErrorZeroThreshold, ErrorZeroThreshold)) | ||
322 | { | ||
323 | // The target can step down to nearly zero but not get there. If close to zero | ||
324 | // it is really zero. | ||
325 | TargetValue = 0f; | ||
326 | } | ||
327 | CurrentValue = TargetValue; | ||
328 | MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", | ||
329 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); | ||
330 | } | ||
331 | |||
332 | return CurrentValue; | ||
333 | } | ||
334 | |||
335 | public virtual float StepError(float timeStep, float error) | ||
336 | { | ||
337 | if (!Enabled) return 0f; | ||
338 | |||
339 | float returnCorrection = 0f; | ||
340 | if (!ErrorIsZero(error)) | ||
341 | { | ||
342 | // correction = error / secondsItShouldTakeToCorrect | ||
343 | float correctionAmount; | ||
344 | if (TimeScale == 0f || TimeScale == BSMotor.Infinite) | ||
345 | correctionAmount = error * timeStep; | ||
346 | else | ||
347 | correctionAmount = error / TimeScale * timeStep; | ||
348 | |||
349 | returnCorrection = correctionAmount; | ||
350 | MDetailLog("{0}, BSFMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}", | ||
351 | BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount); | ||
352 | } | ||
353 | return returnCorrection; | ||
354 | } | ||
355 | |||
356 | public override string ToString() | ||
357 | { | ||
358 | return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>", | ||
359 | UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale); | ||
272 | } | 360 | } |
361 | |||
273 | } | 362 | } |
274 | 363 | ||
364 | // ============================================================================ | ||
365 | // ============================================================================ | ||
275 | // Proportional, Integral, Derivitive Motor | 366 | // Proportional, Integral, Derivitive Motor |
276 | // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. | 367 | // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. |
277 | public class BSPIDVMotor : BSVMotor | 368 | public class BSPIDVMotor : BSVMotor |
@@ -281,6 +372,12 @@ public class BSPIDVMotor : BSVMotor | |||
281 | public Vector3 integralFactor { get; set; } | 372 | public Vector3 integralFactor { get; set; } |
282 | public Vector3 derivFactor { get; set; } | 373 | public Vector3 derivFactor { get; set; } |
283 | 374 | ||
375 | // The factors are vectors for the three dimensions. This is the proportional of each | ||
376 | // that is applied. This could be multiplied through the actual factors but it | ||
377 | // is sometimes easier to manipulate the factors and their mix separately. | ||
378 | // to | ||
379 | public Vector3 FactorMix; | ||
380 | |||
284 | // Arbritrary factor range. | 381 | // Arbritrary factor range. |
285 | // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct. | 382 | // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct. |
286 | public float EfficiencyHigh = 0.4f; | 383 | public float EfficiencyHigh = 0.4f; |
@@ -295,6 +392,7 @@ public class BSPIDVMotor : BSVMotor | |||
295 | proportionFactor = new Vector3(1.00f, 1.00f, 1.00f); | 392 | proportionFactor = new Vector3(1.00f, 1.00f, 1.00f); |
296 | integralFactor = new Vector3(1.00f, 1.00f, 1.00f); | 393 | integralFactor = new Vector3(1.00f, 1.00f, 1.00f); |
297 | derivFactor = new Vector3(1.00f, 1.00f, 1.00f); | 394 | derivFactor = new Vector3(1.00f, 1.00f, 1.00f); |
395 | FactorMix = new Vector3(0.5f, 0.25f, 0.25f); | ||
298 | RunningIntegration = Vector3.Zero; | 396 | RunningIntegration = Vector3.Zero; |
299 | LastError = Vector3.Zero; | 397 | LastError = Vector3.Zero; |
300 | } | 398 | } |
@@ -310,20 +408,24 @@ public class BSPIDVMotor : BSVMotor | |||
310 | set | 408 | set |
311 | { | 409 | { |
312 | base.Efficiency = Util.Clamp(value, 0f, 1f); | 410 | base.Efficiency = Util.Clamp(value, 0f, 1f); |
411 | |||
313 | // Compute factors based on efficiency. | 412 | // Compute factors based on efficiency. |
314 | // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot. | 413 | // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot. |
315 | // If efficiency is low (0f), use a factor value that overcorrects. | 414 | // If efficiency is low (0f), use a factor value that overcorrects. |
316 | // TODO: might want to vary contribution of different factor depending on efficiency. | 415 | // TODO: might want to vary contribution of different factor depending on efficiency. |
317 | float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f; | 416 | float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f; |
318 | // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow; | 417 | // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow; |
418 | |||
319 | proportionFactor = new Vector3(factor, factor, factor); | 419 | proportionFactor = new Vector3(factor, factor, factor); |
320 | integralFactor = new Vector3(factor, factor, factor); | 420 | integralFactor = new Vector3(factor, factor, factor); |
321 | derivFactor = new Vector3(factor, factor, factor); | 421 | derivFactor = new Vector3(factor, factor, factor); |
422 | |||
423 | MDetailLog("{0},BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor); | ||
322 | } | 424 | } |
323 | } | 425 | } |
324 | 426 | ||
325 | // Ignore Current and Target Values and just advance the PID computation on this error. | 427 | // Advance the PID computation on this error. |
326 | public override Vector3 Step(float timeStep, Vector3 error) | 428 | public override Vector3 StepError(float timeStep, Vector3 error) |
327 | { | 429 | { |
328 | if (!Enabled) return Vector3.Zero; | 430 | if (!Enabled) return Vector3.Zero; |
329 | 431 | ||
@@ -331,15 +433,17 @@ public class BSPIDVMotor : BSVMotor | |||
331 | RunningIntegration += error * timeStep; | 433 | RunningIntegration += error * timeStep; |
332 | 434 | ||
333 | // A simple derivitive is the rate of change from the last error. | 435 | // A simple derivitive is the rate of change from the last error. |
334 | Vector3 derivFactor = (error - LastError) * timeStep; | 436 | Vector3 derivitive = (error - LastError) * timeStep; |
335 | LastError = error; | 437 | LastError = error; |
336 | 438 | ||
337 | // Correction = -(proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError) | 439 | // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError) |
338 | Vector3 ret = -( | 440 | Vector3 ret = error * timeStep * proportionFactor * FactorMix.X |
339 | error * proportionFactor | 441 | + RunningIntegration * integralFactor * FactorMix.Y |
340 | + RunningIntegration * integralFactor | 442 | + derivitive * derivFactor * FactorMix.Z |
341 | + derivFactor * derivFactor | 443 | ; |
342 | ); | 444 | |
445 | MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}", | ||
446 | BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret); | ||
343 | 447 | ||
344 | return ret; | 448 | return ret; |
345 | } | 449 | } |