UAV Altitude Control Simulation
Er, JavaScript doesn't seem to be running, so basically all you're going to see is the bare code...
/* Manual keyboard control: Press i (more thrust) and k (less thrust). Or, uncomment and tune this one-line autopilot! */ // uav.lift=-10.0*uav.error-2.0*uav.rate-0.5*uav.total; /* Variables: uav.lift: fan motor force, red Newtons (positive means up) uav.error: altitude error, blue meters (positive means too high) uav.rate: altitude rate, green meters/second (positive means climbing) uav.total: total recent altitude errors meters (positive means usually too high) */ /* Simulate Physics */ uav.simulate_physics(lib.dt);
// Simulation Parameters: /* motorLag: simulates spin-up time. 0.0 for ideal case (easy mode) 0.3 for reality (hard mode!) */ var motorLag=0.2; // Make a "UAV" (looks more like a brick for now) function uavSim() { this.mesh=new THREE.Mesh( new THREE.BoxGeometry(0.30, 0.10, 0.08), new THREE.MeshLambertMaterial({color: 0x331133}) ); this.mesh.castShadow=true; scene.add(this.mesh); this.V=new vec3(0,0,0); // at rest (m/s) this.P=new vec3(0,0,2); // in air (m) this.last_z=this.P.z; this.last_z=this.P.z; this.error=this.P.z-1.0; // proportional term this.total=0.0; // integral term this.rate=0.0; // derivative term this.lift=3.0; // target thrust (Newtons) this.outputLift=this.lift; // actual thrust this.time=0.0; this.thrustArrow=new THREE.ArrowHelper( new vec3(0,0,1).normalize(), this.P, 1.0, 0xff0000 ); scene.add(this.thrustArrow); } sim.uav=new uavSim(); window.uav=sim.uav; // add a global, for easier access uavSim.prototype.simulate_physics=function(dt) { // sanity-check user's lift value this.lift=Math.max(this.lift,0.0); // fan can't push down var maxLift=10.0; // maximum thrust (Newtons) this.lift=Math.min(this.lift,maxLift); // fan limit this.time+=dt; if (motorLag<=0.0) this.outputLift=this.lift; else if (dt>0.0) { // simulate motor lag var cur=dt/motorLag; // fraction of target to blend in if (cur>1.0) cur=1.0; this.outputLift=this.lift*cur+this.outputLift*(1.0-cur); } // Newtonian physics! var m=0.400; // mass of UAV (Kg) var A=new vec3(0,0,-9.8); // Acceleration A.z+=this.outputLift/m; // F=mA, so A=F/m this.V.add(A.t(dt)); // Velocity this.P.add(this.V.t(dt)); // Position if (this.P.z<0.0) { // hit ground this.P.z=0.0; // push up to ground this.V.te(0.6); // lose energy this.V.z=Math.abs(this.V.z); // move upwards } // Update altitude error and rate calculation if (dt>0.0) // timer roundoff! this.rate=(this.P.z-this.last_z)/dt; // derivative this.last_z=this.P.z; this.error=this.P.z-1.0; // proportional this.total+=this.error; // integral term var bigtot=10.0; // clamp for integral term this.total=Math.max(this.total,-bigtot); this.total=Math.min(this.total,+bigtot); trace("uav.lift="+this.lift); trace("uav.error="+this.error); trace("uav.rate="+this.rate); trace("uav.total="+this.total); trace("uav.P.z="+this.P.z); trace("uav.V.z="+this.V.z); trace("uav.A.z="+A.z); trace("uav.dt="+dt); trace("uav.time="+this.time); // Update 3D geometry display this.mesh.position.copy(this.P.p(new vec3(0,0,0.04))); this.thrustArrow.position.copy(this.mesh.position); this.thrustArrow.setLength(this.lift/(10.0*m)); sim.chartZ.add(this.time, this.error+1.0); sim.chartR.add(this.time, this.rate*0.1+1.0); sim.chartL.add(this.time, this.lift*2.0/maxLift); sim.chartOL.add(this.time, this.outputLift*2.0/maxLift); } // Ground var groundTex=THREE.ImageUtils.loadTexture("pixanvil_2015_01/checkerboard_noisy.jpg" ); groundTex.wrapS=groundTex.wrapT=THREE.RepeatWrapping; groundTex.repeat=new vec2(25.0,25.0); sim.ground = new THREE.Mesh( new THREE.BoxGeometry(50,50,0.0001), new THREE.MeshLambertMaterial( {color: 0xffccaa, opacity: 1}) // , map:groundTex}) ); sim.ground.position.z=0; /* ground is at Z=0 */ sim.ground.receiveShadow=true; scene.add(sim.ground); // Sun-like spotlight (shadows help you see depth) var l=new THREE.SpotLight(); sim.light = l; l.position.set( -50, -100, 100 ); l.castShadow=true; l.shadowCameraNear = 50; l.shadowCameraFar = 500; l.shadowCameraFov = 10; // degrees field of view scene.add(l); camera.lookAt(scene.position); /* Charting: Blue is altitude Green is altitude rate Red is lift (dark read is actual after motor lag) */ function chart(hasGrid,lineColor) { this.range=10.0; this.dx=0.01; this.last_vtx=-1; this.last_x=0.0; this.geom=new THREE.Geometry(); /* for some reason THREE.js doesn't let you change vertex counts, so preallocate lots of vertices. */ for (var x=0;x<=this.range;x+=this.dx) { this.geom.vertices.push(new vec3(x,0,0.5)); } this.line=new THREE.Line(this.geom, new THREE.LineBasicMaterial({ color: lineColor }) ); scene.add(this.line); if (hasGrid) { var minor=1.0; this.grid = new THREE.Mesh( new THREE.PlaneGeometry( this.range, 2.0, minor*this.range, minor*2.0 ), new THREE.MeshBasicMaterial( { color: 0x7777ff, wireframe: true } ) ); this.grid.rotation.x=3.141592/2; // XY to XZ this.grid.position.z=+1.0; scene.add(this.grid); } } /* Add a new (x,y) location to this chart. */ chart.prototype.add=function(new_x,new_y) { if (new_x
=this.last_x+this.dx) { var vtx={x:new_x,y:0.0,z:new_y}; for (i=1+this.last_vtx;i
// User Interface (UI) code: // Manual UAV thrust control: var dl=2.0*lib.dt; // newtons of thrust per second of key if (lib.key['i']) uav.lift+=dl; // more thrust if (lib.key['k']) uav.lift-=dl; // less thrust if (lib.key['j']) uav.P.z=2.0; // jump up! // Camera control: // Update camera coordinate system var s=camera; if (!s.X) { // startup: create initial coordinates s.X=new vec3(1,0,0); s.Y=new vec3(0,0,1); s.Z=new vec3(0,-1,-0.0); // camera Z is world Y s.P=new vec3(0,-4,1.0); // initial location } // Move camera via keyboard var move=new vec3(0,0,0); // sums current frame motion // X control via A and D if (lib.key['a']) move.pe(new vec3(-1,0,0)); if (lib.key['d']) move.pe(new vec3(+1,0,0)); // Y control via W and S if (lib.key['w']) move.pe(new vec3(0,0,-1)); if (lib.key['s']) move.pe(new vec3(0,0,+1)); // Z control via Q and Z if (lib.key['q']) move.pe(new vec3(0,+1,0)); if (lib.key['z']) move.pe(new vec3(0,-1,0)); move.te(2.0*lib.dt); // meters/second motion rate s.P.pe(s.X.t(move.x).p(s.Y.t(move.y)).p(s.Z.t(move.z))); // Rotate camera via mouse var speed=0.01; // radians per mouse pixel if (lib.mouseleft) { // move Z with mouse s.Z.pe(s.X.t(-lib.mousedx*speed).p( s.Y.t( lib.mousedy*speed))); } // Keep level: make sure X is horizontal. s.X.z=0.0; s.Y.crossVectors(s.Z,s.X.normalize()); // Orthonormalize s.X.crossVectors(s.Y,s.Z).normalize(); s.Y.crossVectors(s.Z,s.X).normalize(); s.Z.normalize(); // Write coordinate system into matrix s.matrixAutoUpdate=false; // don't trash s.matrixWorldNeedsUpdate=true; // show var m=s.matrix; // the camera's matrix // Utility function: set a matrix column function setCol(m,col,vec) { m.elements[4*col+0]=vec.x; m.elements[4*col+1]=vec.y; m.elements[4*col+2]=vec.z; } setCol(m,0,s.X); setCol(m,1,s.Y); setCol(m,2,s.Z); setCol(m,3,s.P); // position from sim