PIDControllerR2

with orbitControls, XZgrid, info

by Leoooo

HTML

<div id="info">PID Control (R2)
  <p id='theta'>
  </p>
</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.vx = 0;
    this.vy = 0;
    this.KP = 150; // 'spring constant'
    this.KD = 20; // 'damping'
    this.KI = 20;
	  this.integralX = 0;
    this.integralY = 0;
}

  update(dt) {
    let errorX = this.xref - this.x;
    let errorY = this.yref - this.y;
		this.integralX += errorX*dt;
		this.integralY += errorY*dt;
		let fx = this.KP * errorX + this.KI*this.integralX - this.KD * this.vx;
    let fy = this.KP * errorY + this.KI*this.integralY - this.KD * this.vy;
    // plant: Euler's method (Newtonian dynamics)
    this.vx += fx * dt;
    this.x += this.vx * dt
    this.vy += fy * dt;
    this.y += this.vy * dt
    return [this.x, this.y]
  }
  setRef(xref, yref) {
    this.xref = xref;
    this.yref = yref;
  }
}


var camera, scene, renderer;
var mousePoint;
var pointer;
var pidControl;
var R2marker;

init();
animate();

function myAtan2(y, x) {
	// no need to modify ...
  return Math.atan2 (y, x)
}

function setTarget(rawAngle) {
	// convert angle to (x,y) on unit circle
  return [Math.cos(rawAngle), Math.sin(rawAngle)]
}

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('mousemove', onDocumentMouseDown, false);

  //////////////
  pidControl = new PIDControllerR2();
  
 ...