BulletSim: add Enabled parameter and operation to motors.

0.7.5-pf-bulletsim
Robert Adams 2012-12-22 17:06:13 -08:00
parent 3e3c168987
commit 16e49035f7
1 changed files with 16 additions and 2 deletions

View File

@ -43,7 +43,9 @@ public abstract class BSMotor
{ {
UseName = useName; UseName = useName;
PhysicsScene = null; PhysicsScene = null;
Enabled = true;
} }
public virtual bool Enabled { get; set; }
public virtual void Reset() { } public virtual void Reset() { }
public virtual void Zero() { } public virtual void Zero() { }
public virtual void GenerateTestOutput(float timeStep) { } public virtual void GenerateTestOutput(float timeStep) { }
@ -98,6 +100,12 @@ 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
{ get {
return (LastError == Vector3.Zero || LastError.LengthSquared() <= ErrorZeroThreshold);
}
}
public BSVMotor(string useName) public BSVMotor(string useName)
: base(useName) : base(useName)
{ {
@ -105,7 +113,7 @@ public class BSVMotor : BSMotor
Efficiency = 1f; Efficiency = 1f;
FrictionTimescale = BSMotor.InfiniteVector; FrictionTimescale = BSMotor.InfiniteVector;
CurrentValue = TargetValue = Vector3.Zero; CurrentValue = TargetValue = Vector3.Zero;
ErrorZeroThreshold = 0.01f; ErrorZeroThreshold = 0.001f;
} }
public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency)
: this(useName) : this(useName)
@ -133,6 +141,8 @@ public class BSVMotor : BSMotor
// Compute the next step and return the new current value // Compute the next step and return the new current value
public virtual Vector3 Step(float timeStep) public virtual Vector3 Step(float timeStep)
{ {
if (!Enabled) return TargetValue;
Vector3 origTarget = TargetValue; // DEBUG Vector3 origTarget = TargetValue; // DEBUG
Vector3 origCurrVal = CurrentValue; // DEBUG Vector3 origCurrVal = CurrentValue; // DEBUG
@ -178,7 +188,7 @@ public class BSVMotor : BSMotor
{ {
// Difference between what we have and target is small. Motor is done. // Difference between what we have and target is small. Motor is done.
CurrentValue = TargetValue; CurrentValue = TargetValue;
MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={2}", MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
} }
@ -186,6 +196,8 @@ public class BSVMotor : BSMotor
} }
public virtual Vector3 Step(float timeStep, Vector3 error) public virtual Vector3 Step(float timeStep, Vector3 error)
{ {
if (!Enabled) return Vector3.Zero;
LastError = error; LastError = error;
Vector3 returnCorrection = Vector3.Zero; Vector3 returnCorrection = Vector3.Zero;
if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
@ -313,6 +325,8 @@ public class BSPIDVMotor : BSVMotor
// Ignore Current and Target Values and just advance the PID computation on this error. // Ignore Current and Target Values and just advance the PID computation on this error.
public override Vector3 Step(float timeStep, Vector3 error) public override Vector3 Step(float timeStep, Vector3 error)
{ {
if (!Enabled) return Vector3.Zero;
// Add up the error so we can integrate over the accumulated errors // Add up the error so we can integrate over the accumulated errors
RunningIntegration += error * timeStep; RunningIntegration += error * timeStep;