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