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; } }