component syrctrl "syringe control";
// syrctrl.comp - control syringe for 2gbp
//
// modes:
//      0: idle <-  (i_running && M68 E0 Q0 (m68==0)) || (!i_running && !homing)
//              en=0
//              o-hvon = 0
//
//      1: homing <- !i-running && i-home && !i-homesw
//              en=1
//              ctrl-type = 1
//              vel-cmd=homevel
//              
//      2: backoff <- i-homesw
//              vel-cmd = -homevel
//
//      3: homed <- !i-homesw
//              set o-homed
//
//      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-target-pos      "jog position cmd";
pin in float    i-limit-pos       "limit3 jog position cmd";
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-vel-cmd         "velocity command for stepgen";
pin out bit     o-homing          "is doing home sequence";
pin out bit     o-homed           "is done 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";

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(_) {

    switch (mode)
    {
    default:
        mode = 0;
    case 0:     // idle
        if (!i_running && i_home && !i_homesw)
            mode = 1;
        if (!i_running && differ(i_target_pos, last_pos))
            mode = 4;
        if (i_running && i_m68==1)
            mode = 5;
        if (i_running && i_m68==2)
            mode = 6;
        o_en = 0;
        o_ctrltype = 0;
        break;
    case 1:     // homing
        if (i_homesw)
            mode = 2;
        o_en = 1;
        o_ctrltype = 1;
        break;
    case 2:     // backoff
        if (!i_homesw)
            mode = 3;
        o_en = 1;
        o_ctrltype = 1;
        break;
    case 3:     // homed
        mode = 0;
        o_en = 0;
        o_ctrltype = 0;
        break;
    case 4:     // jogging
        if (!differ(i_target_pos,last_pos) && !differ(i_target_pos,i_limit_pos))
            mode = 0;
        o_en = 1;
        o_ctrltype = 0;
        break;
    case 5:     // extruding
        if ((i_running && i_m68==0) || (!i_running))
            mode = 0;
        o_en = 1;
        o_ctrltype = 1;
        break;
    case 6:     // spinning
        if ((i_running && i_m68==0) || (!i_running))
            mode = 0;
        o_en = 1;
        o_ctrltype = 1;
        break;
    }
}