public FilteredData(ulong ID, string source, float[] position, float[] velocity, float[] rawposition, float[] head, double clock, Photodiode pd)
{
this.ID = ID;
this.source = source;
this.position = position;
this.velocity = velocity;
this.rawposition = rawposition;
this.head = head;
this.clock = clock;
this.photodiode = pd;
}
protected override void Process(FilteredData data)
{
counter += 1;
if (counter == photodiodeShadowFrames)
{
counter = 0;
constant = -constant;
this.positionService.position.X += constant;
this.positionService.position.Y += constant;
photoStatus = !photoStatus;
}
photodiode = data.photodiode;
if (photodiode != null)
{
photoLvl = photodiode.Read;
// SEE PHOTODIODE RESULT
// Console.WriteLine(photoLvl);
}
this.Send <PhotoData>(new PhotoData(this.positionService.position.X, photoStatus, photoLvl, data.ID));
}
public KalmanFilterTrack(bool use, bool twoflies, Photodiode pd)
{
//initialize new kalman filter with appropriate number of parameters
kal = new Kalman(6, 3, 0);
mk = new ModelKalman();
Matrix <float> state = new Matrix <float>(new float[]
{
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
});
//define kalman filter according with the kalman model
kal.CorrectedState = state;
kal.TransitionMatrix = mk.transitionMatrix;
kal.MeasurementNoiseCovariance = mk.measurementNoise;
kal.ProcessNoiseCovariance = mk.processNoise;
kal.ErrorCovariancePost = mk.errorCovariancePost;
kal.MeasurementMatrix = mk.measurementMatrix;
pars = new float[3];
this.use = use;
this.twoflies = twoflies;
photodiode = pd;
}
public void Initialize(int Size)
{
// Initialize vr components
render = new RenderSubsystem(this, new StaticCamera(Size), Size);
update = new UpdateSubsystem(this);
// Load virtual world
if (protName == null)
{
GetStimulus();
}
else
{
GetStimulus(protName);
}
// Initialize Photodiode & Kalman filter
if (vRProtocol.usePhotodiode)
{
pd = new Photodiode(vRProtocol.portPhotodiode);
pd.StartPhotodiode();
}
kft = new KalmanFilterTrack <MovementData>(false, vRProtocol.twoFlies, pd);
// Initialize data acquisition objects
if (vRProtocol.usePulsePal)
{
pp = new PulsePal <MovementData>(vRProtocol.portPulsePal);
pp.StartPulsePal();
}
if (vRProtocol.useCam1)
{
cam1 = new uEyeCamera(0, vRProtocol.paramsPathCam1, vRProtocol.trackCam1, vRProtocol.dispCam1, 0, vRProtocol.fpsCam1, null);
while (!cam1.m_IsLive)
{
}
if (cam1.m_IsLive)
{
cam1.Start();
if (vRProtocol.trackCam1)
{
fastT = new FastTracking <Frame>(this, cam1.firstFrame, 1, 100, 50, 0, vRProtocol.twoFlies, true);
fastT.Start();
kft.Start();
}
}
}
if (vRProtocol.useCam2)
{
if (vRProtocol.usePulsePal)
{
cam2 = new uEyeCamera(1, vRProtocol.paramsPathCam2, vRProtocol.trackCam2, vRProtocol.dispCam2, 800, vRProtocol.fpsCam2, pp);
}
else
{
cam2 = new uEyeCamera(1, vRProtocol.paramsPathCam2, vRProtocol.trackCam2, vRProtocol.dispCam2, 800, vRProtocol.fpsCam2, null);
}
if (cam2.m_IsLive)
{
cam2.Start();
if (vRProtocol.trackCam2)
{
fastT = new FastTracking <Frame>(this, cam2.firstFrame, 10, 5000, 35, 0, vRProtocol.twoFlies, false);
fastT.Start();
kft.Start();
}
}
}
if (vRProtocol.recordTracking & vRProtocol.useCam2)
{
dataRecorder = new DataRecorder <FilteredData>(vRProtocol.recordPathTracking, cam2, true, vRProtocol.twoFlies, this);
dataRecorder.Start();
}
else if (vRProtocol.recordTracking & !vRProtocol.useCam2)
{
dataRecorder = new DataRecorder <FilteredData>(vRProtocol.recordPathTracking, true, vRProtocol.twoFlies, this);
dataRecorder.Start();
}
if (vRProtocol.recordStimulus & vRProtocol.useCam2)
{
stimRecorder = new StimRecorder <StimData>(vRProtocol.recordPathStimulus, cam2, true, this);
stimRecorder.Start();
}
else if (vRProtocol.recordStimulus & !vRProtocol.useCam2)
{
stimRecorder = new StimRecorder <StimData>(vRProtocol.recordPathStimulus, true, this);
stimRecorder.Start();
}
if (vRProtocol.recordPhotodiode & vRProtocol.useCam2)
{
photoRecorder = new PhotoRecorder <PhotoData>(vRProtocol.recordPathPhotodiode, cam2, true, this);
photoRecorder.Start();
}
else if (vRProtocol.recordPhotodiode & !vRProtocol.useCam2)
{
photoRecorder = new PhotoRecorder <PhotoData>(vRProtocol.recordPathPhotodiode, true, this);
photoRecorder.Start();
}
}
