public void Logic()
{
// The wind wrapped 180 for when waypoint into the wind
double rotateWind = myMath.Wrap(mySailboat.WindDirection + Math.PI, 2 * Math.PI);
switch (Attack)
{
case SailingTechnique.Setup:
//Tack
if (Invert(rotateWind - mySailboat.Heading, 2 * Math.PI) < (Math.PI / 6.0))
{
Attack = SailingTechnique.CloseHaul;
mySailboat.Bearing = rotateWind + TackDirection(rotateWind);
Direction(mySailboat.Bearing, mySailboat.Orientation);
//Bearing = Heading;
//Bearing = TackDirection(rotateWind, Orientation);
}
// With the wind
else
{
Attack = SailingTechnique.Direct;
Direction(mySailboat.Heading, mySailboat.Orientation);
mySailboat.Bearing = mySailboat.Heading;
}
// TODO Reaching case
break;
case SailingTechnique.CloseHaul:
double distanceToTarget = Math.Sqrt(Math.Pow(mySailboat.Lattitude - mySailboat.HeadingY, 2) + Math.Pow(mySailboat.Longitude - mySailboat.HeadingX, 2)) / 10.0;
double distanceAfterMoving = Math.Sqrt(Math.Pow((mySailboat.Lattitude - Math.Cos(mySailboat.Orientation)) - mySailboat.HeadingY, 2) + Math.Pow((mySailboat.Longitude - Math.Sin(mySailboat.Orientation)) - mySailboat.HeadingX, 2)) / 10.0;
double Adjustment = Math.Sin(mySailboat.Heading - rotateWind);
double dx = mySailboat.HeadingX - (mySailboat.Longitude - Math.Sin(mySailboat.Orientation));
double dy = (mySailboat.Lattitude - (Math.Cos(mySailboat.Orientation))) - mySailboat.HeadingY;
double tempTheta = ((Math.Atan((dy) / (dx)) + Math.PI * Convert.ToInt64((dy < 0 && dx < 0) || (dy > 0 && dx < 0))) - (Math.PI / 2));
double Adjustment2 = Math.Sin(tempTheta - rotateWind);
double Exit = rotateWind - mySailboat.Heading;
if (Invert(rotateWind - mySailboat.Heading, 2 * Math.PI) > Math.PI / 5)
{
Attack = SailingTechnique.Setup;
}
else if (Math.Abs(distanceToTarget * Adjustment) > TackWidth &&
Math.Abs(distanceToTarget * Adjustment) < Math.Abs(distanceAfterMoving * Adjustment2))
{
mySailboat.Bearing = rotateWind - TackDirection(rotateWind);
Direction(mySailboat.Bearing, mySailboat.Orientation);
Attack = SailingTechnique.Tack;
}
break;
case SailingTechnique.Tack:
if (Math.Abs(mySailboat.Bearing - mySailboat.Orientation) < Math.PI / 180)
{
Attack = SailingTechnique.Setup;
}
break;
case SailingTechnique.Direct:
if (Math.Abs(mySailboat.Heading - mySailboat.Bearing) > Math.PI / 180)
{
Attack = SailingTechnique.Setup;
}
break;
}
// Check if target reached
if (Math.Abs(mySailboat.Lattitude - mySailboat.HeadingY) > 10 || Math.Abs(mySailboat.Longitude - mySailboat.HeadingX) > 10)
{
if (Math.Abs(mySailboat.Orientation - mySailboat.Bearing) > Math.PI / 180)
{
mySailboat.Orientation += Math.PI * (1 - 2 * (int)AttackDirection) / 180.0;
}
mySailboat.Lattitude = mySailboat.Lattitude - (Math.Cos(mySailboat.Orientation));
mySailboat.Longitude = mySailboat.Longitude - Math.Sin(mySailboat.Orientation);
mySailboat.DrawBoat();
}
// Target reached
// TODO Go to next waypoint
else
{
mySailboat.OpModeChanged(true);
}
}
public void Logic()
{
// The wind wrapped 180 for when waypoint into the wind
double rotateWind = myMath.Wrap(mySailboat.WindDirection + Math.PI, 2 * Math.PI);
switch (Attack)
{
case SailingTechnique.Setup:
//Tack
if (Invert(rotateWind - mySailboat.Heading,2*Math.PI) < (Math.PI / 6.0))
{
Attack = SailingTechnique.CloseHaul;
mySailboat.Bearing = rotateWind + TackDirection(rotateWind);
Direction(mySailboat.Bearing, mySailboat.Orientation);
//Bearing = Heading;
//Bearing = TackDirection(rotateWind, Orientation);
}
// With the wind
else
{
Attack = SailingTechnique.Direct;
Direction(mySailboat.Heading, mySailboat.Orientation);
mySailboat.Bearing = mySailboat.Heading;
}
// TODO Reaching case
break;
case SailingTechnique.CloseHaul:
double distanceToTarget = Math.Sqrt(Math.Pow(mySailboat.Lattitude - mySailboat.HeadingY, 2) + Math.Pow(mySailboat.Longitude - mySailboat.HeadingX, 2)) / 10.0;
double distanceAfterMoving = Math.Sqrt(Math.Pow((mySailboat.Lattitude - Math.Cos(mySailboat.Orientation)) - mySailboat.HeadingY, 2) + Math.Pow((mySailboat.Longitude - Math.Sin(mySailboat.Orientation)) - mySailboat.HeadingX, 2)) / 10.0;
double Adjustment = Math.Sin(mySailboat.Heading-rotateWind);
double dx = mySailboat.HeadingX - (mySailboat.Longitude - Math.Sin(mySailboat.Orientation));
double dy = (mySailboat.Lattitude - (Math.Cos(mySailboat.Orientation))) - mySailboat.HeadingY;
double tempTheta = ((Math.Atan((dy) / (dx)) + Math.PI * Convert.ToInt64((dy < 0 && dx < 0) || (dy > 0 && dx < 0))) - (Math.PI / 2));
double Adjustment2 = Math.Sin(tempTheta-rotateWind);
double Exit = rotateWind - mySailboat.Heading;
if (Invert(rotateWind - mySailboat.Heading,2*Math.PI) > Math.PI / 5)
{
Attack = SailingTechnique.Setup;
}
else if(Math.Abs(distanceToTarget*Adjustment)>TackWidth
&& Math.Abs(distanceToTarget*Adjustment)<Math.Abs(distanceAfterMoving*Adjustment2))
{
mySailboat.Bearing = rotateWind - TackDirection(rotateWind);
Direction(mySailboat.Bearing, mySailboat.Orientation);
Attack = SailingTechnique.Tack;
}
break;
case SailingTechnique.Tack:
if (Math.Abs(mySailboat.Bearing-mySailboat.Orientation) < Math.PI/180)
Attack = SailingTechnique.Setup;
break;
case SailingTechnique.Direct:
if (Math.Abs(mySailboat.Heading - mySailboat.Bearing) > Math.PI / 180)
{
Attack = SailingTechnique.Setup;
}
break;
}
// Check if target reached
if (Math.Abs(mySailboat.Lattitude - mySailboat.HeadingY) > 10 || Math.Abs(mySailboat.Longitude - mySailboat.HeadingX) > 10)
{
if (Math.Abs(mySailboat.Orientation - mySailboat.Bearing) > Math.PI / 180)
{
mySailboat.Orientation += Math.PI * (1 - 2 * (int)AttackDirection) / 180.0;
}
mySailboat.Lattitude = mySailboat.Lattitude - (Math.Cos(mySailboat.Orientation));
mySailboat.Longitude = mySailboat.Longitude - Math.Sin(mySailboat.Orientation);
mySailboat.DrawBoat();
}
// Target reached
// TODO Go to next waypoint
else
{ mySailboat.OpModeChanged(true); }
}
public void Reset()
{
Attack = SailingTechnique.Setup;
}
public void Reset()
{
Attack = SailingTechnique.Setup;
}
