private double CalculatePredictedTimeOfFlight()
{
var zCoeffs = polyfitZ.CalculateCoefficients();
double a = zCoeffs[2];
double b = zCoeffs[1];
double c = zCoeffs[0] - targetHitHeight;
if (a >= 0.0)
{
return(-1.0);
}
double delta = b * b - 4.0 * a * c;
if (delta < 0)
{
return(-1.0);
}
else
{
return((-b - Math.Sqrt(delta)) / (2.0 * a));
}
}
public void AddMeasurement(Vector <double> position, double elapsedTime)
{
if (polyfitZ.Values.Count == maxPolyfitSamples)
{
ShiftPolyfitSamples();
}
polyfitX.AddPoint(elapsedTime, position[0]);
polyfitY.AddPoint(elapsedTime, position[1]);
polyfitZ.AddPoint(elapsedTime, position[2]);
if (SamplesCount < 30) //TODO: DO TESTOWANIA - KIEDY MA WYSTARTOWAC
{
IsReady = false;
SamplesCount++;
return;
}
double positionX = 0;
double positionY = 0;
double positionZ = 0;
double velocityX = 0;
double velocityY = 0;
double velocityZ = 0;
var zCoeffs = polyfitZ.CalculateCoefficients();
double timeOfFlight = CalculatePredictedTime(zCoeffs[0], zCoeffs[1], zCoeffs[2], TargetHitHeight);
if (timeOfFlight > 0.0 && IsPredictedTimeStable(timeOfFlight))
{
TimeToHit = timeOfFlight - elapsedTime;
IsReady = true;
var xCoeffs = polyfitX.CalculateCoefficients();
var yCoeffs = polyfitY.CalculateCoefficients();
positionX = xCoeffs[1] * timeOfFlight + xCoeffs[0];
positionY = yCoeffs[1] * timeOfFlight + yCoeffs[0];
positionZ = TargetHitHeight;
velocityX = xCoeffs[1];
velocityY = yCoeffs[1];
velocityZ = 2.0 * zCoeffs[2] * timeOfFlight + zCoeffs[1];
}
else
{
TimeToHit = -1;
IsReady = false;
}
Position = Vector <double> .Build.DenseOfArray(new double[] {
positionX, positionY, positionZ
});
Velocity = Vector <double> .Build.DenseOfArray(new double[] {
velocityX, velocityY, velocityZ
});
SamplesCount++;
}
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;
//}
}
}
}
}
