public virtual void ReceiveData(Data data) { allData.AddLast(data); data.AddWayPoint(this.Output.Center); data.Cruise(); SetHasData(); }
private void SyncData(double threshold = 2) { if (allData.Count < 2) { return; } int lastIndex = allData.Count - 1; int dataSynched = 0; for (int i = 0; i < lastIndex; i++) { int dataIndex = lastIndex - i; Data data = allData.ElementAt(dataIndex); int previousIndex = dataIndex - 1; Data previousData = allData.ElementAt(previousIndex); float clockwiseArcAngle = (float)Program.GetClockwiseArcAngle(data.CenterF, previousData.CenterF, this.Center); //float lastDataAngle = (float)Program.GetAbsAngle(this.Center, bufferData.CenterF); //float secondLastDataAngle = (float)Program.GetAbsAngle(this.Center, previousData.CenterF); //float clockwiseArcAngle = (secondLastDataAngle - lastDataAngle) % 360; //if (clockwiseArcAngle < 0) //{ // clockwiseArcAngle += 360; //} float desiredArcAngle = 360 / DataSlots; float angleDif = Math.Abs(clockwiseArcAngle - desiredArcAngle); if (angleDif < threshold) { data.Cruise(); dataSynched++; continue; } float currentVelocity = data.Velocity; float cruiseVelocity = data.CruiseVelocity; float halfCruise = cruiseVelocity / 2; if (clockwiseArcAngle > desiredArcAngle) { if (currentVelocity > cruiseVelocity) { continue; } data.ChangeVelocity(halfCruise); } else { if (currentVelocity < cruiseVelocity) { continue; } data.ChangeVelocity(-halfCruise); } } }