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 setPanKinect(double degreesFromCenter)
{
currentPanKinect = degreesFromCenter;
double mks = PanTiltAlignment.getInstance().mksPanKinect(degreesFromCenter);
setPanKinectMks((int)mks);
}
private async void Window_Loaded(object sender, EventArgs e)
{
// UI is up at this moment.
_panTiltAlignment = PanTiltAlignment.RestoreOrDefault();
OpenBrickConnector();
await AssumePostureScrollbars(SafePosture);
speak("Ready for action!");
}
private void Window_Loaded(object sender, EventArgs e)
{
_panTiltAlignment = PanTiltAlignment.RestoreOrDefault();
OpenBrickConnector();
SafePosture();
this.ListAnimCombos.ItemsSource = animationCombo.Keys;
this.ListAnim.ItemsSource = animations.Keys;
speak("Ready for action!");
}
private void Window_Loaded(object sender, EventArgs e)
{
_panTiltAlignment = PanTiltAlignment.RestoreOrDefault();
// panKinectTargetPosScrollBar.IsEnabled = false;
SafePosture();
InitKinect();
InitPidControls();
CollectAndApplyPidControls();
dispatcherTimer.Start();
}
public void setTilt(double degreesFromCenter, bool rightGun)
{
double mks = 0.0d;
if (rightGun)
{
currentTiltGunRight = degreesFromCenter;
mks = PanTiltAlignment.getInstance().mksTiltGunRight(degreesFromCenter);
}
else
{
currentTiltGunLeft = degreesFromCenter;
mks = PanTiltAlignment.getInstance().mksTiltGunLeft(degreesFromCenter);
}
setTiltMks((int)mks, rightGun);
}
/// <summary>
/// preferred method for pan/tilt control
/// </summary>
/// <param name="panDegreesFromCenterGunLeft"></param>
/// <param name="tiltDegreesFromCenterGunLeft"></param>
public void setPanTilt(double panDegreesFromCenterGunLeft, double tiltDegreesFromCenterGunLeft, double panDegreesFromCenterGunRight, double tiltDegreesFromCenterGunRight, double?panKinectDegreesFromCenter)
{
currentPanGunLeft = panDegreesFromCenterGunLeft;
double mksPanGunLeft = PanTiltAlignment.getInstance().mksPanGunLeft(panDegreesFromCenterGunLeft);
int panMksGunLeft = (int)mksPanGunLeft;
currentTiltGunLeft = tiltDegreesFromCenterGunLeft;
double mksTiltGunLeft = PanTiltAlignment.getInstance().mksTiltGunLeft(tiltDegreesFromCenterGunLeft);
int tiltMksGunLeft = (int)mksTiltGunLeft;
currentPanGunRight = panDegreesFromCenterGunRight;
double mksPanGunRight = PanTiltAlignment.getInstance().mksPanGunRight(panDegreesFromCenterGunRight);
int panMksGunRight = (int)mksPanGunRight;
currentTiltGunRight = tiltDegreesFromCenterGunRight;
double mksTiltGunRight = PanTiltAlignment.getInstance().mksTiltGunRight(tiltDegreesFromCenterGunRight);
int tiltMksGunRight = (int)mksTiltGunRight;
currentPanKinect = panKinectDegreesFromCenter;
double?mksPanKinect = panKinectDegreesFromCenter.HasGoodValue() ? (double?)PanTiltAlignment.getInstance().mksPanKinect(panKinectDegreesFromCenter.Value) : null;
int panKinectMks = (int)mksPanKinect.GetValueOrDefault();
if (panMksGunLeft != panMksLastGunLeft || tiltMksGunLeft != tiltMksLastGunLeft ||
panMksGunRight != panMksLastGunRight || tiltMksGunRight != tiltMksLastGunRight ||
panKinectDegreesFromCenter.HasGoodValue() && panKinectMks != panKinectMksLast
)
{
panMksLastGunLeft = panMksGunLeft;
tiltMksLastGunLeft = tiltMksGunLeft;
panMksLastGunRight = panMksGunRight;
tiltMksLastGunRight = tiltMksGunRight;
List <ChannelValuePair> channelValues = new List <ChannelValuePair>();
// Pololu servo target is in units of quarter microseconds.
// For typical servos, 6000 is neutral and the acceptable range is 4000-8000.
channelValues.Add(new ChannelValuePair()
{
Channel = ServoChannelMap.leftGunPan, Target = (UInt16)(panMksGunLeft << 2)
});
channelValues.Add(new ChannelValuePair()
{
Channel = ServoChannelMap.leftGunTilt, Target = (UInt16)(tiltMksGunLeft << 2)
});
channelValues.Add(new ChannelValuePair()
{
Channel = ServoChannelMap.rightGunPan, Target = (UInt16)(panMksGunRight << 2)
});
channelValues.Add(new ChannelValuePair()
{
Channel = ServoChannelMap.rightGunTilt, Target = (UInt16)(tiltMksGunRight << 2)
});
if (mksPanKinect.HasValue)
{
channelValues.Add(new ChannelValuePair()
{
Channel = ServoChannelMap.panKinect, Target = (UInt16)(panKinectMks << 2)
});
}
TrySetTarget(channelValues);
}
}
