public virtual void translate(Distance dist, IDirection dir)
{
double distMeters = dist.Meters;
double dTheta = -DirectionMath.toRad(dir.bearing.Value); // radians; theta is positive to left, bearing - positive to right
this.translate(distMeters * Math.Cos(dTheta), distMeters * Math.Sin(dTheta));
}
/// <summary>
/// Computes drive speed based on requestedSpeed, current DrivingState, timing and sensors input
/// </summary>
/// <returns></returns>
public override IEnumerator <ISubsumptionTask> Execute()
{
FiredOn = true;
while (!MustExit && !MustTerminate)
{
double?gbd = goalBearingDegrees;
if (!gbd.HasValue || behaviorData.robotPose.direction == null || !behaviorData.robotPose.direction.heading.HasValue)
{
// no goal set in RobotState. Stop till it appears.
setSpeedAndTurn(0.0d, 0.0d);
goalBearingDegrees = null; // set it in robot state
}
else if (!GrabByOther())
{
//double heading = behaviorData.sensorsData.CompassHeadingDegrees; // robot's heading by compass
double heading = behaviorData.robotPose.direction.heading.Value; // robot's heading by SLAM
double desiredTurnDegrees = DirectionMath.to180(heading - (double)gbd);
//Debug.WriteLine("BehaviorGoToAngle: desiredTurnDegrees=" + desiredTurnDegrees);
behaviorData.robotState.goalBearingRelativeDegrees = desiredTurnDegrees;
if (Math.Abs(desiredTurnDegrees) > 30.0d)
{
// sharp turn:
setSpeedAndTurn(0.0d, -turnFactor * Math.Sign(desiredTurnDegrees));
}
else
{
// adjust trajectory towards goal:
setSpeedAndTurn(cruiseSpeed, -turnFactor * desiredTurnDegrees / 30.0d);
}
}
yield return(RobotTask.Continue);
}
FiredOn = false;
Debug.WriteLine("BehaviorGoToAngle: " + (MustExit ? "MustExit" : "completed"));
yield break;
}
private void SetCurrentHeadingByTheta(IBehaviorData behaviorData)
{
// XMeters and Lng axes are "horizontal right pointed". YMeters and Lat are "vertical up". North (0 degrees) is up.
// Theta 0 is along the X axis, so we need a 90 degrees offset here.
// Positive Theta is counterclockwise, positive heading - clockwise. That's why the minus sign.
double currentHeading = DirectionMath.toDegrees(-behaviorData.robotPose.ThetaRadians + Math.PI / 2);
if (behaviorData.robotPose.direction != null)
{
behaviorData.robotPose.direction.heading = currentHeading;
}
else
{
behaviorData.robotPose.direction = new Direction()
{
heading = currentHeading,
bearing = behaviorData.robotPose.direction == null ? null : behaviorData.robotPose.direction.bearing
};
}
}
public Direction dir; // straight forward is 0, right is positive
public RelPosition(Direction direction, Distance distance)
{
this.dir = (Direction)direction.Clone();
this.dist = (Distance)distance.Clone();
if (direction.bearingRelative.HasValue)
{
double bearingRad = DirectionMath.toRad(direction.bearingRelative.Value);
XMeters = distance.Meters * Math.Sin(bearingRad);
YMeters = -distance.Meters * Math.Cos(bearingRad);
}
else if (direction.bearing.HasValue)
{
double bearingRad = DirectionMath.toRad(direction.bearing.Value);
XMeters = distance.Meters * Math.Sin(bearingRad);
YMeters = -distance.Meters * Math.Cos(bearingRad);
}
}
/// <summary>
/// Computes goalBearingDegrees based on current location and the next trackpoint
/// </summary>
/// <returns></returns>
public override IEnumerator <ISubsumptionTask> Execute()
{
FiredOn = true;
while (!MustExit && !MustTerminate)
{
nextWp = missionTrack.nextTargetWp;
while (!MustActivate && !MustExit && !MustTerminate) // wait for activation
{
yield return(RobotTask.Continue); // dormant state - no need to calculate
}
if (MustExit || MustTerminate)
{
break;
}
// we have next trackpoint to go to, enter the work loop:
//int i = 0;
while (!MustDeactivate && !MustExit && !MustTerminate)
{
nextWp = missionTrack.nextTargetWp;
if (nextWp == null)
{
break;
}
Direction dirToWp = nextWp.directionToWp(behaviorData.robotPose.geoPosition, behaviorData.robotPose.direction);
Distance distToWp = nextWp.distanceToWp(behaviorData.robotPose.geoPosition);
if (distToWp.Meters < closeEnoughMeters)
{
nextWp.trackpointState = TrackpointState.Passed; // trackpoint will be ignored on the next cycle
//speaker.Speak("Waypoint " + nextWp.number + " reached");
if (nextWp.headingDegrees.HasValue)
{
// if the trackpoint was saved with a heading, we need to turn to that heading:
speaker.Speak("turning to " + Math.Round(nextWp.headingDegrees.Value));
SetGrabByMe();
turnStartedTime = DateTime.Now;
// TODO: we overshoot a lot here. Maybe having a PID controller would help.
while (Math.Abs(behaviorData.robotPose.direction.heading.Value - nextWp.headingDegrees.Value) > trackpointHeadingToleranceDegrees &&
(DateTime.Now - turnStartedTime).TotalSeconds < waitOnTurnSeconds)
{
double heading = behaviorData.robotPose.direction.heading.Value; // robot's heading by SLAM
double desiredTurnDegrees = DirectionMath.to180(heading - nextWp.headingDegrees.Value);
setSpeedAndTurn(0.0d, -turnFactor * Math.Sign(desiredTurnDegrees));
yield return(RobotTask.Continue); // let the chain work
}
if (Math.Abs(behaviorData.robotPose.direction.heading.Value - nextWp.headingDegrees.Value) > trackpointHeadingToleranceDegrees)
{
speaker.Speak("Error: turning to " + Math.Round(nextWp.headingDegrees.Value) + " failed");
}
ClearGrab();
}
// pause and declare to other behaviors that we reached trackpoing:
if (stopStartedTime == null)
{
speaker.Speak("waiting");
stopStartedTime = DateTime.Now;
while ((DateTime.Now - stopStartedTime.Value).TotalSeconds < waitOnTrackpointsSeconds)
{
goalBearingRelativeDegrees = null; // BehaviorGoToAngle will stop the robot
goalDistanceMeters = null;
getCoordinatorData().EnablingRequest = "trackpoint"; // let other behaviors know we are waiting on a trackpoint
yield return(RobotTask.Continue); // wait a bit on the trackpoint
}
stopStartedTime = null;
}
if (missionTrack.nextTargetWp != null)
{
speaker.Speak("proceeding");
}
}
else if (nextWp.estimatedTimeOfArrival.HasValue &&
(DateTime.Now - nextWp.estimatedTimeOfArrival.Value).TotalSeconds > waypointCantReachSeconds)
{
nextWp.trackpointState = TrackpointState.CouldNotReach; // will be ignored on the next cycle
speaker.Speak("Waypoint " + nextWp.number + " could not reach");
}
else
{
goalBearingRelativeDegrees = dirToWp.bearingRelative.Value; // update robotState
goalDistanceMeters = distToWp.Meters;
Debug.WriteLine(string.Format("IP: Trackpoint {0} abs bearing= {1} distToWp.Meters= {2}", nextWp.number, dirToWp.bearing, distToWp.Meters));
if (!nextWp.estimatedTimeOfArrival.HasValue)
{
nextWp.trackpointState = TrackpointState.SelectedAsTarget;
double maxVelocityMSec = ToVelocity(behaviorData.robotState.powerLevelPercent * powerFactor); // m/sec
double timeToReachSec = distToWp.Meters / maxVelocityMSec;
nextWp.estimatedTimeOfArrival = DateTime.Now.AddSeconds(timeToReachSec);
speaker.Speak("Next " + Math.Round(distToWp.Meters) + " " + Math.Round(timeToReachSec));
//speaker.Speak("Next trackpoint " + Math.Round(distToWp.Meters) + " meters away. I expect reaching it in " + Math.Round(timeToReachSec) + " seconds");
}
}
yield return(RobotTask.Continue);
}
speaker.Speak("No more trackpoints");
goalBearingRelativeDegrees = null; // BehaviorGoToAngle will stop the robot
goalDistanceMeters = null;
}
FiredOn = false;
Debug.WriteLine("BehaviorRouteFollowing: " + (MustExit ? "MustExit" : "completed"));
yield break;
}