Scara math

HTML

<canvas id=star width=300 height=300></canvas>
    <p>
        <label for="p_scnts">Linkage_1<input type="text" id="Linkage_1" size="4" name="p_scnt" value="160" placeholder="Linkage_1" /></label>
        <label for="p_scnts">Linkage_2<input type="text" id="Linkage_2" size="4" name="p_scnt" value="160" placeholder="Linkage_2"  /></label>
    </p>
    <button id="draw">Update</button>

<pre id="output"></pre>

JavaScript

var canvas, ctx, length = 15; // length of the star's arm

function out()
{
    var args = Array.prototype.slice.call(arguments, 0);
    document.getElementById('output').innerHTML += args.join(" ") + "\n";
}

function line(p1,p2,color)
{
  ctx.strokeStyle = color;
  ctx.beginPath();
  ctx.moveTo(p1.x,p1.y);
  ctx.lineTo(p2.x,p2.y);
  ctx.stroke();
}

// grab a reference to the 2d context from canvas element
canvas = document.getElementById("star");
ctx = canvas.getContext("2d");

// move into the middle of the canvas, just to make room

var width = canvas.width;
var height = canvas.height;

var centerx=width/2;
var zeroy=height-30;

var bedPosLeft = -100;
var bedPosBottom = 0;
var bedWidth = 200;
var bedHeight = 200;

var SCARA_pos_X_AXIS = -5;
var SCARA_pos_Y_AXIS = 110;

var Linkage_1 = Number(document.getElementById("Linkage_1").value);
var Linkage_2 = Number(document.getElementById("Linkage_2").value);

//ctx.beginPath();
//ctx.moveTo(centerx,zeroy);
//ctx.lineTo(centerx+100,zeroy-100);
//ctx.stroke();

ctx.beginPath();
ctx.moveTo(0,0);
ctx.lineTo(width,0);
ctx.lineTo(width,height);
ctx.lineTo(0,height);
ctx.stroke();

// Converts from degrees to radians.
Math.radians = function(degrees) {
  return degrees * Math.PI / 180;
};
 
// Converts from radians to degrees.
Math.degrees = function(radians) {
  return radians * 180 / Math.PI;
};

function draw(x,y,extraAngX,exraAngY,scaleX,scaleY,iDrawArms)
{
  var L1_2 = Linkage_1*Linkage_1;
  var L2_2 = Linkage_2*Linkage_2;
  var DBL_L1_L2 = (2*Linkage_1*Linkage_2);

  var SCARA_C2 =   ( x*x + y*y - L1_2 - L2_2 )/DBL_L1_L2;
  //out("SCARA_C2:", SCARA_C2);
  var SCARA_S2 = Math.sqrt( 1 - (SCARA_C2*SCARA_C2) );
  //out("SCARA_S2:", SCARA_S2);

  var SCARA_K1 = Linkage_1 + Linkage_2 * SCARA_C2;
  var SCARA_K2 = Linkage_2 * SCARA_S2;
  var SCARA_theta = ( Math.atan2(x,y)-Math.atan2(SCARA_K1, SCARA_K2) ) * -1;

  var SCARA_psi   =  ...