BulletSim: reorganize motor code a little to pull together common functions.
Add BSFMotor.user_profiles
Robert Adams
parent
44492b3a49
commit
5432180027
|
|
@ -59,10 +59,7 @@ public abstract class BSMotor
|
|||
{
|
||||
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 LastError { get; protected set; }
|
||||
|
||||
public virtual bool ErrorIsZero
|
||||
{ get {
|
||||
return (LastError == Vector3.Zero || LastError.LengthSquared() <= ErrorZeroThreshold);
|
||||
}
|
||||
public virtual bool ErrorIsZero()
|
||||
{
|
||||
return ErrorIsZero(LastError);
|
||||
}
|
||||
public virtual bool ErrorIsZero(Vector3 err)
|
||||
{
|
||||
return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
|
||||
}
|
||||
|
||||
public BSVMotor(string useName)
|
||||
|
|
@ -148,7 +148,7 @@ public class BSVMotor : BSMotor
|
|||
|
||||
Vector3 correction = Vector3.Zero;
|
||||
Vector3 error = TargetValue - CurrentValue;
|
||||
if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
|
||||
if (!ErrorIsZero(error))
|
||||
{
|
||||
correction = Step(timeStep, error);
|
||||
|
||||
|
|
@ -200,7 +200,7 @@ public class BSVMotor : BSMotor
|
|||
|
||||
LastError = error;
|
||||
Vector3 returnCorrection = Vector3.Zero;
|
||||
if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
|
||||
if (!ErrorIsZero())
|
||||
{
|
||||
// correction = error / secondsItShouldTakeToCorrect
|
||||
Vector3 correctionAmount;
|
||||
|
|
@ -246,32 +246,139 @@ public class BSVMotor : BSMotor
|
|||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// ============================================================================
|
||||
public class BSFMotor : BSMotor
|
||||
{
|
||||
public float TimeScale { get; set; }
|
||||
public float DecayTimeScale { get; set; }
|
||||
public float Friction { get; set; }
|
||||
public float Efficiency { get; set; }
|
||||
public virtual float TimeScale { get; set; }
|
||||
public virtual float TargetValueDecayTimeScale { get; set; }
|
||||
public virtual float FrictionTimescale { get; set; }
|
||||
public virtual float Efficiency { get; set; }
|
||||
|
||||
public float Target { get; private set; }
|
||||
public float CurrentValue { get; private set; }
|
||||
public virtual float ErrorZeroThreshold { get; 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)
|
||||
: 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)
|
||||
{
|
||||
TargetValue = target;
|
||||
}
|
||||
public override void Zero()
|
||||
{
|
||||
base.Zero();
|
||||
CurrentValue = TargetValue = 0f;
|
||||
}
|
||||
|
||||
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
|
||||
// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
|
||||
public class BSPIDVMotor : BSVMotor
|
||||
|
|
@ -319,6 +426,7 @@ public class BSPIDVMotor : BSVMotor
|
|||
proportionFactor = new Vector3(factor, factor, factor);
|
||||
integralFactor = 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
|
||||
);
|
||||
|
||||
MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},derivFact={4},ret={5}",
|
||||
BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivFactor, ret);
|
||||
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue