BulletSim: reorganize motor code a little to pull together common functions.
Add BSFMotor.user_profiles
parent
44492b3a49
commit
5432180027
|
@ -59,10 +59,7 @@ public abstract class BSMotor
|
||||||
{
|
{
|
||||||
if (PhysicsScene != null)
|
if (PhysicsScene != null)
|
||||||
{
|
{
|
||||||
if (PhysicsScene.VehicleLoggingEnabled)
|
PhysicsScene.DetailLog(msg, parms);
|
||||||
{
|
|
||||||
PhysicsScene.DetailLog(msg, parms);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -100,10 +97,13 @@ public class BSVMotor : BSMotor
|
||||||
public virtual Vector3 CurrentValue { get; protected set; }
|
public virtual Vector3 CurrentValue { get; protected set; }
|
||||||
public virtual Vector3 LastError { get; protected set; }
|
public virtual Vector3 LastError { get; protected set; }
|
||||||
|
|
||||||
public virtual bool ErrorIsZero
|
public virtual bool ErrorIsZero()
|
||||||
{ get {
|
{
|
||||||
return (LastError == Vector3.Zero || LastError.LengthSquared() <= ErrorZeroThreshold);
|
return ErrorIsZero(LastError);
|
||||||
}
|
}
|
||||||
|
public virtual bool ErrorIsZero(Vector3 err)
|
||||||
|
{
|
||||||
|
return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
|
||||||
}
|
}
|
||||||
|
|
||||||
public BSVMotor(string useName)
|
public BSVMotor(string useName)
|
||||||
|
@ -148,7 +148,7 @@ public class BSVMotor : BSMotor
|
||||||
|
|
||||||
Vector3 correction = Vector3.Zero;
|
Vector3 correction = Vector3.Zero;
|
||||||
Vector3 error = TargetValue - CurrentValue;
|
Vector3 error = TargetValue - CurrentValue;
|
||||||
if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
|
if (!ErrorIsZero(error))
|
||||||
{
|
{
|
||||||
correction = Step(timeStep, error);
|
correction = Step(timeStep, error);
|
||||||
|
|
||||||
|
@ -200,7 +200,7 @@ public class BSVMotor : BSMotor
|
||||||
|
|
||||||
LastError = error;
|
LastError = error;
|
||||||
Vector3 returnCorrection = Vector3.Zero;
|
Vector3 returnCorrection = Vector3.Zero;
|
||||||
if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
|
if (!ErrorIsZero())
|
||||||
{
|
{
|
||||||
// correction = error / secondsItShouldTakeToCorrect
|
// correction = error / secondsItShouldTakeToCorrect
|
||||||
Vector3 correctionAmount;
|
Vector3 correctionAmount;
|
||||||
|
@ -246,32 +246,139 @@ public class BSVMotor : BSMotor
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// ============================================================================
|
||||||
|
// ============================================================================
|
||||||
public class BSFMotor : BSMotor
|
public class BSFMotor : BSMotor
|
||||||
{
|
{
|
||||||
public float TimeScale { get; set; }
|
public virtual float TimeScale { get; set; }
|
||||||
public float DecayTimeScale { get; set; }
|
public virtual float TargetValueDecayTimeScale { get; set; }
|
||||||
public float Friction { get; set; }
|
public virtual float FrictionTimescale { get; set; }
|
||||||
public float Efficiency { get; set; }
|
public virtual float Efficiency { get; set; }
|
||||||
|
|
||||||
public float Target { get; private set; }
|
public virtual float ErrorZeroThreshold { get; set; }
|
||||||
public float CurrentValue { get; private set; }
|
|
||||||
|
public virtual float TargetValue { get; protected set; }
|
||||||
|
public virtual float CurrentValue { get; protected set; }
|
||||||
|
public virtual float LastError { get; protected set; }
|
||||||
|
|
||||||
|
public virtual bool ErrorIsZero()
|
||||||
|
{
|
||||||
|
return ErrorIsZero(LastError);
|
||||||
|
}
|
||||||
|
public virtual bool ErrorIsZero(float err)
|
||||||
|
{
|
||||||
|
return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold);
|
||||||
|
}
|
||||||
|
|
||||||
public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency)
|
public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency)
|
||||||
: base(useName)
|
: base(useName)
|
||||||
{
|
{
|
||||||
|
TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
|
||||||
|
Efficiency = 1f;
|
||||||
|
FrictionTimescale = BSMotor.Infinite;
|
||||||
|
CurrentValue = TargetValue = 0f;
|
||||||
|
ErrorZeroThreshold = 0.01f;
|
||||||
}
|
}
|
||||||
public void SetCurrent(float target)
|
public void SetCurrent(float current)
|
||||||
{
|
{
|
||||||
|
CurrentValue = current;
|
||||||
}
|
}
|
||||||
public void SetTarget(float target)
|
public void SetTarget(float target)
|
||||||
{
|
{
|
||||||
|
TargetValue = target;
|
||||||
}
|
}
|
||||||
|
public override void Zero()
|
||||||
|
{
|
||||||
|
base.Zero();
|
||||||
|
CurrentValue = TargetValue = 0f;
|
||||||
|
}
|
||||||
|
|
||||||
public virtual float Step(float timeStep)
|
public virtual float Step(float timeStep)
|
||||||
{
|
{
|
||||||
return 0f;
|
if (!Enabled) return TargetValue;
|
||||||
|
|
||||||
|
float origTarget = TargetValue; // DEBUG
|
||||||
|
float origCurrVal = CurrentValue; // DEBUG
|
||||||
|
|
||||||
|
float correction = 0f;
|
||||||
|
float error = TargetValue - CurrentValue;
|
||||||
|
if (!ErrorIsZero(error))
|
||||||
|
{
|
||||||
|
correction = Step(timeStep, error);
|
||||||
|
|
||||||
|
CurrentValue += correction;
|
||||||
|
|
||||||
|
// The desired value reduces to zero which also reduces the difference with current.
|
||||||
|
// If the decay time is infinite, don't decay at all.
|
||||||
|
float decayFactor = 0f;
|
||||||
|
if (TargetValueDecayTimeScale != BSMotor.Infinite)
|
||||||
|
{
|
||||||
|
decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
|
||||||
|
TargetValue *= (1f - decayFactor);
|
||||||
|
}
|
||||||
|
|
||||||
|
// The amount we can correct the error is reduced by the friction
|
||||||
|
float frictionFactor = 0f;
|
||||||
|
if (FrictionTimescale != BSMotor.Infinite)
|
||||||
|
{
|
||||||
|
// frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
|
||||||
|
// Individual friction components can be 'infinite' so compute each separately.
|
||||||
|
frictionFactor = 1f / FrictionTimescale;
|
||||||
|
frictionFactor *= timeStep;
|
||||||
|
CurrentValue *= (1f - frictionFactor);
|
||||||
|
}
|
||||||
|
|
||||||
|
MDetailLog("{0}, BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
|
||||||
|
BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
|
||||||
|
timeStep, error, correction);
|
||||||
|
MDetailLog("{0}, BSFMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}",
|
||||||
|
BSScene.DetailLogZero, UseName,
|
||||||
|
TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor,
|
||||||
|
TargetValue, CurrentValue);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Difference between what we have and target is small. Motor is done.
|
||||||
|
CurrentValue = TargetValue;
|
||||||
|
MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
|
||||||
|
BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
|
||||||
|
}
|
||||||
|
|
||||||
|
return CurrentValue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public virtual float Step(float timeStep, float error)
|
||||||
|
{
|
||||||
|
if (!Enabled) return 0f;
|
||||||
|
|
||||||
|
LastError = error;
|
||||||
|
float returnCorrection = 0f;
|
||||||
|
if (!ErrorIsZero())
|
||||||
|
{
|
||||||
|
// correction = error / secondsItShouldTakeToCorrect
|
||||||
|
float correctionAmount;
|
||||||
|
if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
|
||||||
|
correctionAmount = error * timeStep;
|
||||||
|
else
|
||||||
|
correctionAmount = error / TimeScale * timeStep;
|
||||||
|
|
||||||
|
returnCorrection = correctionAmount;
|
||||||
|
MDetailLog("{0}, BSFMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
|
||||||
|
BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
|
||||||
|
}
|
||||||
|
return returnCorrection;
|
||||||
|
}
|
||||||
|
|
||||||
|
public override string ToString()
|
||||||
|
{
|
||||||
|
return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4},frictTS={5}>",
|
||||||
|
UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale);
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// ============================================================================
|
||||||
|
// ============================================================================
|
||||||
// Proportional, Integral, Derivitive Motor
|
// Proportional, Integral, Derivitive Motor
|
||||||
// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
|
// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
|
||||||
public class BSPIDVMotor : BSVMotor
|
public class BSPIDVMotor : BSVMotor
|
||||||
|
@ -319,6 +426,7 @@ public class BSPIDVMotor : BSVMotor
|
||||||
proportionFactor = new Vector3(factor, factor, factor);
|
proportionFactor = new Vector3(factor, factor, factor);
|
||||||
integralFactor = new Vector3(factor, factor, factor);
|
integralFactor = new Vector3(factor, factor, factor);
|
||||||
derivFactor = new Vector3(factor, factor, factor);
|
derivFactor = new Vector3(factor, factor, factor);
|
||||||
|
MDetailLog("{0},BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -341,6 +449,9 @@ public class BSPIDVMotor : BSVMotor
|
||||||
+ derivFactor * derivFactor
|
+ derivFactor * derivFactor
|
||||||
);
|
);
|
||||||
|
|
||||||
|
MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},derivFact={4},ret={5}",
|
||||||
|
BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivFactor, ret);
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue