private double penAng_linearInterp(List <Arduino_MeasPoint> list, PNA_MeasPoint point)
{
double penAng = 180;
// find left index
int li = 0;
int ri = 1;
while (list[li].time < point.time && li < list.Count - 1)
{
li++;
}
if (li > 0 && li < list.Count - 1) // point is not inside the list
{
ri = li + 1;
double k = (list[ri].penAng - list[li].penAng) / (list[ri].time - list[li].time);
penAng = list[li].penAng + k * (point.time - list[li].time);
}
return(penAng);
}
private void processMeasuredData()
{
this.processed_MeasList = new List <System_MeasPoint>();
for (int i = 0; i < this.pna_MeasList.Count; i++)
{
List <PNA_MeasPoint> pmpl = this.pna_MeasList[i];
List <Arduino_MeasPoint> ampl = this.penAng_MeasList[i];
List <Motor_MeasPoint> mmpl = this.motorAng_MeasList[i];
bool isNormPolar = this.isNormPolarList[i];
for (int j = 0; j < pmpl.Count; j++)
{
PNA_MeasPoint pmp = pmpl[j];
System_MeasPoint smp = this.kinematics(mmpl, ampl, pmp, isNormPolar);
if (Math.Abs(smp.penAng) <= Globals.TARGET_ANGLE)
{
this.processed_MeasList.Add(smp);
}
}
}
}
private void threadRun_PNA()
{
this.pna_inMeas = true;
List <PNA_MeasPoint> pmpl = new List <PNA_MeasPoint>();
this.pna.configMeasurement(this.frequency, this.maxNumOfPoint, this.ifbw);
int triggerCount = 0;
while (this.motor_inMotion)
{
PNA_MeasPoint pmp;
if (debug)
{
this.penAngDebug.Add(this.arduino.GetInstanceAngle());
}
this.pna.manualTrigger(); // this timing is umpredictable
pmp.time = system_watch.Elapsed.TotalMilliseconds;
pmp.S21_imag = 0.0F; //dummy value
pmp.S21_real = 0.0F; //dummy value
pmpl.Add(pmp);
triggerCount++;
if (!debug)
{
this.nop(Globals.TRIGGER_TIME_INTERVAL); // measurement time interval
}
}
this.triggerCountList.Add(triggerCount);
float[,] E_comp = this.pna.outputData(triggerCount);
for (int i = 0; i < triggerCount; i++)
{
PNA_MeasPoint temp = pmpl[i];
temp.S21_real = E_comp[i, 0];
temp.S21_imag = E_comp[i, 1];
pmpl[i] = temp;
}
this.pna_MeasList.Add(pmpl);
this.pna_inMeas = false;
}
private double motorAng_linearInterp(List <Motor_MeasPoint> list, PNA_MeasPoint point)
{
double motorAng = -1.0;
// find left index
int li = 0;
int ri = 1;
while (list[li].time < point.time && li < list.Count - 1)
{
li++;
}
if (li > 0 && li < list.Count - 1) // point is not inside the list
{
ri = li + 1;
double k = (list[ri].motorAng - list[li].motorAng) / (list[ri].time - list[li].time);
motorAng = list[li].motorAng + k * (point.time - list[li].time);
}
else
{
motorAng = list[list.Count - 1].motorAng;
}
return(motorAng);
}
private System_MeasPoint kinematics(List <Motor_MeasPoint> mmpl, List <Arduino_MeasPoint> ampl, PNA_MeasPoint pmp, bool isNormPolar)
{
System_MeasPoint smp;
smp.time = pmp.time;
double x, y;
smp.penAng = this.penAng_linearInterp(ampl, pmp);
smp.motorAng = this.motorAng_linearInterp(mmpl, pmp);
smp.S21_real = pmp.S21_real;
smp.S21_imag = pmp.S21_imag;
double penAng = smp.penAng * Math.PI / 180;
double motorAng = smp.motorAng * Math.PI / 180;
// normal pendulum coordinate
x = Math.Sin(-penAng) * Globals.ARM_LENGTH;
y = -Math.Cos(-penAng) * Globals.ARM_LENGTH;
// pendulum highest point at (0,0)
x = x + Math.Sin(Globals.TARGET_ANGLE * Math.PI / 180) * Globals.ARM_LENGTH;
y = y + Math.Cos(Globals.TARGET_ANGLE * Math.PI / 180) * Globals.ARM_LENGTH;
// coor rotation
smp.x = Math.Cos(motorAng) * x - Math.Sin(motorAng) * y;
smp.y = Math.Sin(motorAng) * x + Math.Cos(motorAng) * y;
smp.isNormPolar = isNormPolar;
if (smp.isNormPolar)
{
smp.phaseAng = smp.motorAng - smp.penAng + 90;
}
else
{
smp.phaseAng = smp.motorAng - smp.penAng;
}
return(smp);
}
