Permalink
Browse files

autopilot works with map

  • Loading branch information...
1 parent 285e782 commit fef875881c647117232c9e8b7fc99434c79e7b88 @HalfdanJ HalfdanJ committed Jul 10, 2011
Showing with 338 additions and 73 deletions.
  1. +189 −36 client/client.pde
  2. +68 −0 client/map.pde
  3. +16 −3 client/osc.pde
  4. +23 −19 server/gps.pde
  5. +20 −11 server/navigation.pde
  6. +15 −2 server/osc.pde
  7. +7 −2 server/server.pde
View
@@ -17,6 +17,19 @@ float serverCompas;
float serverBearing;
float serverGoalDistance;
+boolean settingGoal = false;
+
+float latSaved = -1;
+float lonSaved = -1;
+float latDiff = -1;
+float lonDiff = -1;
+
+float mapCenterLat;
+float mapCenterLon;
+int mapZoom = 16;
+
+PImage mapImg;
+
void setup() {
frameRate(20);
@@ -46,17 +59,75 @@ boolean onKeyDown(int keyCode, KeyEvent event) {
fwdbkwOn = true;
}
if (keyCode == KeyEvent.KEYCODE_DPAD_LEFT) {
- left = 0;
+ if (mode == 0) {
+ left = 0;
+ }
+ else {
+ settingGoal = true;
+ if (mapImg != null) {
+ float[] goalP = LLToXY(serverGoalLat, serverGoalLon, mapCenterLat, mapCenterLon, mapZoom);
+ goalP[0] += 5;
+ float[] goalP2 = XYToLL((int)goalP[0], (int)goalP[1], mapCenterLat, mapCenterLon, mapZoom);
+
+ serverGoalLat = goalP2[0];
+ serverGoalLon = goalP2[1];
+ }
+ }
}
if (keyCode == KeyEvent.KEYCODE_DPAD_RIGHT) {
- right = 0;
+ if (mode == 0) {
+ right = 0;
+ }
+ else {
+ settingGoal = true;
+ if (mapImg != null) {
+ float[] goalP = LLToXY(serverGoalLat, serverGoalLon, mapCenterLat, mapCenterLon, mapZoom);
+ goalP[0] -= 5;
+ float[] goalP2 = XYToLL((int)goalP[0], (int)goalP[1], mapCenterLat, mapCenterLon, mapZoom);
+
+ serverGoalLat = goalP2[0];
+ serverGoalLon = goalP2[1];
+ }
+ }
+ }
+ if (keyCode == KeyEvent.KEYCODE_DPAD_UP) {
+ if (mode == 0) {
+ }
+ else {
+ settingGoal = true;
+ if (mapImg != null) {
+ float[] goalP = LLToXY(serverGoalLat, serverGoalLon, mapCenterLat, mapCenterLon, mapZoom);
+ goalP[1] += 5;
+ float[] goalP2 = XYToLL((int)goalP[0], (int)goalP[1], mapCenterLat, mapCenterLon, mapZoom);
+
+ serverGoalLat = goalP2[0];
+ serverGoalLon = goalP2[1];
+ }
+ }
+ }
+ if (keyCode == KeyEvent.KEYCODE_DPAD_DOWN) {
+ if (mode == 0) {
+ }
+ else {
+ settingGoal = true;
+ if (mapImg != null) {
+ float[] goalP = LLToXY(serverGoalLat, serverGoalLon, mapCenterLat, mapCenterLon, mapZoom);
+ goalP[1] -= 5;
+ float[] goalP2 = XYToLL((int)goalP[0], (int)goalP[1], mapCenterLat, mapCenterLon, mapZoom);
+
+ serverGoalLat = goalP2[0];
+ serverGoalLon = goalP2[1];
+ }
+ }
}
if (keyCode == 102) { //LEFT
- if (!fwdbkwOn) {
- leftRev = true;
- rightRev = false;
- left = 255;
- right = 255;
+ if (mode == 0) {
+ if (!fwdbkwOn) {
+ leftRev = true;
+ rightRev = false;
+ left = 255;
+ right = 255;
+ }
}
}
if (keyCode == 103) { //RIGHT
@@ -72,17 +143,24 @@ boolean onKeyDown(int keyCode, KeyEvent event) {
String strSend = "goalSet:";
strSend += currentLatitude+";";
strSend += currentLongitude;
- strSend = strSend.replace(".","*");
+ strSend = strSend.replace(".", "*");
talk.SendToAllFriends(strSend);
+ /* latSaved = currentLatitude;
+ lonSaved = currentLongitude; */
}
- if (keyCode == 82) {
- String strSend = "goalSet:";
- strSend += currentLatitude+";";
- strSend += currentLongitude;
- strSend = strSend.replace(".","*");
- talk.SendToAllFriends(strSend);
+ /* if (keyCode == 82) {
+ String strSend = "goalSet:";
+ strSend += serverLat+";";
+ strSend += serverLon;
+ strSend = strSend.replace(".", "*");
+ talk.SendToAllFriends(strSend);
+ }*/
+ if (keyCode == 82) {
+ mapImg = loadImage("http://maps.google.com/maps/api/staticmap?center="+serverLat+","+serverLon+"&zoom=+"+mapZoom+"&size=350x480&sensor=false", "jpg");
+ mapCenterLat = serverLat;
+ mapCenterLon = serverLon;
}
-
+
if (keyCode == 109) {
if (mode == 0)
mode = 1;
@@ -111,13 +189,45 @@ boolean onKeyUp(int keyCode, KeyEvent event) {
fwdbkwOn = false;
}
if (keyCode == KeyEvent.KEYCODE_DPAD_LEFT) {
- if (fwdbkwOn) {
- left = 255;
+ if (mode == 0) {
+ if (fwdbkwOn) {
+ left = 255;
+ }
+ }
+ else {
+ settingGoal = false;
+ String strSend = "goalSet:";
+ strSend += serverGoalLat+";";
+ strSend += serverGoalLon;
+ strSend = strSend.replace(".", "*");
+ talk.SendToAllFriends(strSend);
}
}
if (keyCode == KeyEvent.KEYCODE_DPAD_RIGHT) {
- if (fwdbkwOn) {
- right = 255;
+ if (mode == 0) {
+ if (fwdbkwOn) {
+ right = 255;
+ }
+ }
+ else {
+ settingGoal = false;
+ String strSend = "goalSet:";
+ strSend += serverGoalLat+";";
+ strSend += serverGoalLon;
+ strSend = strSend.replace(".", "*");
+ talk.SendToAllFriends(strSend);
+ }
+ }
+ if (keyCode == KeyEvent.KEYCODE_DPAD_UP || keyCode == KeyEvent.KEYCODE_DPAD_DOWN) {
+ if (mode == 0) {
+ }
+ else {
+ settingGoal = false;
+ String strSend = "goalSet:";
+ strSend += serverGoalLat+";";
+ strSend += serverGoalLon;
+ strSend = strSend.replace(".", "*");
+ talk.SendToAllFriends(strSend);
}
}
if (keyCode == 102) {
@@ -128,22 +238,63 @@ boolean onKeyUp(int keyCode, KeyEvent event) {
left = 0;
right = 0;
}
+
return true;
}
void draw() {
+ if (latSaved != -1) {
+ latDiff = latSaved - currentLatitude;
+ lonDiff = lonSaved - currentLongitude;
+ }
+
updateOsc();
background(0);
fill(255);
+ if (mapImg != null) {
+ pushMatrix();
+ translate(screenWidth-350, 0);
+ image(mapImg, 0, 0, 350, 480);
+ float[] centP = LLToXY(serverLat, serverLon, mapCenterLat, mapCenterLon, mapZoom);
+ float[] weP = LLToXY(currentLatitude, currentLongitude, mapCenterLat, mapCenterLon, mapZoom);
+ float[] goalP = LLToXY(serverGoalLat, serverGoalLon, mapCenterLat, mapCenterLon, mapZoom);
+ fill(255, 0, 0);
+ translate(0.5*350, 0.5*480);
+
+ //ellipse(centP[0], centP[1], 30, 30);
+ fill(0, 255, 0);
+ ellipse(-weP[0], -weP[1], 30, 30);
+ fill(0, 0, 255);
+ ellipse(-goalP[0], -goalP[1], 30, 30);
+
+ fill(255, 0, 0);
+ pushMatrix();
+ translate(-centP[0], -centP[1]);
+ scale(0.3);
+ rotate(-serverCompas/360.0*TWO_PI);
+ beginShape();
+ vertex(0, -50);
+ vertex(-20, 60);
+ vertex(0, 50);
+ vertex(20, 60);
+ endShape(CLOSE);
+ popMatrix();
+
+ popMatrix();
+ }
// Display current GPS data
- text("Latitude: "+currentLatitude, 20, 40);
- text("Longitude: "+currentLongitude, 20, 75);
+ fill(255);
+ text("Latitude: "+currentLatitude + " ("+latDiff+")", 20, 40);
+ text("Longitude: "+currentLongitude+ " ("+lonDiff+")", 20, 75);
text("Accuracy: "+currentAccuracy, 20, 110);
text("Mode: "+mode, 20, 145);
- text("Distance: "+serverGoalDistance,20,180);
+ text("Distance: "+serverGoalDistance, 20, 180);
fill(70, 100, 100);
+
+ pushMatrix();
+ translate(-200, 100);
stroke(255, 255, 255);
rect(screenWidth/2.0-100, screenHeight/2.0-100, 200, 200);
@@ -158,9 +309,9 @@ void draw() {
if (rightRev)
fill(255, 0, 0);
arc(screenWidth/2.0+70, screenHeight/2.0+100, 50, 50, 0-HALF_PI, TWO_PI*right/255.0-HALF_PI);
-
+
fill(255);
- pushMatrix();
+ pushMatrix();
translate(width/2, height/2);
scale(1);
rotate(serverCompas/360.0*TWO_PI);
@@ -172,19 +323,21 @@ void draw() {
endShape(CLOSE);
popMatrix();
- if (serverBearing != 0){
+ if (serverBearing != 0) {
fill(255, 0, 0);
- pushMatrix();
- translate(width/2, height/2);
- scale(1);
- rotate(serverBearing/360.0*TWO_PI);
- beginShape();
- vertex(0, -50);
- vertex(-20, 60);
- vertex(0, 50);
- vertex(20, 60);
- endShape(CLOSE);
- popMatrix();
+ pushMatrix();
+ translate(width/2, height/2);
+ scale(1);
+ rotate(serverBearing/360.0*TWO_PI);
+ beginShape();
+ vertex(0, -50);
+ vertex(-20, 60);
+ vertex(0, 50);
+ vertex(20, 60);
+ endShape(CLOSE);
+ popMatrix();
}
+
+ popMatrix();
}
View
@@ -0,0 +1,68 @@
+int offset=268435456;
+float radius=offset/PI;
+
+int LToX(float x) {
+ return round(offset+radius*x*PI/180);
+}
+
+int LToY(float y)
+{
+ return round(offset-radius*log((1+sin(y*PI/180))/(1-sin(y*PI/180)))/2);
+}
+
+float XToL(int x)
+{
+ return ((round(x)-offset)/radius)*180/PI;
+}
+
+float YToL(int y)
+{
+ return (PI/2-2*atan(exp((round(y)-offset)/radius)))*180/PI;
+}
+
+
+float[] Adjust(float X, float Y, float x, float y, int z, boolean w) {
+ float[] ret;
+ ret = new float[2];
+ if (w) {
+ ret[0] = ((LToX(X)-LToX(x))>>(21-z));
+ ret[1] = ((LToY(Y)-LToY(y))>>(21-z));
+ }
+
+ else
+ {
+ ret[0] = XToL(LToX(x)+(((int)X)<<(21-z)));
+ ret[1] = YToL(LToY(y)+(((int)Y)<<(21-z)));
+ }
+
+ return ret;
+}
+
+
+
+float[] XYToLL(int X, int Y, float x, float y, int z) {
+ return Adjust(X, Y, x, y, z, false);
+}
+
+// X = X pixel offset of new map center from old map center
+// Y = Y pixel offset of new map center from old map center
+// x = Longitude of map center
+// y = Latitude of map center
+// z = Zoom level
+
+// result.x = Longitude of adjusted map center
+// result.y = Latitude of adjusted map center
+
+float[] LLToXY(float X, float Y, float x, float y, int z) {
+ return Adjust(X, Y, x, y, z, true);
+}
+
+// X = Longitude of marker center
+// Y = Latitude of marker center
+// x = Longitude of map center
+// y = Latitude of map center
+// z = Zoom level
+
+// result.x = X pixel offset of marker center from map center
+// result.y = Y pixel offset of marker center from map center
+
Oops, something went wrong.

0 comments on commit fef8758

Please sign in to comment.