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); }