Polar Controller (R2)

with R2 controller core to avoid singularity

by jmcjc5u

HTML

<div id="info">Polar Control with R2 controller core
  <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.lastErrorX = 0;
    this.lastErrorY = 0;
    this.KP = 25; // 'spring constant'
    this.KD = 20; // 'damping'
    this.KI = 20;
	  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(x,y) {
  	this.x = x;
    this.y = y;
  }
  setRef(xref, yref) {
    this.xref = xref;
    this.yref = yref;
  }
}

var camera, scene, renderer;
var mousePoint;
var theta = 0, omega = 0;
var  pointer;
var thetaRef = 0;

var r2Control;

init();
animate();

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


function setTarget(mouseX, mouseY) { 
//	return myAtan2(mouseY, mouseX);
  
//	if (mouseX > 0)
  	return Math.atan2(mouseY, mouseX);
  //else
	//	return myAtan2(mouseY, mouseX);
}

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'
  }));
 ...