component syringe "syringe control";
// syrctrl1.comp - control syringe for 2gbp
//
// modes:
//      0: idle <-  (i_running && M68 E0 Q0 (m68==0)) || (!i_running && !homing)
//              en=0
//              o-hvon = 0
//
//      4: jogging <- !i-running && slider changes (i-target-pos != last_pos)
//              en=1
//              ctrl-type = 0
//              pos-cmd=jogpos
//
//      5: extruding <- i-running && M67 E0 Q1 (m68==1)
//              en=1
//              ctrl-type = 1
//              vel-cmd = extrvel
//
//      6: spinning <- i-running && M68 E0 Q2 (m68==2)
//              en=1
//              ctrl-type = 1
//              vel-cmd = spinvel
//              o-hvon = hv_arm
//              

pin in float    i-homevel         "homing velocity";
pin in float    i-extrvel         "extrusion velocity";
pin in float    i-spinvel         "electrospin velocity";
pin in float    i-vectrate        "vector rate";
pin in float    i-target-pos      "jog position cmd";
pin in float    i-limit-pos       "limit3 jog position cmd";
pin in float    i-pos-fb          "position feedback";
pin in float    i-m68             "M67/68 E0 value";
pin in bit      i-running         "running G-code or MDI";
pin in bit      i-homesw          "home and limit switches";
pin in bit      i-home            "start homing sequence";
pin in bit      i-hv_arm          "arm HV";
pin out float   o-velcmd          "velocity command for stepgen";
pin out bit     o-homing          "doing homing";
pin out bit     o-en              "enable stepgen";
pin out bit     o-hvon            "HV is on";
pin out bit     o-ctrltype        "stepgen type: 0=pos, 1=vel";
pin out bit     o-spinning        "is spinning";
pin out bit     o-extruding       "is extruding";
pin out float   o-mode            "debug - current mode";
pin out float   o-pos-cmd         "position command";

function _;

option singleton yes;
license "GPL";
author "Ralph Stirling";

variable int mode = 0;
variable float last_pos = 0;

;;

//#include <rtapi_math.h>

static int differ(float v1, float v2);

static int differ(float v1, float v2)
{
    if (abs(v1) > abs(v2)+0.01)
        return (1);
    if (abs(v1) < abs(v2)-0.01)
        return (1);
    return (0);
}

FUNCTION(_) {

    o_mode = mode;
    switch (mode)
    {
    default:
        mode = 0;
    case 0:     // idle
        if (!i_running && differ(i_target_pos, last_pos))
        //if (!i_running && differ(i_target_pos, i_pos_fb))
            mode = 4;
        if (i_running && i_m68==1)
            mode = 5;
        if (i_running && i_m68==2)
            mode = 6;
        o_en = 0;
        o_ctrltype = 0;
        o_spinning = 0;
        o_extruding = 0;
        o_velcmd = 0;
        o_hvon = 0;
        o_homing = 0;
        o_pos_cmd = i_limit_pos;
        break;
    case 4:     // jogging
        if (!differ(i_target_pos,last_pos) && !differ(i_target_pos,i_limit_pos))
        //if (!differ(i_target_pos,i_pos_fb) && !differ(i_target_pos,i_limit_pos))
            mode = 0;
        o_en = 1;
        o_ctrltype = 0;
        o_spinning = 0;
        o_extruding = 0;
        o_hvon = 0;
        o_homing = 0;
        o_pos_cmd = i_limit_pos;
        break;
    case 5:     // extruding
        if ((i_running && i_m68==0) || (!i_running))
            mode = 0;
        o_en = 1;
        o_ctrltype = 1;
        o_spinning = 0;
        o_extruding = 1;
        o_velcmd = i_extrvel * i_vectrate;
        o_hvon = 0;
        o_homing = 0;
        o_pos_cmd = i_pos_fb;
        break;
    case 6:     // spinning
        if ((i_running && i_m68==0) || (!i_running))
            mode = 0;
        o_en = 1;
        o_ctrltype = 1;
        o_spinning = 1;
        o_extruding = 0;
        o_velcmd = i_spinvel;
        o_hvon = i_hv_arm;
        o_homing = 0;
        o_pos_cmd = i_pos_fb;
        break;
    }
    last_pos = i_target_pos;
}