PIDControllerR2

with orbitControls, XZgrid, info

by jmcjc5u

HTML

<div id="info">PID Control (R2)
<br> generic controller
</div>
<script src="https://threejs.org/build/three.min.js"></script>
<script src="https://code.jquery.com/jquery-2.1.4.min.js"></script>

CSS

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

body {
  overflow: hidden;
}

JavaScript

class PIDControllerR2 {
  constructor(x = 0, y = 0, xref = 0, yref = 0) {
    this.x = x;
    this.y = y;
    this.xref = xref;
    this.yref = yref;
    this.lastErrorX = 0;
    this.lastErrorY = 0;
    this.KP = 40; // 'spring constant'
    this.KD = 30; // 'damping'
    this.KI = 30;
	  this.integralX = 0;
    this.integralY = 0;
  }

  controlLaw(dt) {
    let errorX = this.xref - this.x;
    let errorY = this.yref - this.y;
		this.integralX += errorX*dt;
		this.integralY += errorY*dt;
		let diffX = (errorX - this.lastErrorX)/dt;
    let diffY = (errorY - this.lastErrorY)/dt;
    this.lastErrorX = errorX;
    this.lastErrorY = errorY;
    let fx = this.KP * errorX + this.KI*this.integralX + this.KD*diffX;
    let fy = this.KP * errorY + this.KI*this.integralY + this.KD*diffY;
    return [fx, fy];    
  }

	setCurrent(pos) {
  	this.x = pos.x;
    this.y = pos.y;
  }
  setRef(xref, yref) {
    this.xref = xref;
    this.yref = yref;
  }
}

var camera, scene, renderer;
var mousePoint;
var pidControl;
var R2marker;
var pos, vel, force;

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(-50, 50, 50, -50, -10, 100);
  camera.position.z = 10;

  let grid = new THREE.GridHelper(100, 10, 'red', 'white')
  scene.add(grid)
  grid.rotation.x = Math.PI / 2

  mousePoint = new THREE.Mesh(new THREE.CircleGeometry(1), new THREE.MeshBasicMaterial({
    color: 'yellow'
  }));
  scene.add(mousePoint);

  window.addEventListener('resize', onWindowResize, false);
  window.addEventListener('pointermove', onDocumentMouseDown, false);

  //////////////////////////////////
  pidControl = new PIDControllerR2();
  R2marker = new THREE.Mesh (new THREE.PlaneGeometry(3,3), new THREE.MeshBasicMaterial());
  scene.add...