Dexter Servo Gripper Code

Current Code
//xyz

function init_parameters(){

    //See "move_to" Doc for dimension diagram
    Dexter.LINK4 = .0508 // 0.0508 (m)
    Dexter.LINK5 = 0.08255 // 0.08255 (m)

    return [
        make_ins("S", "MaxSpeed", 20),        // (deg/s)
        make_ins("S", "StartSpeed", .1),         // (deg/s)
        make_ins("S", "Acceleration", 0.000129), // (deg/s^2)
        make_ins("S", "J1BoundryHigh", 180),
        make_ins("S", "J1BoundryLow", -180),
        make_ins("S", "J2BoundryHigh", 90),
        make_ins("S", "J2BoundryLow", -90),
        make_ins("S", "J3BoundryHigh", 165),
        make_ins("S", "J3BoundryLow", -165),
        make_ins("S", "J4BoundryHigh", 100),
        make_ins("S", "J4BoundryLow", -100),
        make_ins("S", "J5BoundryHigh", 185),
        make_ins("S", "J5BoundryLow", -185)
    ]
}

var pos1 = [0, 0, 150, 0, 0]            // bend down
var pos2 = [-3.5, 52.75, 109, -10, 0]  // go to print and gripper close
var pos3 = [-8.5, 51.25, 109, -10, 0]     // move off print board
var pos4 = [-30, 0, 150, 0, 0]          // rotate to box
var pos5 = [-30, 40, 150, -30, 0]       // down to box and gripper open

var pos30 = [0, 0, 0, 0, 0]             // slide across
var pos40 = [0, 0, 0, 0, 0]             // back up after slide
var home = [0, 0, 0, 0, 0]              // home

var pause = 5
var o = 180
var c = 98
new Job({name: "job_gripper",
        do_list: [
                  init_parameters(),
                  Dexter.wait_until(pause), // pause
                  // Gripper Demo
                  // Close
                  Dexter.move_all_joints(home),
                  Dexter.set_parameter("EESpan", c),
                  Dexter.move_all_joints(home),

                  // Open
                  Dexter.move_all_joints(home),
                  Dexter.set_parameter("EESpan", o),
                  Dexter.move_all_joints(home),

                  // Close
                  Dexter.move_all_joints(home),
                  Dexter.set_parameter("EESpan", c),
                  Dexter.move_all_joints(home),

                  // Open
                  Dexter.move_all_joints(home),
                  Dexter.set_parameter("EESpan", o),
                  Dexter.move_all_joints(home),

                  // Step 1: home, down to print, gripper close
                  Dexter.move_all_joints(home),  //home
                  Dexter.move_all_joints(pos1),
                  Dexter.move_all_joints(pos2),

                  Dexter.wait_until(pause), // pause
                  Dexter.set_parameter("EESpan", c), // gripper
                  Dexter.wait_until(pause), // pause
                  Dexter.move_all_joints(pos2),

                  Dexter.wait_until(pause), // pause

                  // Step 2: take print off board
                  Dexter.move_all_joints(pos3),

                  // Step 3: up and rotate towards bucket
                  Dexter.move_all_joints(pos1),
                  Dexter.move_all_joints(pos4),

                  // Step 4: to drop off and gripper open
                  Dexter.move_all_joints(pos5),

                  Dexter.wait_until(pause), // pause
                  Dexter.set_parameter("EESpan", o), // gripper open
                  Dexter.move_all_joints(pos5),
                  Dexter.wait_until(pause), // pause

                  // Step 5: home safely
                  Dexter.move_all_joints(pos4),
                  Dexter.move_all_joints(home)
                 ]})