private double GetCost(RobotTwoWheelCommand command)
{
double lookAheadRemaining = lookAhead;
double cost = 0;
PointOnPath closestPoint = path.GetClosest(state.Pose.ToVector2());
PointOnPath lookAheadPoint = path.AdvancePoint(closestPoint, ref lookAheadRemaining);
RobotTwoWheelState newState = RobotTwoWheelModel.Simulate(command, state, (lookAhead - lookAheadRemaining) / command.velocity);
return(newState.Pose.ToVector2().DistanceTo(lookAheadPoint.pt) * DISTANCE_WEIGHT); // - command.velocity * VELOCITY_WEIGHT - command.turn * TURN_WEIGHT;
}
public void UpdatePath(IPath path)
{
if (path == null || path.Count == 0)
{
return;
}
double dist = 0.45;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 goalpointGlobal = lookAhead.pt;
double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
goalpoint = new Vector2(xg, yg);
}
public void UpdatePath(IPath path, LineSegment gap)
{
if (path == null || path.Count == 0)
{
return;
}
/*double dist = 0.45;
* PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
*
* Vector2 goalpointGlobal = lookAhead.pt;
* double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
* double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
* goalpoint = new Vector2(xg, yg);*/
double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 mean = new Vector2((gap.P0.X + gap.P1.X) / 2, (gap.P0.Y + gap.P1.Y) / 2);
double theta = Math.Atan2(gap.P0.Y - gap.P1.Y, gap.P0.X - gap.P1.X);
Vector2 goal1 = mean + 0.25 * (new Vector2(Math.Cos(theta + Math.PI / 2), Math.Sin(theta + Math.PI / 2)));
Vector2 goal2 = mean - 0.25 * (new Vector2(Math.Cos(theta + Math.PI / 2), Math.Sin(theta + Math.PI / 2)));
Vector2 goalpointGlobal = (goal1.DistanceTo(lookAhead.pt) < goal2.DistanceTo(lookAhead.pt)) ? goal1 : goal2;
double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
goalpoint = new Vector2(xg, yg);
/*double dist = 1.5;
* PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
*
* double goal1Y = (gap.P0.Y + gap.P1.Y) / 2 + (gap.P0.X - gap.P1.X) / (gap.Length) * 0.25;
* double goal1X = (gap.P0.X + gap.P1.X) / 2 + (gap.P0.Y - gap.P1.Y) / (gap.Length) * 0.25;
* Vector2 goal1 = new Vector2(goal1X, goal1Y);
*
* double goal2Y = (gap.P0.Y + gap.P1.Y) / 2 + (gap.P0.X - gap.P1.X) / (gap.Length) * -0.25;
* double goal2X = (gap.P0.X + gap.P1.X) / 2 + (gap.P0.Y - gap.P1.Y) / (gap.Length) * -0.25;
* Vector2 goal2 = new Vector2(goal2X, goal2Y);
*
* goalpoint = (goal1.DistanceTo(lookAhead.pt) < goal2.DistanceTo(lookAhead.pt)) ? goal1 : goal2;*/
}
public void UpdatePath(IPath path, LineSegment gap)
{
if (path == null || path.Count == 0 || pose == null)
{
return;
}
double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 mean = new Vector2((gap.P0.X + gap.P1.X) / 2, (gap.P0.Y + gap.P1.Y) / 2);
double theta = Math.Atan2(gap.P0.Y - gap.P1.Y, gap.P0.X - gap.P1.X) + Math.PI / 2;
Vector2 goal1 = mean + 0.25 * (new Vector2(Math.Cos(theta), Math.Sin(theta)));
Vector2 goal2 = mean - 0.25 * (new Vector2(Math.Cos(theta), Math.Sin(theta)));
Vector2 goalPointGlobal = ((goal1.X - lookAhead.pt.X) * (goal1.X - lookAhead.pt.X) + (goal1.Y - lookAhead.pt.Y) * (goal1.Y - lookAhead.pt.Y)
< (goal2.X - lookAhead.pt.X) * (goal2.X - lookAhead.pt.X) + (goal2.Y - lookAhead.pt.Y) * (goal2.Y - lookAhead.pt.Y)) ? goal1 : goal2;
double xg = (goalPointGlobal.X - pose.x) * Math.Cos(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Sin(pose.yaw);
double yg = -(goalPointGlobal.X - pose.x) * Math.Sin(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Cos(pose.yaw);
goalPoint = new Vector2(xg, yg);
}
public void UpdatePath(IPath path, LineSegment gap)
{
if (path == null || path.Count == 0 || pose == null)
return;
double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 mean = new Vector2((gap.P0.X + gap.P1.X) / 2, (gap.P0.Y + gap.P1.Y) / 2);
double theta = Math.Atan2(gap.P0.Y - gap.P1.Y, gap.P0.X - gap.P1.X) + Math.PI / 2;
Vector2 goal1 = mean + 0.25 * (new Vector2(Math.Cos(theta), Math.Sin(theta)));
Vector2 goal2 = mean - 0.25 * (new Vector2(Math.Cos(theta), Math.Sin(theta)));
Vector2 goalPointGlobal = ((goal1.X - lookAhead.pt.X) * (goal1.X - lookAhead.pt.X) + (goal1.Y - lookAhead.pt.Y) * (goal1.Y - lookAhead.pt.Y)
< (goal2.X - lookAhead.pt.X) * (goal2.X - lookAhead.pt.X) + (goal2.Y - lookAhead.pt.Y) * (goal2.Y - lookAhead.pt.Y)) ? goal1 : goal2;
double xg = (goalPointGlobal.X - pose.x) * Math.Cos(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Sin(pose.yaw);
double yg = -(goalPointGlobal.X - pose.x) * Math.Sin(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Cos(pose.yaw);
goalPoint = new Vector2(xg, yg);
}
public void UpdatePath(IPath path)
{
if (path == null || path.Count == 0 || pose == null)
return;
double dist = 0.45;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 goalPointGlobal = lookAhead.pt;
double xg = (goalPointGlobal.X - pose.x) * Math.Cos(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Sin(pose.yaw);
double yg = -(goalPointGlobal.X - pose.x) * Math.Sin(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Cos(pose.yaw);
goalPoint = new Vector2(xg, yg);
}
private void GenrefVWHuniformdt(IPath path, double deltadist, double dt)
{
PointOnPath currentPointOnPath = path[0].ClosestPoint(currentPoint.ToVector2());
int numpoints = (int)((path.Length) / deltadist);
int n = 5; //number of additional points
xr = new double[numpoints+n];
yr = new double[numpoints+n];
double starttimestamp = (DateTime.Now.Ticks - 621355968000000000) / 10000000;
timestamps = new double[numpoints];
for (int i = 0; i < timestamps.Length; i++)
{
timestamps[i] = starttimestamp + i * dt;
}
for (int i = 0; i <= numpoints+3; i++)
{
double d = deltadist*(i);
PointOnPath lookaheadpointi = path.AdvancePoint(currentPointOnPath, ref d);
xr[i] = lookaheadpointi.pt.X;
yr[i] = lookaheadpointi.pt.Y;
}
double[] xrdot = new double[xr.Length - 1];
double[] yrdot = new double[xrdot.Length];
double[] xr2dot = new double[xrdot.Length - 1];
double[] yr2dot = new double[xr2dot.Length];
double[] vr = new double[xr.Length];
double[] wr = new double[xr.Length];
double[] hr = new double[xr.Length];
for (int i = 0; i < xr.Length - 1; i++)
{
xrdot[i] = (xr[i + 1] - xr[i]) / dt;
yrdot[i] = (yr[i + 1] - yr[i]) / dt;
}
for (int i = 0; i < xrdot.Length - 1; i++)
{
xr2dot[i] = (xrdot[i + 1] - xrdot[i]) / dt;
yr2dot[i] = (yrdot[i + 1] - yrdot[i]) / dt;
}
for (int i = 0; i < xrdot.Length; i++)
{
vr[i] = Math.Sqrt(Math.Pow(xrdot[i], 2) + Math.Pow(yrdot[i], 2));
hr[i] = Math.Atan2(yrdot[i], xrdot[i]); // in radians
// unwrap headings
if (i > 0)
{
while (hr[i] - hr[i - 1] >= Math.PI)
{
hr[i] -= 2 * Math.PI; ;
}
while (hr[i] - hr[i - 1] <= -Math.PI)
{
hr[i] += 2 * Math.PI;
}
}
if (i < xr2dot.Length)
{
wr[i] = ((xrdot[i] * yr2dot[i] - yrdot[i] * xr2dot[i]) / (Math.Pow(vr[i], 2))); // in radians/sec
}
}
//pad the reference vectors
for (int i = 1; i < n; i++)
{
wr[numpoints + i] = 0;
vr[numpoints + i] = 0;
hr[numpoints + i] = 0;
}
TextWriter tw = new StreamWriter("parametrization.txt");
for (int i = 0; i < timestamps.Length; i++)
{
tw.WriteLine("index timestamp x y vr wr hr");
tw.WriteLine("{0} {1} {2} {3} {4} {5} {6}",i,timestamps[i],xr[i],yr[i],vr[i],wr[i],hr[i]);
}
tw.Close();
}
public void UpdatePath(IPath path, LineSegment gap)
{
if (path == null || path.Count == 0) return;
/*double dist = 0.45;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 goalpointGlobal = lookAhead.pt;
double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
goalpoint = new Vector2(xg, yg);*/
double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 mean = new Vector2((gap.P0.X + gap.P1.X)/2, (gap.P0.Y + gap.P1.Y)/2);
double theta = Math.Atan2(gap.P0.Y - gap.P1.Y, gap.P0.X - gap.P1.X);
Vector2 goal1 = mean + 0.25 * (new Vector2(Math.Cos(theta + Math.PI / 2), Math.Sin(theta + Math.PI / 2)));
Vector2 goal2 = mean - 0.25 * (new Vector2(Math.Cos(theta + Math.PI / 2), Math.Sin(theta + Math.PI / 2)));
Vector2 goalpointGlobal = (goal1.DistanceTo(lookAhead.pt) < goal2.DistanceTo(lookAhead.pt)) ? goal1 : goal2;
double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
goalpoint = new Vector2(xg, yg);
/*double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
double goal1Y = (gap.P0.Y + gap.P1.Y) / 2 + (gap.P0.X - gap.P1.X) / (gap.Length) * 0.25;
double goal1X = (gap.P0.X + gap.P1.X) / 2 + (gap.P0.Y - gap.P1.Y) / (gap.Length) * 0.25;
Vector2 goal1 = new Vector2(goal1X, goal1Y);
double goal2Y = (gap.P0.Y + gap.P1.Y) / 2 + (gap.P0.X - gap.P1.X) / (gap.Length) * -0.25;
double goal2X = (gap.P0.X + gap.P1.X) / 2 + (gap.P0.Y - gap.P1.Y) / (gap.Length) * -0.25;
Vector2 goal2 = new Vector2(goal2X, goal2Y);
goalpoint = (goal1.DistanceTo(lookAhead.pt) < goal2.DistanceTo(lookAhead.pt)) ? goal1 : goal2;*/
}
private void PlanPurePursuit()
{
if (pathCurrentlyTracked == null)
{
return;
}
//we are really far off the path, just get on the stupid path
//mark sucks and wants this behavior
if (pathCurrentlyTracked.Length == 0)
{
goalPoint = pathCurrentlyTracked.EndPoint.pt;
}
else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2)
{
//Console.WriteLine("2");
double lookaheadRef = 0;
PointOnPath pTemp = segmentCurrentlyTracked.StartPoint;
goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt;
}
else
{
//Console.WriteLine("1");
//see if we can track the next segment and if so, update that...
PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2());
double lookaheadRef = lookAheadDistParam;
PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef);
//the path lookahead point is at the end, and we cant do antyhing
segmentCurrentlyTracked = lookaheadPointOnPath.segment;
goalPoint = lookaheadPointOnPath.pt;
}
double errorX = (goalPoint.X - currentPoint.x);
double errorY = (goalPoint.Y - currentPoint.y);
Vector2 verr = new Vector2(errorX, errorY);
//Console.WriteLine(errorX + " | " + errorY);
double errorDistance = verr.Length;
double currentYaw = currentPoint.yaw;
double errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw);
//Console.Write("neg Hyst: " + hysterisisNegative + " pos Hyst: " + hysterisisPositive + " yawError" + errorYaw * 180.0/ Math.PI+ " ||");
//the reason for this is that our angles are defined from 0 to 360
//but the PID controller expects angle to be -180 to 180
//calculate the control outputs
//the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual
//position
double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI;
double commandedVelocity = (kPPosition * errorDistance);
double commandedHeading = kPHeading * errorYaw;
if (Math.Abs(commandedVelocity) > capVelocity)
{
commandedVelocity = Math.Sign(commandedVelocity) * capVelocity;
}
if (Math.Abs(commandedHeading) > capHeadingDot)
{
commandedHeading = Math.Sign(commandedHeading) * capHeadingDot;
}
//if (unsignedYawErrorNormalizeToOne > .1)
// find which input to use
RobotTwoWheelCommand currentInput = inputList[pathCurrentlyTracked.IndexOf(segmentCurrentlyTracked)];
//commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor));
//command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading);
if (pathCurrentlyTracked.EndPoint.pt.DistanceTo(currentPoint.ToVector2()) < .2)
{
command = new RobotTwoWheelCommand(0, 0);
}
else
{
if (currentInput.velocity < 0.3)
{
command = new RobotTwoWheelCommand(0.4, commandedHeading);
}
else
{
command = new RobotTwoWheelCommand(currentInput.velocity, commandedHeading);
}
}
//if (reachedGoal)
//{
// command.velocity = 0;
// command.turn = 0;
//}
//Console.WriteLine("Current: " + currentPoint.x + " " + currentPoint.y + " " + currentPoint.yaw + " | " + errorDistance + " | " + errorYaw + " | " + commandedVelocity + " " + commandedHeading);
//Console.WriteLine();
}
private void GenrefVWHuniformdt(IPath path, double deltadist, double dt)
{
PointOnPath currentPointOnPath = path[0].ClosestPoint(currentPoint.ToVector2());
int numpoints = (int)((path.Length) / deltadist);
int n = 5; //number of additional points
xr = new double[numpoints + n];
yr = new double[numpoints + n];
double starttimestamp = (DateTime.Now.Ticks - 621355968000000000) / 10000000;
timestamps = new double[numpoints];
for (int i = 0; i < timestamps.Length; i++)
{
timestamps[i] = starttimestamp + i * dt;
}
for (int i = 0; i <= numpoints + 3; i++)
{
double d = deltadist * (i);
PointOnPath lookaheadpointi = path.AdvancePoint(currentPointOnPath, ref d);
xr[i] = lookaheadpointi.pt.X;
yr[i] = lookaheadpointi.pt.Y;
}
double[] xrdot = new double[xr.Length - 1];
double[] yrdot = new double[xrdot.Length];
double[] xr2dot = new double[xrdot.Length - 1];
double[] yr2dot = new double[xr2dot.Length];
double[] vr = new double[xr.Length];
double[] wr = new double[xr.Length];
double[] hr = new double[xr.Length];
for (int i = 0; i < xr.Length - 1; i++)
{
xrdot[i] = (xr[i + 1] - xr[i]) / dt;
yrdot[i] = (yr[i + 1] - yr[i]) / dt;
}
for (int i = 0; i < xrdot.Length - 1; i++)
{
xr2dot[i] = (xrdot[i + 1] - xrdot[i]) / dt;
yr2dot[i] = (yrdot[i + 1] - yrdot[i]) / dt;
}
for (int i = 0; i < xrdot.Length; i++)
{
vr[i] = Math.Sqrt(Math.Pow(xrdot[i], 2) + Math.Pow(yrdot[i], 2));
hr[i] = Math.Atan2(yrdot[i], xrdot[i]); // in radians
// unwrap headings
if (i > 0)
{
while (hr[i] - hr[i - 1] >= Math.PI)
{
hr[i] -= 2 * Math.PI;;
}
while (hr[i] - hr[i - 1] <= -Math.PI)
{
hr[i] += 2 * Math.PI;
}
}
if (i < xr2dot.Length)
{
wr[i] = ((xrdot[i] * yr2dot[i] - yrdot[i] * xr2dot[i]) / (Math.Pow(vr[i], 2))); // in radians/sec
}
}
//pad the reference vectors
for (int i = 1; i < n; i++)
{
wr[numpoints + i] = 0;
vr[numpoints + i] = 0;
hr[numpoints + i] = 0;
}
TextWriter tw = new StreamWriter("parametrization.txt");
for (int i = 0; i < timestamps.Length; i++)
{
tw.WriteLine("index timestamp x y vr wr hr");
tw.WriteLine("{0} {1} {2} {3} {4} {5} {6}", i, timestamps[i], xr[i], yr[i], vr[i], wr[i], hr[i]);
}
tw.Close();
}
public RobotTwoWheelCommand GetCommand(double transMax, double turnMax)
{
lock (followerLock)
{
if (pathCurrentlyTracked == null)
{
Console.WriteLine("Null path tracked!");
return(new RobotTwoWheelCommand(0, 0));
}
//we are really far off the path, just get on the stupid path
//mark sucks and wants this behavior
if (pathCurrentlyTracked.Length == 0) //single waypoint case
{
goalPoint = pathCurrentlyTracked.EndPoint.pt;
startPoint = pathCurrentlyTracked.EndPoint.pt;
}
//else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2)
//{
// //Console.WriteLine("2");
// double lookaheadRef = 0;
// PointOnPath pTemp = segmentCurrentlyTracked.StartPoint;
// goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt;
//}
else
{
//Console.WriteLine("1");
//see if we can track the next segment and if so, update that...
PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2());
double lookaheadRef = lookAheadDistParam;
PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef);
if (segmentCurrentlyTracked != lookaheadPointOnPath.segment)
{
segmentCurrentlyTracked = lookaheadPointOnPath.segment;
}
goalPoint = lookaheadPointOnPath.pt;
startPoint = currentPointOnPath.pt;
}
// Calculate errors
double errorX = (goalPoint.X - currentPoint.x);
double errorY = (goalPoint.Y - currentPoint.y);
Vector2 verr = new Vector2(errorX, errorY);
double tangentX = (goalPoint.X - startPoint.X);
double tangentY = (goalPoint.Y - startPoint.Y);
Vector2 tangent = new Vector2(tangentX, tangentY);
double errorDistance = currentPoint.ToVector2().DistanceTo(startPoint);
double currentYaw = currentPoint.yaw;
double tempCurrentYaw = currentPoint.yaw;
errorYawTangent = UnwrapAndCalculateYawError(tangentX, tangentY, ref tempCurrentYaw);
errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw);
double tangentEquation = (goalPoint.Y - startPoint.Y) * currentPoint.x / (goalPoint.X - startPoint.Y)
- (goalPoint.Y - startPoint.Y) * goalPoint.X / (goalPoint.X - startPoint.Y) + startPoint.Y;
//Console.Clear();
//Console.WriteLine("Current yaw is: " + currentYaw);
if (tangentEquation < currentPoint.y)
{
if (Math.PI / 2 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI)
{
//Console.WriteLine("Above line, pointing left");
errorDistance *= -1;
}
//else Console.WriteLine("Above line, pointing right");
}
else
{
if (0 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI / 2)
{
//Console.WriteLine("Below line, pointing right");
errorDistance *= -1;
}
//else Console.Write("Below line, pointing left");
}
//if we are really close to last waypoint, make the robot just stop
if ((segmentCurrentlyTracked == pathCurrentlyTracked[pathCurrentlyTracked.Count - 1]) &&
(PathCurrentlyTracked.EndPoint.pt - currentPoint.ToVector2()).Length < lookAheadDistParam)
{
command = new RobotTwoWheelCommand(0, 0);
//Console.WriteLine("Acheived!");
}
else
{
//the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual
//position
double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI;
double velocityPercentage = Math.Abs((Math.PI - Math.Abs(errorYawTangent)) / Math.PI);
double commandedVelocity = (velocityPercentage < 0.5) ? 0.0 : transMax * velocityPercentage;
//double commandedHeading = 200 * errorYawTangent + 100 * errorYaw + 300 * errorDistance;
double commandedHeading = 150 * errorYawTangent + 100 * errorYaw + 200 * errorDistance;
if (Math.Abs(commandedVelocity) > transMax)
{
commandedVelocity = Math.Sign(commandedVelocity) * transMax;
}
if (Math.Abs(commandedHeading) > turnMax)
{
commandedHeading = Math.Sign(commandedHeading) * turnMax;
commandedVelocity = 0.0;
}
/*if (unsignedYawErrorNormalizeToOne > .1)
* commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor));*/
command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading);
//Console.WriteLine(errorYaw + ", " + errorYawTangent + ", " + errorDistance);
}
return(command);
}
}