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