Lathe with C axis and some challenges...

component spindlecv "Component to control lathe spindle and C";

pin in  bit   enable      "enable signal for c axis mode";
pin in  float c_pos_cmd_in    "current commanded pos of c axis";
pin in  float c_pos_fb_in       "c axis feedback position from encoder";
pin out float c_pos_fb_out     "c axis feedback out _ takin offset into account";
pin out float joint-pos-fb-out   "c axis feedback to linuxcnc joint feedback";
pin io bit set_c_index_enable      "connect to index_enable for c axis encoder";
pin out bit drive_position_mode   "to output pin for enabling drive position mode";
pin in s32 decel_delay         "delay time to allow spindle to decelerate";
pin out float c_pos_cmd_out     "pos cmd out for c axis";
pin in float spindle_vel_cmd_in;
pin out float spindle_vel_cmd_out;
pin out bit control_mode =1    "connect to stepgen control type";   
pin in bit spindle_enable_in_fwd;
pin out bit spindle_enable_out_fwd;
pin in bit spindle_enable_in_rev;
pin out bit spindle_enable_out_rev;
pin out bit drive_analog_mode;
pin out float pid-maxerror;
pin in float pid-maxerror-homeing;
pin in float pid-maxerror-operating;
pin in s32 counts        "encoder counts";
pin in float enc-scale   "encoder counts/degree"; 
pin out float spindle-position;
pin out float spindle-speed-fb-rpm;
pin in bit index-enable-spindle;
pin in float c-enc-vel-fb-per-min; 

// debug pins
pin out s32 step_state;
pin out bit last-enable-dbug;
pin out s32 step-point =0;
pin out s32 pos_counts;

pin out float err = 1;
pin in float tolerance = 0.5  "Tolerance for iitial positioning before enbling linuxcnc feedback, the c axis pid loop wil do the rest";
/*

*/    

variable int   last_enable = 0;
variable int    debounce = 0; 
variable float timer=0;
variable int min_speed = 100;





option fp yes;

option singleton yes;

function _;
author "HDP";




license "GPL";
;;
#include <rtapi_math.h>

FUNCTION(_) {


    if (enable) {
       if (enable != last_enable) { //rising edge of enble

    step_state=0;
    last_enable=enable;
    debounce=0;
    timer=0;

    }
            switch (step_state) {
            case 0: // run spindle in analog mode, allow time to decel in case it was running fast, set index-enable
              

        c_pos_fb_out = c_pos_cmd_in;
        spindle_enable_out_fwd = 1;
        spindle_enable_out_rev= 0;
        drive_analog_mode=1;
        spindle_vel_cmd_out = min_speed;
        timer += fperiod;
        
        if (timer >= decel_delay) {
            set_c_index_enable = 1;
                       step_state = 1;
            timer = 0;
        }

                break;

            case 1: // monitor index-enable

                if (set_c_index_enable==0) {
            spindle_vel_cmd_out=0;
            drive_analog_mode = 0;
            control_mode = 1;
            timer=0;
            step_state=2;
        }
                
        break;

            case 2: // delay for spindle to stop, enable drive position mode, set pid_maxerror higher to position spindle, check error
        c_pos_fb_out = c_pos_cmd_in;
        timer += fperiod;
        if (timer >= 1) {
        drive_position_mode=1;
             control_mode = 1;
        }

        if (timer>=2) {
               
        pid_maxerror = pid_maxerror_homeing;
        c_pos_cmd_out = c_pos_cmd_in; 
        c_pos_fb_out = c_pos_fb_in;
        
        err = (c_pos_fb_in - c_pos_cmd_in);
        debounce += (fabs(err) < tolerance && debounce <=100);
        
        if (debounce > 100) {
        
        
        step_state=3;

        }
        }
                      
                break;



        case 3: //change to c axis mode, connect feedback, reset pid_maxerror

        pid_maxerror = pid_maxerror_operating;
        joint_pos_fb_out = c_pos_fb_in;
        c_pos_fb_out = c_pos_fb_in;
        spindle_enable_out_fwd=1;
        spindle_enable_out_rev=0;

        drive_position_mode=1;
        c_pos_cmd_out = c_pos_cmd_in; 

        break;    
            }
       }

 if (!enable) { //if not enable, connect c-pos-fb with c-pos-cmd to prevent following error, connect spindle control signals
    step_state=0;
     c_pos_fb_out = c_pos_cmd_in;
     spindle_vel_cmd_out = spindle_vel_cmd_in;
     spindle_enable_out_fwd = spindle_enable_in_fwd;
     spindle_enable_out_rev = spindle_enable_in_rev;
     drive_position_mode=0;
     drive_analog_mode=1;

    spindle_position = c_pos_fb_in/360; // scle c axis position for spindle mode 

    set_c_index_enable = index_enable_spindle; // provide spindle index-enable signal for threading, this only avoids additional hal connections

}
     

        last_enable = enable;
    last_enable_dbug=last_enable;    
    spindle_speed_fb_rpm = c_enc_vel_fb_per_min/360; //scale c vel-fb for spindle mode

}