PID control

by Leoooo

HTML

<div id="info">PID Control
  <br> Left click to choose reference
</div>
<script src="https://cdnjs.cloudflare.com/ajax/libs/three.js/84/three.min.js"></script>
<script src="https://jyunming-chen.github.io/tutsplus/js/KeyboardState.js"></script>

CSS

#info {
  position: absolute;
  top: 0px;
  width: 100%;
  padding: 10px;
  text-align: center;
  color: #ffff00
}

body {
  overflow: hidden;
}

JavaScript

var camera, scene, renderer, point;
var clock = new THREE.Clock();
var offset = 4; // graphics: [-4,4]; real: [0,8]

/////////////////////
var yy, vv;
var yref;
var omega;
var integral = 0;


(function() {
  Math.clamp = function(val, min, max) {
    return Math.min(Math.max(val, min), max);
  }
})();

init();
animate();

function init() {
  renderer = new THREE.WebGLRenderer({
    antialias: true
  });

  renderer.setSize(window.innerWidth, window.innerHeight);
  renderer.setClearColor(0x888888);
  document.body.appendChild(renderer.domElement);

  scene = new THREE.Scene();
  camera = new THREE.OrthographicCamera(-10, 10, 10, -10, -10, 100);
  camera.position.z = 10;

	//////////////////////////////////////////////////
  var geometry = new THREE.Geometry();
  geometry.vertices.push(
    new THREE.Vector3(-4, 0, 0),
    new THREE.Vector3(4, 0, 0));

  var border = new THREE.Line(geometry, new THREE.LineBasicMaterial({
    color: 0x0000ff
  }));
  var border3 = border.clone();
  border3.rotation.z = Math.PI/2;
  scene.add (border3)

  point = new THREE.Mesh(new THREE.CircleGeometry(0.4, 10),
    new THREE.MeshBasicMaterial({
      color: 0xff0000,
    }));
  scene.add(point);

  window.addEventListener('mousedown', onDocumentMouseDown, false);
	yy = 0;
  vv = 0;
  yref = 2;
  omega = 10;
}

function onDocumentMouseDown(event) {
  event.preventDefault();
	yref = 10 - 20*event.clientY/window.innerHeight;
  yref = Math.clamp (yref, -4, 4) + offset;
}


function controlLaw (dt) {
  let error = yref - yy;
  integral += error*dt;
	const KP = 4;
	const KI = 18;
  const KD = 4;
  let omega = KP*error + KI*integral + KD*(-vv);
	//omega = Math.clamp (omega, 0, 100);
	//console.log (error.toFixed(1) + ' + ' + omega.toFixed(1))
  
  return omega;
}

function animate() {
	let dt = clock.getDelta();
  let omega = controlLaw(0.01);
  //console.log(dt);
  // process
	let gravity = -70;
  let KK = 0.1;
	let f = KK*omega*omega + gravity;
  console.log(f)
	// process: Euler
  vv +=...