private Ema analogValue1Ema = new Ema(7); // filter out spikes from imperfect kinect platform pan measurement
void trpbUpdateAnalogNotification(proxibrick.UpdateAnalogData update)
{
//LogInfo("DriveBehaviorServiceBase: trpbUpdateAnalogNotification() value=" + update.Body);
//Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateAnalogNotification() analogValue1=" + update.Body.analogValue1 + " TimeStamp=" + update.Body.TimeStamp);
//Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateAnalogNotification() analogValue1=" + update.Body.analogValue1 + " TimeStamp=" + update.Body.TimeStamp + " angle=" + PanTiltAlignment.getInstance().degreesPanKinect(update.Body.analogValue1));
try
{
proxibrick.AnalogDataDssSerializable pds = update.Body;
double analogValue1 = pds.analogValue1;
// just overwrite the values in place, no need to lock or create another object:
_state.MostRecentAnalogValues.analogValue1 = analogValue1;
_state.MostRecentAnalogValues.TimeStamp = pds.TimeStamp;
analogValue1 = analogValue1Ema.Compute(analogValue1); // filter out spikes from imperfect measurement
_state.currentPanKinect = PanTiltAlignment.getInstance().degreesPanKinect(analogValue1);
computeAndExecuteKinectPlatformTurn();
}
catch (Exception exc)
{
Tracer.Trace("trpbUpdateAnalogNotification() - " + exc);
}
}
public void Init()
{
Dropping = false;
IsTurning = false;
LastTurnStarted = DateTime.MinValue;
LastTurnCompleted = DateTime.MinValue;
if (WheelsEncoderState == null)
{
WheelsEncoderState = new WheelsEncoderState();
}
if (collisionState == null)
{
collisionState = new CollisionState();
}
if (gpsState == null)
{
gpsState = new GpsState();
}
if (MostRecentAnalogValues == null)
{
MostRecentAnalogValues = new proxibrick.AnalogDataDssSerializable()
{
TimeStamp = DateTime.MinValue
};
}
MostRecentLaserTimeStamp = DateTime.Now;
if (VoiceCommandState == null)
{
VoiceCommandState = new VoiceCommandState();
}
int MAX_HUMANS_TO_TRACK = 7; // FrameProcessor preallocates 7
HumanInteractionStates = new HumanInteractionState[MAX_HUMANS_TO_TRACK];
for (int i = 0; i < HumanInteractionStates.Length; i++)
{
HumanInteractionStates[i] = new HumanInteractionState();
}
if (followDirectionPidControllerAngularSpeed == null)
{
followDirectionPidControllerAngularSpeed = new PIDController()
{
Name = "AngularSpeed",
MaxIntegralError = 180.0d, // degrees; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 100.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (followDirectionPidControllerLinearSpeed == null)
{
followDirectionPidControllerLinearSpeed = new PIDController()
{
Name = "LinearSpeed",
MaxIntegralError = 2000.0d, // mm/sec; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 1000.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (PowerScale == 0.0d)
{
PowerScale = 0.5d;
}
}
public void Init()
{
Dropping = false;
IsTurning = false;
LastTurnStarted = DateTime.MinValue;
LastTurnCompleted = DateTime.MinValue;
if (WheelsEncoderState == null)
{
WheelsEncoderState = new WheelsEncoderState();
}
if (collisionState == null)
{
collisionState = new CollisionState();
}
if (gpsState == null)
{
gpsState = new GpsState();
}
if (MostRecentAnalogValues == null)
{
MostRecentAnalogValues = new proxibrick.AnalogDataDssSerializable() { TimeStamp = DateTime.MinValue };
}
MostRecentLaserTimeStamp = DateTime.Now;
if (VoiceCommandState == null)
{
VoiceCommandState = new VoiceCommandState();
}
int MAX_HUMANS_TO_TRACK = 7; // FrameProcessor preallocates 7
HumanInteractionStates = new HumanInteractionState[MAX_HUMANS_TO_TRACK];
for (int i = 0; i < HumanInteractionStates.Length; i++)
{
HumanInteractionStates[i] = new HumanInteractionState();
}
if (followDirectionPidControllerAngularSpeed == null)
{
followDirectionPidControllerAngularSpeed = new PIDController()
{
Name = "AngularSpeed",
MaxIntegralError = 180.0d, // degrees; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 100.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (followDirectionPidControllerLinearSpeed == null)
{
followDirectionPidControllerLinearSpeed = new PIDController()
{
Name = "LinearSpeed",
MaxIntegralError = 2000.0d, // mm/sec; anything more causes controller reset (error too large)
MaxUpdateIntervalSec = 10.0d, // ms; anything more causes controller reset (interval too long)
MaxPidValue = 1000.0d, // pid factor upper limit
MinPidValue = 0.0d, // pid factor lower limit
Kp = PIDController.ProportionalGainDefault, // Proportional constant, 3.0
Ki = PIDController.IntegralGainDefault, // Integral constant, 0.1
Kd = PIDController.DerivativeGainDefault // Derivative constant, 0.5
};
}
if (PowerScale == 0.0d)
{
PowerScale = 0.5d;
}
}