Commits

Benoît Allard committed bc57946

Lots of pilot 'improvements'

Comments (0)

Files changed (3)

-<!DOCTYPE HTML PUBLIC "-//IETF//DTD HTML//EN">
+<!DOCTYPE html>
 <html> <head>
 <title>Harvester simulation</title>
 <link rel="stylesheet" type="text/css" href="style.css">
 </head>
 
 <body>
-<div id=page>
- <h1>Harvester simulation <span class=small>(harvesim)</span></h1>
+<div id="page">
+ <h1>Harvester simulation <span class="small">(harvesim)</span></h1>
 
- <p class=opera>WARNING: There are some drawing troubles with
+ <p class="opera">WARNING: There are some drawing troubles with
  Opera. This should'nt affect the simulation though.</p>
 
- <div id=stack>
-  <canvas width=760 height=760 id=harvester></canvas>
-  <canvas width=760 height=760 id=field>
+ <div id="stack">
+  <canvas width="760" height="760" id="harvester"></canvas>
+  <canvas width="760" height="760" id="field">
    <pre>If you can read this, your browser does not support the
-        <canvas> HTML5 element, please try with a more recent one
+        <canvas> HTML5 element, please try with a more recent one.
    </pre>
   </canvas>
-  <img id=map alt="Map">
+  <img id="map" alt="Map">
  </div>
 
  <div>
-  <button onclick="sim.startInterval()" id=startBtn>Start</button>
-  <button onclick="sim.stopInterval()" id=pauseBtn>Pause</button>
-  <button onclick="sim.reset()" id=resetBtn>Reset</button>
-  <button onclick="sim.step()" id=stepBtn>Perform one Step</button>
+  <button onclick="sim.startInterval()" id="startBtn">Start</button>
+  <button onclick="sim.stopInterval()" id="pauseBtn">Pause</button>
+  <button onclick="sim.reset()" id="resetBtn">Reset</button>
+  <button onclick="sim.step()" id="stepBtn">Perform one Step</button>
   <label for="speed">Speed:</label>
   <input name="speed"
          type="number"
  </div>
  <div>
   <p>Parameters below only take effect on 'Reset'.</p>
-  <label for="fields">Field:</field>
-  <select name=fields id=fields>
+  <label for="fields">Field:</label>
+  <select name="fields" id="fields">
   </select>
  </div>
 
+import requests
+
+import elementtree.ElementTree as ET
+
+import math
+
+import json
+
+def getWay(id):
+    r = requests.get('http://www.openstreetmap.org/api/0.6/way/%s' % id)
+    elem = ET.XML(r.text)
+    elem = elem.find('way')
+    nodes = elem.findall('nd')
+    nds = []
+    for nd in nodes:
+        nds.append(getNode(nd.attrib['ref']))
+    return nds
+
+def getNode(ref):
+    r = requests.get('http://www.openstreetmap.org/api/0.6/node/%s' % ref)
+    elem = ET.XML(r.text)
+    node = elem.find('node')
+    attribs = node.attrib
+    return {'lat': float(attribs['lat']),
+            'lon': float(attribs['lon'])}
+
+
+"""
+lon0 <- mean(range(Long))
+lat0 <- mean(range(Lat))
+
+rx <- 6371 * acos(sin(lat0 *pi/180)^2 + cos(lat0*pi/180)^2 * cos((lon0+.5)*pi/180 - (lon0-.5)*pi/180))
+ry <- 6371 * acos(sin((lat0 -.5)*pi/180) *  sin((lat0+.5)*pi/180) + cos((lat0-.5)*pi/180) * cos((lat0+.5)*pi/180))
+
+x.km <-(Long-lon0)*rx
+y.km <-(Lat-lat0)*ry
+"""
+
+def XYkm(nodes):
+    lon0 = 0
+    lat0 = 0
+    for n in nodes:
+        lon0 += n['lon']
+        lat0 += n['lat']
+    lon0 /= len(nodes)
+    lat0 /= len(nodes)
+
+    rx = 6371 * math.acos(math.sin(math.radians(lat0))**2 + math.cos(math.radians(lat0))**2 * math.cos(math.radians(lon0+.5) - math.radians(lon0-.5)))
+    ry = 6371 * math.acos(math.sin(math.radians(lat0 - .5)) * math.sin(math.radians(lat0+.5)) + math.cos(math.radians(lat0-.5)) * math.cos(math.radians(lat0+.5)))
+
+    nds = []
+    for n in nodes:
+        nds.append({'x':(n['lon'] - lon0)*rx*1000,
+                    'y':(n['lat']-lat0)*ry*1000})
+    return nds
+
+id = 117739400
+
+print json.dumps({'name': 'osm-%s' % id, 'points':XYkm(getWay(id))})
     {name: 'Example 4', points:[{"x":77.2677519521676,"y":1.8924515997239168e-14},{"x":22.145847935089837,"y":70.99739373003555},{"x":-39.00922121656229,"y":63.41442774542689},{"x":22.305967055182684,"y":-72.31080229409193}]},
     {name: 'Example 5', points: [{"x":61.977320404257625,"y":1.517956407198783e-14},{"x":17.50011034157625,"y":76.35434721445728},{"x":-71.57081269026901,"y":12.163722542150296},{"x":-24.846350851342986,"y":-70.52182892372107},{"x":-9.424086448965982,"y":-76.65545258735527},{"x":10.247250217904533,"y":-67.31722592444896}]},
     {name: 'Example 6', points:[{"x":59.50104014482349,"y":1.4573070364078238e-14},{"x":45.19669198569412,"y":53.23884377975345},{"x":-25.51142085507122,"y":48.17018454116998},{"x":-56.43721712870133,"y":-22.80505795733697},{"x":-17.035821485363975,"y":-58.03067279083557},{"x":66.57579329376456,"y":-27.016922832446372}]},
-    {name: 'osm-117739400 (Not working)',
-     points:toMeters([{lon:5.8427743, lat:50.2649766}, {lon:5.8431924, lat:50.2651622},
-                      {lon:5.8439405, lat:50.2653564}, {lon:5.8444906, lat:50.2654070},
-                      {lon:5.8447591, lat:50.2645574}, {lon:5.8449841, lat:50.2640527},
-                      {lon:5.8433728, lat:50.2637359}])},
+    {"points": [{"y": 22.07219293773181, "x": -71.91230855081342}, {"y": 42.709971323158264, "x": -42.1938497939149}, {"y": 64.30402607784173, "x": 10.980938524745657}, {"y": 69.93048936622999, "x": 50.08192910633636}, {"y": -24.540720311225247, "x": 69.16685179521252}, {"y": -80.66079978878055, "x": 85.15980376910674}, {"y": -115.88735254979859, "x": -29.37105630017522}, {"y": 22.07219293773181, "x": -71.91230855081342}], "name": "osm-117739400"},
 ];
 
 var initfieldslen = fields.length;
 
 init();
 
-/* This function should convert lat/lon to meters */
-function toMeters(pts){
-    var R = 6378.137; // Radius of earth in KM
-    var ps = new Array();
-    for (var i=0; i < pts.length; i++){
-        ps[i] = {x:0, y:0};
-        ps[i].y = pts[i].lat * R;
-    }
-    return ps;
-};
-
 function ellipse(context, cx, cy, rx, ry, angle){
     context.save(); // save state
     context.beginPath();
     sim = new Simulation(harvester, field, pilot);
 }
 
+/* restrict all angles between -PI and PI */
+function restrictAngle(angle){
+    var limit = Math.PI;
+    while (angle <= -limit){
+        angle += 2*limit;
+    }
+    while (angle > limit){
+        angle -= 2*limit; 
+    }
+    return angle;
+};
+
+/* Same, but between -2PI and 2PI */
+function limit2PI(angle){
+    var limit = 2*Math.PI;
+    while (angle <= -limit){
+        angle += 2*limit;
+    }
+    while (angle > limit){
+        angle -= 2*limit; 
+    }
+    return angle;
+};
+
 function Simulation(harvester, field, pilot){
     this.runner = null;
 
         this.a = Math.atan2(pt1.y - pt0.y, pt1.x - pt0.x);
         this.x = pt0.x - (this.length/2)*Math.cos(this.a) - (this.width/2)*Math.sin(this.a);
         this.y = pt0.y - (this.length/2)*Math.sin(this.a) + (this.width/2)*Math.cos(this.a);
+        frc = this.getPt(this.length/2, this.width/2)
+        if ( ! field.isInside(frc.x, frc.y)){
+            this.a += Math.PI;
+            this.x = pt0.x - (this.length/2)*Math.cos(this.a) - (this.width/2)*Math.sin(this.a);
+            this.y = pt0.y - (this.length/2)*Math.sin(this.a) + (this.width/2)*Math.cos(this.a);
+        }
+        this.a = restrictAngle(this.a);
     };
 
     /* Apply the transformation of the field to the harvester */
         this.y += f0.y + f1.y;
         // (5/8)*this.length is the distance between the front and rear wheels
         this.a += (this.v*-Math.sin(this.wa))/((5/8)*this.length)
+        this.a = restrictAngle(this.a);
     };
 
     this.stop = function(){
         }
     };
 
+    this.initialised = false;
+    this.rounds = 0;
+    this.counttherounds = function (harvester){
+        if (! this.initialised){
+            this.x0 = harvester.x;
+            this.y0 = harvester.y;
+            this.prevdist = 0;
+            this.prevdelta = 0;
+            this.initialised = true;
+            this.counter = 0;
+        } else {
+            harvester.drawProbe(this.x0, this.y0, true);
+            this.counter += 1;
+            if ((this.counter % 5) == 0){
+                this.counter = 0;
+                var dist = Math.sqrt(Math.pow(harvester.x - this.x0, 2) + Math.pow(harvester.y - this.y0, 2));
+                var delta = dist - this.prevdist;
+                if ((this.prevdelta < 0) && (delta > 0) && (dist<5*harvester.width)){
+                    this.x0 = harvester.x;
+                    this.y0 = harvester.y;
+                    this.prevdist = 0;
+                    this.prevdelta = 0;
+                    this.counter = 1;
+                    this.rounds += 1;
+                    console.log('One more round : ', this.rounds);
+                } else {
+                    this.prevdelta = delta;
+                    this.prevdist = dist;
+                }
+            }
+        }
+    }
+
     /* an auto-pilot that spirals in the field */
     this.spiral = function(harvester, field){
         // We take the direction as granted: CW
             return res;
         };
 
+        this.counttherounds(harvester);
+
+        if (this.rounds == 3){
+            this.drive = this.followleft;
+        }
+
         var pb1 = probe(harvester.length, -Math.PI/40);
         var pb2 = probe(harvester.length, Math.PI/40);
 
                 if (pb6){
                     this.mode = this.spiral;
                     console.log('changing mode: reverse');
-                    this.drive = this.reverse(harvester.a+Math.PI/4, this.forwardtofield);
+                    harvester.reverse(100);
+                    this.drive = this.turnleftto(harvester.a+Math.PI/4, this.forwardtofield);
                 } else {
                     // full right
                     harvester.right(100);
         }
     };
 
-    this.reverse = function(angle, next){
+    this.turnleftto = function(angle, next){
         // The beauty of closures
         return function(harvester, field){
-            if (harvester.a > angle){
-                console.log('reverse done, calling next');
+            // greatest possible difference in angle in one step
+            var epsilon = Math.abs((harvester.v*-Math.sin(harvester.maxwheelangle))/((5/8)*harvester.length));
+            var cond = (angle - harvester.a + 2*Math.PI) % (2*Math.PI) < 3*epsilon;
+            if (cond){
+                console.log('turn done, calling next');
                 this.drive = next;
             } else {
                 if (!field.isInside(harvester.x, harvester.y)){
                     console.log('getting out, setting mode back');
-                    harvester.stop();
                     this.drive = this.mode;
                 } else {
-                    harvester.reverse(100);
                     harvester.left(100);
                 }
             }
         };
     };
 
+    this.turnrightto = function(angle, next){
+        // The beauty of closures
+        return function(harvester, field){
+            var epsilon = Math.abs((harvester.v*-Math.sin(harvester.maxwheelangle))/((5/8)*harvester.length));
+            var cond = (angle - harvester.a + 2*Math.PI) % (2*Math.PI) < 3*epsilon;
+            if (cond){
+                console.log('turn done, calling next');
+                this.drive = next;
+            } else {
+                if (!field.isInside(harvester.x, harvester.y)){
+                    console.log('getting out, setting mode back');
+                    this.drive = this.mode;
+                } else {
+                    harvester.right(100);
+                }
+            }
+        };
+    };
+
     this.forwardtofield = function(harvester, field){
         harvester.forward(100);
         harvester.straight();
 
         if (!field.isInside(pt.x, pt.y)){
             console.log('About to exit the field, reversing');
-            harvester.stop();
-            this.drive = this.reverse(harvester.a+Math.PI/2, this.mode);
+            harvester.reverse(100);
+            this.drive = this.turnleftto(harvester.a+Math.PI/2, this.mode);
         }
 
         if (! res){
         }
     };
 
+    this.followleft = function(harvester, field){
+        harvester.forward(100);
+
+        // the beauty of closures ...
+        var probeRA = function(r, a){
+            var pt = harvester.getPt(harvester.length/2 + r*Math.cos(a),
+                                     -harvester.width/2 + r*Math.sin(a));
+            var res = field.isHarvested(pt.x, pt.y);
+            harvester.drawProbe(pt.x, pt.y, res);
+            return res;
+        };
+        var probeXY = function(x, y){
+            var pt = harvester.getPt(x, y);
+            var res = field.isHarvested(pt.x, pt.y);
+            harvester.drawProbe(pt.x, pt.y, res);
+            return res;
+        };
+
+        var pb1 = probeRA(harvester.length/2, -Math.PI/40);
+        var pb2 = probeRA(harvester.length/2, Math.PI/40);
+        if (probeXY(harvester.length*1.25, 0)){
+            harvester.straight();
+            if (probeXY(harvester.length*.6, 0)){
+                // turning around
+                var a0 = harvester.a;
+                var r = ((5/8)*harvester.length) / Math.sin(harvester.maxwheelangle);
+                var alpha = Math.acos((2*r+harvester.width)/(4*r));
+                //console.log('turning', a0, a0-alpha, a0+Math.PI + alpha, a0+Math.PI);
+                harvester.forward(100);
+                this.mode = this.followright;
+                this.drive = this.turnleftto(
+                    a0 - alpha,
+                    this.turnrightto(
+                        a0 + Math.PI + alpha,
+                        this.turnleftto(
+                            a0+Math.PI,
+                            this.followright)));
+            }
+        }
+        else if (pb1 && !pb2){
+            // easy case, straight ahead !
+            harvester.forward(100);
+            harvester.straight();
+        } else if (pb1 && pb2 && !probeRA(harvester.length/2, 3*Math.PI/40)){
+            // bit right
+            harvester.right(50);
+            harvester.forward(100);
+        } else if (!pb1 && !pb2 && probeRA(harvester.length/2, -3*Math.PI/40)){
+            // bit left
+            harvester.left(50);
+            harvester.forward(100);
+        } else {
+            // find the edge ...
+            console.log('find the edge');
+        }
+
+    }
+
+    this.followright = function(harvester, field){
+        harvester.forward(100);
+        // the beauty of closures ...
+        var probeRA = function(r, a){
+            var pt = harvester.getPt(harvester.length/2 + r*Math.cos(a),
+                                     harvester.width/2 + r*Math.sin(a));
+            var res = field.isHarvested(pt.x, pt.y);
+            harvester.drawProbe(pt.x, pt.y, res);
+            return res;
+        };
+        var probeXY = function(x, y){
+            var pt = harvester.getPt(x, y);
+            var res = field.isHarvested(pt.x, pt.y);
+            harvester.drawProbe(pt.x, pt.y, res);
+            return res;
+        };
+
+        var pb1 = probeRA(harvester.length/2, Math.PI/40);
+        var pb2 = probeRA(harvester.length/2, -Math.PI/40);
+        if (probeXY(harvester.length*1.25, 0)){
+            harvester.straight();
+            if (probeXY(harvester.length*0.6, 0)){
+                // turning around
+                var a0 = harvester.a;
+                var r = ((5/8)*harvester.length) / Math.sin(harvester.maxwheelangle);
+                var alpha = Math.acos((2*r+harvester.width)/(4*r));
+                //console.log('turning', a0, a0+alpha, a0+Math.PI - alpha, a0+Math.PI);
+                harvester.forward(100);
+                this.mode = this.followleft;
+                this.drive = this.turnrightto(
+                    a0 + alpha,
+                    this.turnleftto(
+                        a0 + Math.PI - alpha,
+                        this.turnrightto(
+                            a0+Math.PI,
+                            this.followleft)));
+            }
+        } else if (pb1 && !pb2){
+            // easy case, straight ahead !
+            harvester.forward(100);
+            harvester.straight();
+        } else if (pb1 && pb2 && !probeRA(harvester.length/2, -3*Math.PI/40)){
+            // bit right
+            harvester.left(50);
+            harvester.forward(100);
+        } else if (!pb1 && !pb2 && probeRA(harvester.length/2, 3*Math.PI/40)){
+            // bit left
+            harvester.right(50);
+            harvester.forward(100);
+        } else {
+            // find the edge
+            console.log('find the edge');
+        }
+
+    }
+
     this.drive = this.spiral;
 }