/**
* Test method
*
* @param args
*/
public static void main(String[] args) {
Trajectory t =
TrajectoryGenerator.generate(
getCnfg(),
TrajectoryGenerator.AutomaticStrategy,
0,
-1.2589682692342312,
905.2071586106686,
0,
-1.2589682692342312);
double[][] ray = new double[t.segments_.length][2];
for (int i = 0; i < t.segments_.length; i++) {
Trajectory.Segment s = t.segments_[i];
double x = Math.cos(s.heading) * s.pos + 0;
double y = Math.sin(s.heading) * s.pos + 0;
// System.out.println(s.x + "," + s.y + " {" + s.heading + "}" + " (" + (s.heading - 0.5) +
// ")");
// System.out.println(x + "," + y);
ray[i][0] = x;
ray[i][1] = y;
}
FalconLinePlot pl = new FalconLinePlot(ray);
}
/**
* Calculates the robot path from the given play (<code>o</code>), with robot parameters
* being specified by <code>conf</code> and <code>rbt_wheel_base
* @param o the play's list of things to run through
* @param conf the configuration of the robot/play
* @param rbt_wheel_base the width of the robot
* @return the path generated from the given inputs, as saved in a <code>SpeedStorage</code> object
*/
public static SpeedStorage createPath(
ArrayList<GUIObject> o, TrajectoryGenerator.Config conf, double rbt_wheel_base) {
cnfg = conf;
ArrayList<GUIObject> objs = copyGUIRay(o);
ArrayList<PositionCommandGroup> stopGroups = new ArrayList<>();
ArrayList<GUIObject> lines = new ArrayList<>();
for (int i = 4; i < objs.size(); i++) {
GUIObject obj = objs.get(i);
if (obj instanceof PositionCommandGroup && ((PositionCommandGroup) obj).stop_robot) {
stopGroups.add(((PositionCommandGroup) obj));
// System.out.println("Stopper found at " + ((PositionCommandGroup)obj).pos);
} else if (obj instanceof StraightLine || obj instanceof SmoothLine) {
lines.add(obj);
}
}
for (int i = 0; i < lines.size(); i++) {
GUIObject obj = lines.get(i);
if (obj instanceof StraightLine) {
int index = -1;
for (PositionCommandGroup grp : stopGroups) {
if (grp.stop_robot == true
&& PositionUtil.onLine(
grp.pos,
((StraightLine) obj).initialPosition,
((StraightLine) obj).finalPosition)) {
// System.out.println("Yo group on line");
StraightLine newLine = new StraightLine();
newLine.initialPosition = new Point(grp.pos);
newLine.finalPosition = new Point(((StraightLine) obj).finalPosition);
double d1 =
PositionUtil.distance(
((StraightLine) obj).initialPosition, ((StraightLine) obj).finalPosition);
double d2 = PositionUtil.distance(((StraightLine) obj).initialPosition, grp.pos);
((StraightLine) obj).finalPosition = new Point(grp.pos);
lines.add(i + 1, newLine);
// System.out.println(d1 + " " + d2 + " " + d2/d1 + " " + ((StraightLine)obj).time);
newLine.time = ((StraightLine) obj).time * d2 / d1;
// System.out.println(newLine.time);
index = stopGroups.indexOf(grp);
break;
}
}
if (index != -1) stopGroups.remove(index);
} else if (obj instanceof SmoothLine) {
SmoothLine ln = (SmoothLine) obj;
if (lines.size() != i + 1) {
while (lines.get(i + 1) instanceof SmoothLine) {
for (Point p : ((SmoothLine) lines.get(i + 1)).coordinates) {
ln.coordinates.add(new Point(p));
}
ln.maxTime += ((SmoothLine) lines.get(i + 1)).maxTime;
lines.remove(i + 1);
}
}
double totalDistance = 0;
for (int j = 1; j < ln.coordinates.size(); j++) {
totalDistance += PositionUtil.distance(ln.coordinates.get(j), ln.coordinates.get(j - 1));
}
double tempDist = 0;
for (int j = 1; j < ln.coordinates.size(); j++) {
tempDist += PositionUtil.distance(ln.coordinates.get(j), ln.coordinates.get(j - 1));
int index = -1;
for (PositionCommandGroup grp : stopGroups) {
if (grp.stop_robot == true
&& PositionUtil.onLine(
grp.pos,
((SmoothLine) obj).coordinates.get(j - 1),
((SmoothLine) obj).coordinates.get(j))) {
double d1 = totalDistance - PositionUtil.distance(ln.coordinates.get(j), grp.pos);
System.out.println("Yo group on smooth line");
SmoothLine newLine = new SmoothLine();
newLine.coordinates = new ArrayList<>();
newLine.coordinates.add(new Point(grp.pos));
double time = ln.maxTime;
// System.out.println(time);
newLine.maxTime = time * (totalDistance - d1) / totalDistance;
ln.maxTime = time * d1 / totalDistance;
// System.out.println(ln.maxTime);
// System.out.println(newLine.maxTime);
while (ln.coordinates.size() > j) {
newLine.coordinates.add(new Point(ln.coordinates.get(j)));
ln.coordinates.remove(j);
}
ln.coordinates.add(new Point(grp.pos));
lines.add(i + 1, newLine);
index = stopGroups.indexOf(index);
}
}
if (index != -1) {
stopGroups.remove(index);
break;
}
}
}
}
SpeedStorage stg = new SpeedStorage();
int numPoints = 0;
int numThings = 0;
double totaltime = 0;
for (GUIObject g : lines) {
System.out.println("New Line: " + g.getClass());
double[][] path = null;
double time = 0;
if (g instanceof StraightLine) {
StraightLine sl = (StraightLine) g;
double dx = sl.finalPosition.getX() - sl.initialPosition.getX();
double dy = sl.finalPosition.getY() - sl.initialPosition.getY();
Trajectory t =
TrajectoryGenerator.generate(
getCnfg(),
TrajectoryGenerator.SCurvesStrategy,
0,
Math.atan2(dy, dx),
PositionUtil.distance(sl.finalPosition, sl.initialPosition),
0,
Math.atan2(dy, dx));
for (Trajectory.Segment s : t.segments_) {
double x = Math.cos(s.heading) * s.pos + sl.initialPosition.getX();
double y = Math.sin(s.heading) * s.pos + sl.initialPosition.getY();
// System.out.println("SL: " + x + "," + y);
stg.headings.add(s.heading);
stg.robot_points.add(new double[] {x, y});
stg.left_track_speeds.add(s.vel);
stg.right_track_speeds.add(s.vel);
}
} else if (g instanceof SmoothLine) {
SmoothLine ln = (SmoothLine) g;
// path = new double[ln.coordinates.size()][2];
double heading1 = 0;
// if(lines.indexOf(g) == 0)
// {
double dx0 = ln.coordinates.get(1).getX() - ln.coordinates.get(0).getX();
double dy0 = ln.coordinates.get(1).getY() - ln.coordinates.get(0).getY();
heading1 = Math.atan2(dy0, dx0);
/*}
else
{
GUIObject prev = lines.get(lines.indexOf(g)-1);
if(prev instanceof StraightLine)
{
double dx = ((StraightLine)prev).finalPosition.getX() - ((StraightLine)prev).initialPosition.getX();
double dy = ((StraightLine)prev).finalPosition.getY() - ((StraightLine)prev).initialPosition.getY();
heading1 = Math.atan2(dy,dx);
}
else
{
SmoothLine prevLn = (SmoothLine)prev;
int max = prevLn.coordinates.size()-1;
double dx = prevLn.coordinates.get(max).getX() - prevLn.coordinates.get(max-1).getY();
double dy = prevLn.coordinates.get(max).getY() - prevLn.coordinates.get(max-1).getY();
heading1 = Math.atan2(dy,dx);
}
}
*/
double v0 = 0;
double totalDist = 0;
double totalInc = 0;
double totalDec = 0;
double d = ln.coordinates.size();
d = d / 2;
for (int i = 0; i < (int) d; i++) {
totalInc += PositionUtil.distance(ln.coordinates.get(i), ln.coordinates.get(i + 1));
}
for (int i = (int) d + 1; i < ln.coordinates.size() - 1; i++) {
totalDec += PositionUtil.distance(ln.coordinates.get(i), ln.coordinates.get(i + 1));
}
for (int i = 0; i < ln.coordinates.size() - 1; i++) {
double dx = ln.coordinates.get(i + 1).getX() - ln.coordinates.get(i).getX();
double dy = ln.coordinates.get(i + 1).getY() - ln.coordinates.get(i).getY();
double heading2 = Math.atan2(dy, dx);
double dist = PositionUtil.distance(ln.coordinates.get(i), ln.coordinates.get(i + 1));
// System.out.println("H1 " + heading1 + " H2 " + heading2);
// System.out.println((dist*Math.cos(heading1) + ln.coordinates.get(i).getX()) + "," +
// (dist*Math.sin(heading1) + ln.coordinates.get(i).getY()));
double v1 = getCnfg().max_vel / 4;
if (i <= d) {
v1 = v0 + dist / totalInc * (ln.maxTime / 2) * getCnfg().max_acc / 10;
} else {
v1 = v0 - dist / totalDec * (ln.maxTime / 2) * getCnfg().max_acc / 10;
}
if (i == ln.coordinates.size() - 2) {
v1 = 0;
}
double v1t = v1;
double v0t = v0;
if (v1 > getCnfg().max_vel) {
v1t = getCnfg().max_vel;
}
if (v0 > getCnfg().max_vel) {
v0t = getCnfg().max_vel;
}
// System.out.println(i + " V0 " + v0t + " " + v1 + " V1 " + v1t + " pos " +
// PositionUtil.distance(ln.coordinates.get(i), ln.coordinates.get(i+1)) + " dist " +
// dist);
// Trajectory t =
// TrajectoryGenerator.generate(getCnfg(),TrajectoryGenerator.AutomaticStrategy,v0t,heading1,PositionUtil.distance(ln.coordinates.get(i), ln.coordinates.get(i+1)),v1t,heading2);
Trajectory t =
TrajectoryGenerator.generate(
getCnfg(),
TrajectoryGenerator.AutomaticStrategy,
0,
heading1,
PositionUtil.distance(ln.coordinates.get(i), ln.coordinates.get(i + 1)),
0,
heading2);
// System.out.println(ln.coordinates.get(i));
double prevX = 0;
double prevY = 0;
double x = 0;
double y = 0;
for (int index = 0; index < t.segments_.length; index++) {
Trajectory.Segment s = t.segments_[index];
double prevHeading;
if (index == 0) {
prevHeading = t.segments_[0].heading;
} else {
prevHeading = t.segments_[index - 1].heading;
}
prevX = x;
prevY = y;
numPoints++;
x = Math.cos(s.heading) * s.pos + ln.coordinates.get(i).getX();
y = Math.sin(s.heading) * s.pos + ln.coordinates.get(i).getY();
stg.headings.add(s.heading);
stg.robot_points.add(new double[] {x, y});
double[] spds = calcSpeed(s.vel, s.heading, prevHeading, rbt_wheel_base, getCnfg().dt);
stg.left_track_speeds.add(spds[0]);
stg.right_track_speeds.add(spds[1]);
// System.out.println("SmoothLine: " + x + " " + y);
}
// System.out.println(ln.coordinates.get(i+1));
double delx = x - prevX;
delx = delx / getCnfg().dt;
double dely = y - prevY;
dely = dely / getCnfg().dt;
// System.out.println(pythagoras(new double[] {delx,dely}) + " " +
// stg.left_track_speeds.get(stg.left_track_speeds.size()-1));
v0 = pythagoras(new double[] {delx, dely});
double prev4X = stg.robot_points.get(stg.robot_points.size() - 7)[0];
double prev4Y = stg.robot_points.get(stg.robot_points.size() - 7)[1];
dx = x - prev4X;
dy = y - prev4Y;
heading1 = Math.atan2(dy, dx);
}
time = ((SmoothLine) g).maxTime;
}
totaltime += time;
System.out.println(time);
}
return stg;
}
