/*
public static void ResetReadings(int index)
{
G.AccelerationNowX[index] = 0;
G.AccelerationNowY[index] = 0;
G.FilteredAccelerationX[index] = 0;
G.FilteredAccelerationY[index] = 0;
G.FilteredAccelerationMinX[index] = 0;
G.FilteredAccelerationMinY[index] = 0;
G.FilteredAccelerationMaxX[index] = 0;
G.FilteredAccelerationMaxY[index] = 0;
G.RangeX[index] = 0;
G.RangeY[index] = 0;
G.Rating[index] = 0;
G.AllRatings[index] = 0;
G.AvgRatings[index] = 0;
G.Readings[index] = 0;
}
*/
public static void CalculateVibration(Frame f, MotorReading r)
{
//G.AccelerationNowX[index] = f.Get(Symbols.kX);
double x = f.Get(Symbols.kX);
//G.AccelerationNowY[index] = f.Get(Symbols.kY);
double y = f.Get(Symbols.kY);
//G.FilteredAccelerationX[index] = U.LowPass(G.FilteredAccelerationX[index], G.AccelerationNowX[index], K.D);
r.X = U.LowPass(r.X, x, K.D);
//G.FilteredAccelerationY[index] = U.LowPass(G.FilteredAccelerationY[index], G.AccelerationNowY[index], K.D);
r.Y = U.LowPass(r.Y, y, K.D);
//if (G.Readings[index] == 0)
if (r.Readings == 0)
{
//G.FilteredAccelerationX[index] = G.AccelerationNowX[index];
r.X = x;
//G.FilteredAccelerationY[index] = G.AccelerationNowY[index];
r.Y = y;
}
//double hpX = G.AccelerationNowX[index] - G.FilteredAccelerationX[index];
double hpX = x - r.X;
//double hpY = G.AccelerationNowY[index] - G.FilteredAccelerationY[index];
double hpY = y - r.Y;
//double D = G.Readings[index] - K.DiscardUnder;
double D = r.Readings - K.DiscardUnder;
if (D < 0)
{
D = 0;
}
//if (D > 30.0)
if (D > 45.0)
{
// D = 30.0;
D = 45.0;
}
//if (hpX > G.FilteredAccelerationMaxX[index])
// G.FilteredAccelerationMaxX[index] = hpX;
if (hpX > r.MaxX)
r.MaxX = hpX;
//if (hpY > G.FilteredAccelerationMaxY[index])
// G.FilteredAccelerationMaxY[index] = hpY;
if (hpY > r.MaxY)
r.MaxY = hpY;
//if (hpX < G.FilteredAccelerationMinX[index])
// G.FilteredAccelerationMinX[index] = hpX;
if (hpX < r.MinX)
r.MinX = hpX;
//if (hpY < G.FilteredAccelerationMinY[index])
// G.FilteredAccelerationMinY[index] = hpY;
if (hpY < r.MinY)
r.MinY = hpY;
//if (G.Readings[index] >= K.DiscardUnder || G.MotorIndex == 4)
if (r.Readings >= K.DiscardUnder || r == G.WaitReading)
{
//G.FilteredAccelerationMaxX[index] *= (D / (D+1.0));
r.MaxX *= (D/(D + 1.0));
//G.FilteredAccelerationMaxY[index] *= (D / (D+1.0));
r.MaxY *= (D/(D + 1.0));
//G.FilteredAccelerationMinX[index] *= (D / (D+1.0));
r.MinX *= (D/(D + 1.0));
//G.FilteredAccelerationMinY[index] *= (D / (D+1.0));
r.MinY *= (D/(D + 1.0));
}
//if (G.Readings[index] >= K.DiscardUnder)
if (r.Readings >= K.DiscardUnder)
{
//double rangeX = G.FilteredAccelerationMaxX[index] - G.FilteredAccelerationMinX[index];
double rangeX = r.MaxX - r.MinX;
//double rangeY = G.FilteredAccelerationMaxY[index] - G.FilteredAccelerationMinY[index];
double rangeY = r.MaxY - r.MinY;
//G.RangeX[index] = U.LowPass(G.RangeX[index], rangeX, 10.0);
r.RangeX = U.LowPass(r.RangeX, rangeX, 10.0);
//G.RangeY[index] = U.LowPass(G.RangeY[index], rangeY, 10.0);
r.RangeY = U.LowPass(r.RangeX, rangeY, 10.0);
//G.Rating[index] = 5.0 - ((((G.RangeX[index] + G.RangeY[index])*0.5)-200.0)/70.0);
r.RawRating = 5.0 - ((((r.RangeX + r.RangeY)*0.5)-200.0)/70.0);
//if (G.Rating[index] > 5.0)
if (r.RawRating > 5.0)
//G.Rating[index] = 5.0;
r.RawRating = 5.0;
//else if (G.Rating[index] < 0.0)
else if (r.RawRating < 0.0)
//G.Rating[index] = 0.0;
r.RawRating = 0.0;
///// G.AllRatings[index] += G.Rating[index];
//G.AvgRatings[index] = U.LowPass(G.AvgRatings[index], G.Rating[index], 10.0); //G.AllRatings[index]/G.Readings[index];
//r.Rating = U.LowPass(r.Rating, r.RawRating, 10.0);
r.Rating = U.LowPass(r.Rating, r.RawRating, 20.0);
}
}
public static void MoveReadingsOver()
{
foreach (var mr in MotorReadings)
{
OldReadings.Add(mr);
}
MotorReadings.Clear();
if (CurrentMotorReading != null)
{
OldReadings.Add(CurrentMotorReading);
CurrentMotorReading = null;
}
}
public static MotorReading PopMotorReading()
{
if (CurrentMotorReading != null)
{
OldReadings.Add(CurrentMotorReading);
CurrentMotorReading = null;
}
if (MotorReadings.Count == 0)
{
return null;
}
CurrentMotorReading = MotorReadings[0];
MotorReadings.RemoveAt(0);
return CurrentMotorReading;
}