public void ProcessOptiTrackData(OptiTrack.InputFrame data) { if (ballFell) { return; } if (ballSpotted) { elapsedTime += data.FrameDeltaTime; } var ballPosition = robot.OptiTrackTransformation.Convert(data.Position); if (ballPosition[0] < 1200.0 && (ballPosition[0] != prevBallPosition[0] || ballPosition[1] != prevBallPosition[1] || ballPosition[2] != prevBallPosition[2]) ) { ballSpotted = true; if (ballPosition[2] <= ballFellHeight) { ballFell = true; return; } polyfitX.AddPoint(elapsedTime, ballPosition[0]); polyfitY.AddPoint(elapsedTime, ballPosition[1]); polyfitZ.AddPoint(elapsedTime, ballPosition[2]); if (polyfitZ.Values.Count == maxPolyfitPoints) { ShiftPolyfitPoints(); } double T = CalculatePredictedTimeOfFlight(); chart.AddPoint(T, T); if (!ballHit && T > 0.1 && IsPredictedTimeStable(T)) // 0.1 mozna zwiekszyc - do przetestowania { double timeToHit = T - elapsedTime; if (timeToHit >= 0.05) { var xCoeffs = polyfitX.CalculateCoefficients(); var yCoeffs = polyfitY.CalculateCoefficients(); RobotVector predictedHitPosition = new RobotVector( xCoeffs[1] * T + xCoeffs[0], // predicted x yCoeffs[1] * T + yCoeffs[0], // predicted y targetHitHeight, robot.HomePosition.ABC ); Console.WriteLine(predictedHitPosition); if (robot.Limits.WorkspaceLimits.CheckPosition(predictedHitPosition)) { robotMoved = true; RobotVector velocity = new RobotVector(0, 0, 0); //robot.MoveTo(predictedHitPosition, velocity, timeToHit); } } } } prevBallPosition = ballPosition; }
public void ProcessOptiTrackData(OptiTrack.InputFrame data) { // Pozycja przekonwertowana z układu optitracka do układu odpowiedniej KUKI var position = robot.OptiTrackTransformation.Convert(data.Position); double ballX = position[0]; double ballY = position[1]; double ballZ = position[2]; if (ballZ < 0) { ballFell = true; } // Zamiast zabawy w te ify trzeba ogarnac LabeledMarkers w optitracku zeby wykryc kiedy dokladnie widzimy pileczke a kiedy nie if (!ballFell && ballX < 1200 && ballX != 791.016 && ballY != 743.144 && ballZ != 148.319) { Console.WriteLine("cc"); if (polyfitZ.Values.Count == maxPolyfitPoints) { for (int i = 0; i < maxPolyfitPoints / 2; i++) { polyfitX.Values[i] = polyfitX.Values[2 * i]; polyfitY.Values[i] = polyfitY.Values[2 * i]; polyfitZ.Values[i] = polyfitZ.Values[2 * i]; } polyfitX.Values.RemoveRange(maxPolyfitPoints / 2, maxPolyfitPoints / 2); polyfitY.Values.RemoveRange(maxPolyfitPoints / 2, maxPolyfitPoints / 2); polyfitZ.Values.RemoveRange(maxPolyfitPoints / 2, maxPolyfitPoints / 2); } polyfitX.AddPoint(elapsedTime, ballX); polyfitY.AddPoint(elapsedTime, ballY); polyfitZ.AddPoint(elapsedTime, ballZ); var xCoeffs = polyfitX.CalculateCoefficients(); var yCoeffs = polyfitY.CalculateCoefficients(); var zCoeffs = polyfitZ.CalculateCoefficients(); var roots = QuadraticSolver.SolveReal(zCoeffs[2], zCoeffs[1], zCoeffs[0] - zPositionAtHit); elapsedTime += data.FrameDeltaTime; if (roots.Length != 2) { // No real roots return; } double T = roots[1]; chart.AddPoint(T, T); var ballTargetVelocity = Vector <double> .Build.DenseOfArray(new double[] { xCoeffs[1], yCoeffs[1], 2 * zCoeffs[2] * T + zCoeffs[1] }); Vector <double> paddleNormal = Normalize(reflectionVector) - Normalize(ballTargetVelocity); if (IsTimeStable(T) && polyfitX.Values.Count >= 10) { double timeToHit = T - elapsedTime; double predX = xCoeffs[1] * T + xCoeffs[0]; double predY = yCoeffs[1] * T + yCoeffs[0]; Console.WriteLine("T: " + T + " X: " + predX + " Y: " + predY); if (!ballHit && timeToHit >= 0.05) // 0.1 DO SPRAWDZENIA! { double angleB = Math.Atan2(paddleNormal[0], paddleNormal[2]) * 180.0 / Math.PI; double angleC = -90.0 - Math.Atan2(paddleNormal[1], paddleNormal[2]) * 180.0 / Math.PI; RobotVector predictedHitPosition = new RobotVector( predX, predY, zPositionAtHit, 0, angleB, angleC ); //if (robot.Limits.WorkspaceLimits.CheckPosition(predictedHitPosition)) { // //predkosc na osiach w [mm/s] // // RobotVector velocity = new RobotVector(0, 0, 0); // // Dla odwaznych: // RobotVector velocity = new RobotVector(0, 0, 150); // robot.MoveTo(predictedHitPosition, velocity, timeToHit); // robotMoved = true; //} } } } }