private bool AddValue(PathSegment s, ICsvLine line, int index, string name) { double v; if (!Double.TryParse(line[index], out v)) { v = Double.NaN; } s.SetValue(name, v); return(true); }
public override Dictionary <string, PathSegment[]> ModifyPath(RobotParams robot, RobotPath path, PathSegment[] segs, double rotvel) { Dictionary <string, PathSegment[]> result = new Dictionary <string, PathSegment[]>(); if (segs == null) { return(null); } double total = segs[segs.Length - 1].GetValue("time"); double rtime = total - path.FacingAngleStartDelay - path.FacingAngleEndDelay; TrapezoidalProfile profile = CreateRotationProfile(robot, path.StartFacingAngle, path.EndFacingAngle, rtime, rotvel); if (profile == null) { throw new DriveModifier.VelocitySplitException(); } PathSegment[] fl = new PathSegment[segs.Length]; PathSegment[] fr = new PathSegment[segs.Length]; PathSegment[] bl = new PathSegment[segs.Length]; PathSegment[] br = new PathSegment[segs.Length]; result["fl"] = fl; result["fr"] = fr; result["bl"] = bl; result["br"] = br; for (int i = 0; i < segs.Length; i++) { // // The time in the path // double time = segs[i].GetValue("time"); // // The current facing angle based on the rotational trapezoidal profile // double current = XeroUtils.BoundDegrees(path.StartFacingAngle + profile.GetDistance(time - path.FacingAngleStartDelay)); // // Get the required velocity to perform the rotation. This is the // linear ground based velocity for the wheel that must be applied to // each wheel in direction perpendicular to the vector from the center of // the robot to the center of the wheel. // double rv = RotationalToGround(robot, profile.GetVelocity(time - path.FacingAngleStartDelay)); double ra = RotationalToGround(robot, profile.GetAcceleration(time - path.FacingAngleStartDelay)); // // Get the velocity vector and acceleration vector relative to each of the // wheels to rotate the robot based on the desired rotational velocity. // XeroVector rotflvel = GetWheelPerpendicularVector(robot, Wheel.FL, rv); XeroVector rotfrvel = GetWheelPerpendicularVector(robot, Wheel.FR, rv); XeroVector rotblvel = GetWheelPerpendicularVector(robot, Wheel.BL, rv); XeroVector rotbrvel = GetWheelPerpendicularVector(robot, Wheel.BR, rv); XeroVector rotflacc = GetWheelPerpendicularVector(robot, Wheel.FL, ra); XeroVector rotfracc = GetWheelPerpendicularVector(robot, Wheel.FR, ra); XeroVector rotblacc = GetWheelPerpendicularVector(robot, Wheel.BL, ra); XeroVector rotbracc = GetWheelPerpendicularVector(robot, Wheel.BR, ra); // // Get the translational velocity vector to follow the path // XeroVector pathvel = XeroVector.FromDegreesMagnitude(segs[i].GetValue("heading"), segs[i].GetValue("velocity")).RotateDegrees(-current); XeroVector pathacc = XeroVector.FromDegreesMagnitude(segs[i].GetValue("heading"), segs[i].GetValue("acceleration")).RotateDegrees(-current); // // For each wheel, the velocity vector is the sum of the rotational vector and the translational vector // XeroVector flv = rotflvel + pathvel; XeroVector frv = rotfrvel + pathvel; XeroVector blv = rotblvel + pathvel; XeroVector brv = rotbrvel + pathvel; XeroVector fla = rotflacc + pathvel; XeroVector fra = rotfracc + pathvel; XeroVector bla = rotblacc + pathvel; XeroVector bra = rotbracc + pathvel; // // Now calculate the wheel positions based on the path position and the // facing angle // XeroVector flpos = new XeroVector(robot.Length / 2.0, robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y")); XeroVector frpos = new XeroVector(robot.Length / 2.0, -robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y")); XeroVector blpos = new XeroVector(-robot.Length / 2.0, robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y")); XeroVector brpos = new XeroVector(-robot.Length / 2.0, -robot.Width / 2.0).RotateDegrees(current).Translate(segs[i].GetValue("x"), segs[i].GetValue("y")); PathSegment newseg = new PathSegment(segs[i]); newseg.SetValue("x", flpos.X); newseg.SetValue("y", flpos.Y); newseg.SetValue("velocity", flv.Magnitude()); newseg.SetValue("heading", flv.AngleDegrees()); newseg.SetValue("acceleration", fla.Magnitude()); fl[i] = newseg; newseg = new PathSegment(segs[i]); newseg.SetValue("x", frpos.X); newseg.SetValue("y", frpos.Y); newseg.SetValue("velocity", frv.Magnitude()); newseg.SetValue("heading", frv.AngleDegrees()); newseg.SetValue("acceleration", fra.Magnitude()); fr[i] = newseg; newseg = new PathSegment(segs[i]); newseg.SetValue("x", blpos.X); newseg.SetValue("y", blpos.Y); newseg.SetValue("velocity", blv.Magnitude()); newseg.SetValue("heading", blv.AngleDegrees()); newseg.SetValue("acceleration", bla.Magnitude()); bl[i] = newseg; newseg = new PathSegment(segs[i]); newseg.SetValue("x", brpos.X); newseg.SetValue("y", brpos.Y); newseg.SetValue("velocity", brv.Magnitude()); newseg.SetValue("heading", brv.AngleDegrees()); newseg.SetValue("acceleration", bra.Magnitude()); br[i] = newseg; } return(result); }
public override Dictionary <string, PathSegment[]> ModifyPath(RobotParams robot, RobotPath path, PathSegment[] segs, double rotvel) { Dictionary <string, PathSegment[]> result = new Dictionary <string, PathSegment[]>(); if (segs == null) { return(null); } double lvel, lacc, ljerk, lpos = 0.0; double rvel, racc, rjerk, rpos = 0.0; double plx = 0.0, ply = 0.0; double prx = 0.0, pry = 0.0; double plvel = 0.0, prvel = 0.0; double placc = 0.0, pracc = 0.0; PathSegment[] lsegs = new PathSegment[segs.Length]; PathSegment[] rsegs = new PathSegment[segs.Length]; result["left"] = lsegs; result["right"] = rsegs; for (int i = 0; i < segs.Length; i++) { double time = segs[i].GetValue("time"); double heading = XeroUtils.DegreesToRadians(segs[i].GetValue("heading")); double ca = Math.Cos(heading); double sa = Math.Sin(heading); double px = segs[i].GetValue("x"); double py = segs[i].GetValue("y"); double lx = px - robot.Width * sa / 2.0; double ly = py + robot.Width * ca / 2.0; double rx = px + robot.Width * sa / 2.0; double ry = py - robot.Width * ca / 2.0; if (i == 0) { lvel = 0.0; lacc = 0.0; lpos = 0.0; ljerk = 0.0; rvel = 0.0; racc = 0.0; rpos = 0.0; rjerk = 0.0; } else { double dt = segs[i].GetValue("time") - segs[i - 1].GetValue("time"); double ldist = Math.Sqrt((lx - plx) * (lx - plx) + (ly - ply) * (ly - ply)); double rdist = Math.Sqrt((rx - prx) * (rx - prx) + (ry - pry) * (ry - pry)); lvel = ldist / dt; rvel = rdist / dt; lacc = (lvel - plvel) / dt; racc = (rvel - prvel) / dt; ljerk = (lacc - placc) / dt; rjerk = (racc - pracc) / dt; lpos += ldist; rpos += rdist; } PathSegment left = new PathSegment(); left.SetValue("time", time); left.SetValue("x", lx); left.SetValue("y", ly); left.SetValue("heading", segs[i].GetValue("heading")); left.SetValue("position", lpos); left.SetValue("velocity", lvel); left.SetValue("acceleration", lacc); left.SetValue("jerk", ljerk); lsegs[i] = left; PathSegment right = new PathSegment(); right.SetValue("time", segs[i].GetValue("time")); right.SetValue("x", rx); right.SetValue("y", ry); right.SetValue("heading", segs[i].GetValue("heading")); right.SetValue("position", rpos); right.SetValue("velocity", rvel); right.SetValue("acceleration", racc); right.SetValue("jerk", rjerk); rsegs[i] = right; plx = lx; ply = ly; prx = rx; pry = ry; plvel = lvel; prvel = rvel; placc = lacc; pracc = racc; } return(result); }