PIDControllerR2

with orbitControls, XZgrid, info

by jmcjc5u

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 theta = 0,
  pointer;
var pidControl;
var R2marker;

init();
animate();

function myAtan2(y, x) {
  let atan2 = Math.atan2(y, x);
  return atan2 >= 0 ? atan2 : atan2 + Math.PI * 2; // [0, 2pi)
}

function setTarget(rawAngle) {
  let rawP = rawAngle - Math.PI * 2;
  if (Math.abs(rawP - theta) < Math.abs(rawAngle - theta))
    return rawP;
  else if (Math.abs(rawAngle + Math.PI * 2 - theta) < Math.abs(rawAngle - theta))
    return rawAngle + Math.PI * 2;
  else
    return 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'
 ...