public Sample Parse(string rawSample)
{
string[] pieces = rawSample.Split(' ');
Sample sample = new Sample();
try
{
const int DEFAULT_VALUE = -100000;
int partIndex = 0;
for (int i = 0; i < pieces.Length; i++)
{
if (!string.IsNullOrEmpty(pieces[i]) && pieces[i] != "\r")
{
int value = DEFAULT_VALUE;
int.TryParse(pieces[i], out value);
if (value != DEFAULT_VALUE)
{
sample.parts[partIndex] = value;
partIndex++;
}
}
}
}
catch (Exception e)
{
throw new InvalidFormatException(rawSample, e);
}
return sample;
}
public static Sample operator +(Sample a, Sample b)
{
Sample result = new Sample();
for(int i = 0; i < a.parts.Length; i++)
{
result.parts[i] = a.parts[i] + b.parts[i];
}
return result;
}
public static Sample operator -(Sample a, Sample b)
{
Sample result = new Sample();
for (int i = 0; i < a.parts.Length; i++)
{
var difference = a.parts[i] - b.parts[i];
if(Math.Abs(a.parts[i] - b.parts[i]) > 10)
{
result.parts[i] = a.parts[i] - b.parts[i];
}
}
return result;
}
private void UpdateChart(Sample sample, Sample raw, RowingSignal rowingSignal)
{
if (chart.InvokeRequired)
{
try
{
UpdateChartCallback d = new UpdateChartCallback(UpdateChart);
Invoke(d, new object[] { sample, raw, rowingSignal });
}
catch (ObjectDisposedException e)
{
Console.WriteLine(e.ToString()); // swallow the exception (this happens when killing the window)
}
}
else
{
if (String.IsNullOrEmpty(X) || String.IsNullOrEmpty(Y))
return;
double valueX = 0, valueY = 0;
double rawX = 0, rawY = 0;
switch (X)
{
case "X":
valueX = sample.x;
rawX = raw.x;
break;
case "Y":
valueX = sample.y;
rawX = raw.y;
break;
case "Z":
valueX = sample.z;
rawX = raw.z;
break;
case "A":
valueX = sample.a;
rawX = raw.a;
break;
case "B":
valueX = sample.b;
rawX = raw.b;
break;
case "C":
valueX = sample.c;
rawX = raw.c;
break;
case "time":
if(time >= MAXX)
{
time = MINX;
rowingSignals.Points.Clear();
}
valueX = time;
rawX = time;
time += X_STEP;
break;
default: return;
}
switch (Y)
{
case "X":
valueY = sample.x;
rawY = raw.x;
break;
case "Y":
valueY = sample.y;
rawY = raw.y;
break;
case "Z":
valueY = sample.z;
rawY = raw.z;
break;
case "A":
valueY = sample.a;
rawY = raw.a;
break;
case "B":
valueY = sample.b;
rawY = raw.b;
break;
case "C":
valueY = sample.c;
rawY = raw.c;
break;
default: return;
}
readsAccumulated.Points.AddXY(valueX, valueY);
lblXValue.Text = valueX.ToString();
lblYValue.Text = valueY.ToString();
lblYRaw.Text = rawY.ToString();
readsRaw.Points.AddXY(rawX, rawY);
if (readsAccumulated.Points.Count > LINE_LENGTH_IN_SAMPLES)
{
readsAccumulated.Points.RemoveAt(0);
}
if(readsRaw.Points.Count > LINE_LENGTH_IN_SAMPLES)
{
readsRaw.Points.RemoveAt(0);
}
if(rowingSignal != null)
{
rowingSignals.Points.AddXY(rawX, rawY);
if(rowingSignal.StrokeTime > 0)
{
lblStrokeRate.Text = rowingSignal.SmoothedStrokeRate.ToString("0.0");
}
}
}
}
public ComManager(string portName, ArduinoSampleParser sampleParser, Action<Sample, Sample, RowingSignal> updateChart)
{
accumulated = new Sample();
accumulatedTwo = new Sample();
horizontalSignals = new List<RowingSignal>();
mySerialPort = new SerialPort("COM5");
mySerialPort.BaudRate = 38400;
mySerialPort.Parity = Parity.None;
mySerialPort.StopBits = StopBits.One;
mySerialPort.DataBits = 8;
mySerialPort.Handshake = Handshake.None;
mySerialPort.DataReceived += new SerialDataReceivedEventHandler((sender, e) => {
try
{
SerialPort sp = (SerialPort)sender;
string lastRead = sp.ReadLine();
Sample lastSample = sampleParser.Parse(lastRead);
RowingSignal signal = null;
const int STROKE_RATE_SMOOTHING = 5;
double strokeTime = -1;
DateTime signalTime = DateTime.Now;
if (lastSample.a >= HORIZONTAL_SIGNAL_THRESHOLD &&
((horizontalSignals.LastOrDefault() != null && horizontalSignals.Last().Phase == RowingPhases.Catch)
|| horizontalSignals.LastOrDefault() == null)
)
{
var lastRelease = horizontalSignals.LastOrDefault(s => s.Phase == RowingPhases.Release);
if(lastRelease != null)
{
strokeTime = (signalTime - lastRelease.TimeStamp).TotalMilliseconds;
}
signal = new RowingSignal { TimeStamp = signalTime, Phase = RowingPhases.Release, StrokeTime = strokeTime };
horizontalSignals.Add(signal);
}
else if (lastSample.a <= -HORIZONTAL_SIGNAL_THRESHOLD &&
((horizontalSignals.LastOrDefault() != null && horizontalSignals.Last().Phase == RowingPhases.Release)
|| horizontalSignals.LastOrDefault() == null))
{
var lastCatch = horizontalSignals.LastOrDefault(s => s.Phase == RowingPhases.Catch);
if(lastCatch != null)
{
strokeTime = (signalTime - lastCatch.TimeStamp).TotalMilliseconds;
}
signal = new RowingSignal { TimeStamp = signalTime, Phase = RowingPhases.Catch, StrokeTime = strokeTime };
horizontalSignals.Add(signal);
}
if (horizontalSignals.Count >= STROKE_RATE_SMOOTHING)
{
var smoothedStrokeRate = horizontalSignals.OrderByDescending(s => s.TimeStamp).Take(STROKE_RATE_SMOOTHING).Average(s => 60000 / s.StrokeTime);
horizontalSignals.Last().SmoothedStrokeRate = smoothedStrokeRate;
}
SmoothingFunction(lastSample, ref smoothedValue, 0.97);
accumulated = accumulated + (lastSample - smoothedValue);
SmoothingFunction(accumulated, ref smoothAccumulated, 0.97);
accumulatedTwo = accumulatedTwo + ( accumulated - smoothAccumulated);
updateChart(accumulated, lastSample, signal);
}
catch (InvalidFormatException ex)
{
Console.WriteLine(ex.Message);
}
});
mySerialPort.Open();
}
private void SmoothingFunction(Sample s, ref Sample smoothed, double smoothingCoefficient)
{
if (smoothed != null)
{
for (int i = 0; i < s.parts.Length; i++)
{
smoothed.parts[i] = (int)(smoothed.parts[i] - (smoothingCoefficient * (smoothed.parts[i] - s.parts[i])));
}
}
else
{
smoothed = s;
}
}