formatting issues

Napalm1432 · 2016-12-26T19:55:08.000Z

i’m writing a script in unity to simulate a future arduino project for a 4 axis robot arm, to make sure the kinematics work out i wanted to simulate it in unity. however, i’m having some formatting problems.

I hope someone can help sort em out!

Thanks in advance!

#pragma strict
//Arm lengths variables
public var base_Height = 20;          public var arm2_Length = 18;
public var arm1_Length = 18;          public var tool_Length = 6;

//precalculation arms 1 & 2 squared
public var arm1L_sq = arm1_Length*arm1_Length;
public var arm2L_sq = arm2_Length*arm2_Length;

function Start () {
	
}

function Update () {
	
}

function IKCalc () {
    /* variable declerations*/
    var x;       var y; 
    var z;       var grip_angle_d;

    /* grip angle in radians for use in calculations */
    var grip_angle_r = grip_angle_d * Mathf.Deg2Rad;  

    /* Base angle and radial distance from x,y coordinates */
    var base_angle_r = Mathf.Atan2(x, y);
    var rdist = Mathf.Sqrt((x * x) + (y * y));

    /* rdist is y coordinate for the arm */
    y = rdist;

    /* Grip offsets calculated based on grip angle */
    var grip_off_z = Mathf.Asin(grip_angle_r) * tool_Length;
    var grip_off_y = Mathf.Acos(grip_angle_r) * tool_Length;

    /* Wrist position */
    var wrist_z = (z - grip_off_z) - base_Height;
    var wrist_y = y - grip_off_y;

    /* Shoulder to wrist distance ( AKA sw ) */
    var s_w = (wrist_z * wrist_z) * (wrist_y * wrist_y);
    var s_w_sqrt = Mathf.Sqrt(s_w);

    /* s_w angle to ground */
    var a1 = Mathf.Atan2(wrist_z, wrist_y);

    /* s_w angle to arm1 */
    var a2 = Mathf.Acos(((arm1L_sq - arm2L_sq) + s_w) / (2 * arm1_Length * s_w_sqrt));

    /* shoulder angle */
    var sh1_angle_r = a1 + a2;
    var sh1_angle_d = sh1_angle_r * Mathf.Rad2Deg;

    /* elbow angle */
    var elb_angle_r = Mathf.Acos((arm1L_sq + arm2L_sq - s_w) / (2 * arm1_Length * arm2_Length));
    var elb_angle_d = elb_angle_r * Mathf.Rad2Deg;
    var elb_angle_dn = -(180.0 - elb_angle_d);

    /* wrist angle */
    var wri_angle_d = (grip_angle_d - elb_angle_dn) - sh1_angle_d;

    /* set angles for arms */
    var SetAngle1 = (base_angle_r * Mathf.Rad2Deg) * 11.11;
    var SetAngle2 = (sh1_angle_d - 90.0) * 6.6;
    var SetAngle3 = (elb_angle_d - 90.0) * 6.6;;
    var SetAngle4 = wri_angle_d * 11.11;
}

TBruce · 2016-12-26T19:55:31.000Z

This will take care of your errors (please forgive me for reformatting your code).

#pragma strict
//Arm lengths variables
public var base_Height = 20;
public var arm2_Length = 18;
public var arm1_Length = 18;
public var tool_Length = 6;

//precalculation arms 1 & 2 squared
public var arm1L_sq = arm1_Length*arm1_Length;
public var arm2L_sq = arm2_Length*arm2_Length;

function Start ()
{
}

function Update ()
{
}

function IKCalc ()
{
    /* variable declerations*/
    var x : float;
    var y : float;
    var z : float;
    var grip_angle_d : float;

    /* grip angle in radians for use in calculations */
    var grip_angle_r = grip_angle_d * Mathf.Deg2Rad;  

    /* Base angle and radial distance from x,y coordinates */
    var base_angle_r = Mathf.Atan2(x, y);
    var rdist = Mathf.Sqrt((x * x) + (y * y));

    /* rdist is y coordinate for the arm */
    y = rdist;

    /* Grip offsets calculated based on grip angle */
    var grip_off_z = Mathf.Asin(grip_angle_r) * tool_Length;
    var grip_off_y = Mathf.Acos(grip_angle_r) * tool_Length;

    /* Wrist position */
    var wrist_z = (z - grip_off_z) - base_Height;
    var wrist_y = y - grip_off_y;

    /* Shoulder to wrist distance ( AKA sw ) */
    var s_w = (wrist_z * wrist_z) * (wrist_y * wrist_y);
    var s_w_sqrt = Mathf.Sqrt(s_w);

    /* s_w angle to ground */
    var a1 = Mathf.Atan2(wrist_z, wrist_y);

    /* s_w angle to arm1 */
    var a2 = Mathf.Acos(((arm1L_sq - arm2L_sq) + s_w) / (2 * arm1_Length * s_w_sqrt));

    /* shoulder angle */
    var sh1_angle_r = a1 + a2;
    var sh1_angle_d = sh1_angle_r * Mathf.Rad2Deg;

    /* elbow angle */
    var elb_angle_r = Mathf.Acos((arm1L_sq + arm2L_sq - s_w) / (2 * arm1_Length * arm2_Length));
    var elb_angle_d = elb_angle_r * Mathf.Rad2Deg;
    var elb_angle_dn = -(180.0 - elb_angle_d);

    /* wrist angle */
    var wri_angle_d = (grip_angle_d - elb_angle_dn) - sh1_angle_d;

    /* set angles for arms */
    var SetAngle1 = (base_angle_r * Mathf.Rad2Deg) * 11.11;
    var SetAngle2 = (sh1_angle_d - 90.0) * 6.6;
    var SetAngle3 = (elb_angle_d - 90.0) * 6.6;;
    var SetAngle4 = wri_angle_d * 11.11;
}

Note: You are using some variables that have not yet been assigned values e.g. x, y, z, grip_angle_d.

Napalm1432 · 2016-12-27T15:22:45.000Z

Yes my problem has been resolved, i’m currently working on making it visible with a model, algorithm part seems to be doing good! I will post the complete IKCalc function+variables here, for future reference

/* x y z coordinates to move end effector to */
public var varXPos = 10;
public var varYPos = 10;
public var varZPos = 10;


/* Arm lengths variables */
public var base_Height = 20;          public var arm2_Length = 18;
public var arm1_Length = 18;          public var tool_Length = 6;

/* precalculation arms 1 & 2 squared */
public var arm1L_sq = arm1_Length*arm1_Length;
public var arm2L_sq = arm2_Length*arm2_Length;

/* raw angle values before model conversion values */
public var GAngle1 = 0.0;
public var GAngle2 = 0.0;
public var GAngle3 = 0.0;
public var GAngle4 = 0.0;

/* attach script on all joints, in the editor select true or false for the joint */
public var applyAngle1 = true;
public var applyAngle2 = false;
public var applyAngle3 = false;
public var applyAngle4 = false;
function Start () {
    
}

function Update () {
    IKCalc(varXPos,varYPos,varZPos,0);
}


function IKCalc (x:float,y:float,z:float,grip_angle_d:float)
{
    /* grip angle in radians for use in calculations */
    var grip_angle_r = grip_angle_d * Mathf.Deg2Rad;  
 
    /* Base angle and radial distance from x,y coordinates */
    var base_angle_r = Mathf.Atan2(x, y);
    var rdist = Mathf.Sqrt((x * x) + (y * y));
 
    /* rdist is y coordinate for the arm */
    y = rdist;
 
    /* Grip offsets calculated based on grip angle */
    var grip_off_z = Mathf.Asin(grip_angle_r) * tool_Length;
    var grip_off_y = Mathf.Acos(grip_angle_r) * tool_Length;
 
    /* Wrist position */
    var wrist_z = (z - grip_off_z) - base_Height;
    var wrist_y = y - grip_off_y;
 
    /* Shoulder to wrist distance ( AKA sw ) */
    var s_w = (wrist_z * wrist_z) * (wrist_y * wrist_y);
    var s_w_sqrt = Mathf.Sqrt(s_w);
 
    /* s_w angle to ground */
    var a1 = Mathf.Atan2(wrist_z, wrist_y);
 
    /* s_w angle to arm1 */
    var a2 = Mathf.Acos(((arm1L_sq - arm2L_sq) + s_w) / (2 * arm1_Length * s_w_sqrt));
 
    /* shoulder angle */
    var sh1_angle_r = a1 + a2;
    var sh1_angle_d = sh1_angle_r * Mathf.Rad2Deg;
 
    /* elbow angle */
    var elb_angle_r = Mathf.Acos((arm1L_sq + arm2L_sq - s_w) / (2 * arm1_Length * arm2_Length));
    var elb_angle_d = elb_angle_r * Mathf.Rad2Deg;
    var elb_angle_dn = -(180.0 - elb_angle_d);
 
    /* wrist angle */
    var wri_angle_d = (grip_angle_d - elb_angle_dn) - sh1_angle_d;
 
    /* set angles for arms */
    var SetAngle1 = (base_angle_r * Mathf.Rad2Deg);
    var SetAngle2 = (sh1_angle_d - 90.0);
    var SetAngle3 = (elb_angle_d - 90.0);
    var SetAngle4 = wri_angle_d;

    /* execute write angles to model function */
    ApplyAngles(SetAngle1,SetAngle2,SetAngle3,SetAngle4);
}